C語言lidar_align雷達里程計校準功能怎么用

本篇內容主要講解“C語言lidar_align雷達里程計校準功能怎么用”，感興趣的朋友不妨來看看。本文介紹的方法操作簡單快捷，實用性強。下面就讓小編來帶大家學習“C語言lidar_align雷達里程計校準功能怎么用”吧!

功能介紹

功能包名稱：lidar_align git網址：鏈接

一種 校準 3D 激光雷達和 6 自由度位姿傳感器 外參 的 方法

適配的ROS版本有 Indigo、Kinect、Melodic

準確的結果需要 大量 非平面的運動，這使得該方法不適合 校準 安裝在汽車上的傳感器 以為只有在那個方向上有數據的變化才能計算出最終的結果。

整個功能包大體上實現了下面的功能：

1、讀取lidar和位姿傳感器的數據

2、通過時間戳匹配lidar每幀里面的每個點和位姿傳感器的坐標變換

3、通過上面的變換矩陣將 利用位姿信息將lidar的每幀拼接成點云

4、每個點和它最近鄰點的距離總和求出，在優化中就是不斷的迭代找到坐標變換使這個距離最小

功能包算法的整體思想：

1、為每幀lidar的每個點通過時間戳去匹配一個位姿數據，并通過插值的方式得到更準確的位姿值，每個點的時間偏移也是優化因素之一

2、利用NLopt的庫的非線性優化方法

3、目標函數：讓拼接后的點云的每個點的最近鄰點最小

4、優化向量：x、y 、z、roll、pitch、yaw、time_offset 總體來說就是不斷的迭代，找到一個合適的優化向量（也就是lidar到里程計的坐標變換）使得拼在一起的點云每個點的最近鄰點距離最小。這個功能包的優點就是可以離線計算，錄一個rosbag，然后跑一下就可以求的外參 它只會讀取一個bag ，所以 lidar和位姿都要在里面

最終的結果： 在運行的時候，功能包會輸出當前的估計變化 優化結束的時候 坐標變換的參數會打印在終端上 也會在路徑下面保存一個txt文件和ply文件。

可以查看ply文件，就是拼接后的點云和場景是否一致

編譯

從git上下載后需要 下載 nlopt的庫

sudo apt-get install libnlopt-dev

然后編譯還是會報錯

Could not find a package configuration file provided by "NLOPT"

解決辦法 將 lidar_align 文件夾下的 NLOPTConfig.cmake 復制到 你ROS工作空間的src路徑下面

相關參數

這些都沒有在配置文件里面 想要修改的話需要改源碼然后再編譯

在每個類里面有個 Config的結構體， 初始化了相關參數

和lidar相關的在 Scan類里面設置的

class Scan {
 public:
  struct Config {
    float min_point_distance = 80;//lidar數據里面點的距離的最小值 大于此值被使用 0.0;
    float max_point_distance = 200;// lidar數據里面點的距離的最大值 小于此值被使用  默認100.0;
    float keep_points_ratio = 0.01;// 用于優化點的比例 （此值越大會極大增加運行時間） 默認0.01
    float min_return_intensity = -1.0;//lidar數據里面點的強度的最小值  大于此值被使用  默認 -1.0
    bool estimate_point_times = false;//是否估計lidar每個點的時間 用下面那倆變量   默認 false
    bool clockwise_lidar = false;//lidar旋轉的方向  順時針還是逆時針              默認  false（逆時針）
    bool motion_compensation = true;//是不是需要運行補償    默認 true
    float lidar_rpm = 600.0;// lidar的轉速  僅用于  estimate_point_times   默認 600
  };

輸入輸出的相關參數在launch里面設置的

    <param name="input_bag_path" value="$(arg bag_file)" />
    <param name="input_csv_path" value="$(arg csv_file)" />
    <param name="output_pointcloud_path" value="$(find lidar_align)/results/$(anon lidar_points).ply" />
    <param name="output_calibration_path" value="$(find lidar_align)/results/$(anon calibration).txt" />
    <param name="transforms_from_csv" value="$(arg transforms_from_csv)"/>

use_n_scans 執行優化開始的 lidar的幀數 默認 2147483647 input_bag_path ros bag 的路徑
transforms_from_csv 是否通過 csv文件 讀取位置 默認 false input_csv_path csv路徑 output_pointcloud_path ply文件輸出路徑 output_calibration_path 校準結果輸出路徑

校正相關參數 在Aligner類里面設置的

class Aligner {
 public:
  struct Config {
    bool local = false;//執行局部優化還是全局優化 初始化為false， 執行全局的優化，并且結果用于初始估計 然后執行局部優化
    std::vector<double> inital_guess{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};//校準的初始估計   僅用于運行 local 模式  
    double max_time_offset = 0.1;//最大的時間對準偏差  x[6]的上下浮動范圍
    double angular_range = 0.5;    //  在初始估計位置的搜索范圍 （弧度）  角度偏移上下限 +-0.5弧度
    double translation_range = 1.0;//在初始估計位置的搜索范圍 （位置）  位置偏移上下限 +-1米
    double max_evals = 200;// 最大的評估次數
    double xtol = 0.0001;//x的容差
    int knn_batch_size = 1000;//點的偏移數量 被用于  尋找最近鄰點時
    int knn_k = 1;//尋找最近鄰點的個數 ， 程序了 自加1了  因為在一個點云里面找其中一個點的最近鄰一個是自己一個才是真的最近鄰
    float local_knn_max_dist = 0.1;//局部優化的時候 最近鄰點的最大距離
    float global_knn_max_dist = 1.0;//全局優化的時候 最近鄰點的最大距離
    bool time_cal = true;   //是否執行時間補充計算  就是在找每個點的位姿時根據時間對里程計插值
    std::string output_pointcloud_path = "";  //點云文件保存路徑
    std::string output_calibration_path = ""; // 校準信息文件保存路徑
  };

代碼文件結構

頭文件四個：

• aligner.h : Lidar 和 Odom 校正（外參計算）時用到的類

• **loader.h ：**從ROS的Bag或CSV格式載入數據的相關函數

• **sensor.h ：**主要包括Odom以及LIdar相關接口

• transform.h ： 一些SO3變化的計算及轉換，在插值、優化時使用

cpp文件四個：

• aligner.cpp ： Lidar 和 Odom 校正（外參計算） 時 的實現函數

• lidar_align_node.cpp ： main的啟動地方 實例各個類

• loader.cpp ： 加載數據

• sensors.cpp ： 處理lidar和里程計的數據

核心代碼

前面的數據讀取處理啥的就不整了，下面分析下主要的代碼部分

在面函數里面 調用了 下面的 函數 前面就是數據讀取和處理，從這個函數里面開啟了校準的過程

aligner.lidarOdomTransform(&lidar, &odom);//執行校準

這個函數里面

  /*根據 是否使用 時間補償  來確定 優化參數的 個數*/
  size_t num_params = 6;//初始化優化參數的個數
  if (config_.time_cal) {
    ++num_params;//使用時間補償計算   優化參數個數為7
  }

根據 是否使用 時間補償 來確定 優化參數的 個數

 /*根據配置參數（是否執行全局優化）  執行 初始角度的賦值 ，如果執行全局優化則先進行一個 估計，得到角度 否則 直接附設置的初始值    */
  if (!config_.local) {  //默認  執行 全局優化  并用于初始估計
    //打印 正在 執行 全局 優化
    ROS_INFO("Performing Global Optimization...                             ");
    std::vector<double> lb = {-M_PI, -M_PI, -M_PI};//設置下限
    std::vector<double> ub = {M_PI, M_PI, M_PI};//設置上限
    std::vector<double> global_x(3, 0.0);//設置 執行全局優化的  x參數向量 設置為3 個，初始值為0.0
    optimize(lb, ub, &opt_data, &global_x);//執行優化（全局優化）  只求 角度的  優化結果
    config_.local = true;//將全局優化的flag關閉  下次則執行 局部優化
    //賦值 角度  優化 的結果
    x[3] = global_x[0];
    x[4] = global_x[1];
    x[5] = global_x[2];
  } else {//local 為 true的 話  則 為 默認的 初始值 
    x = config_.inital_guess;//初始值 全為0    局部優化則初始認為 lidar和imu的坐標軸一致對齊  其它情況最好按實際情況粗略配置 初始值 
  }

根據配置參數（是否執行全局優化） 執行 初始角度的賦值 ，如果執行全局優化則先進行一個 估計，得到角度 否則 直接附設置的初始值

  //設置 參數向量 的下限   ，默認參數 -1，-1，-1，-0.5，-0.5，-0.5
  std::vector<double> lb = {
      -config_.translation_range, -config_.translation_range,
      -config_.translation_range, -config_.angular_range,
      -config_.angular_range,     -config_.angular_range};
  //設置 參數向量 的上限    默認參數 1，1，1，0.5，0.5，0.5
  std::vector<double> ub = {
      config_.translation_range, config_.translation_range,
      config_.translation_range, config_.angular_range,
      config_.angular_range,     config_.angular_range};
  //將  全局優化得到的 角度的 初始估計值  疊加到 上下限中   也就是說  初始估計 roll 為0.1弧度，則 優化的范圍是 0.1 +- 0.5 弧度  
  for (size_t i = 0; i < 6; ++i) {
    lb[i] += x[i];//疊加下限
    ub[i] += x[i];//疊加上限
  }
    //將時間補償的 優化 范圍 加 到 上下限中
  if (config_.time_cal) {
    ub.push_back(config_.max_time_offset);
    lb.push_back(-config_.max_time_offset);
  }

設置 優化因素的 上限和下限

 /*執行局部優化*/
  optimize(lb, ub, &opt_data, &x);

執行局部優化

下面看下執行局部優化的主要內容

也就是 optimize()函數 里面就是用的nlopt的方法進行的非線性優化

  if (config_.local) {//看是執行全局優化還是局部優化   用的算法不同  x的個數在前面配置的也不同， 全局優化為3個，只求角度 局部優化為7個
    opt = nlopt::opt(nlopt::LN_BOBYQA, x->size());
  } else {
    opt = nlopt::opt(nlopt::GN_DIRECT_L, x->size());//選用 全局優化函數  不需要求導的  算法 DIRECT_L   x的個數在前面配置的是三個 
  }

根據執行全局優化還是局部優化 選擇不同的算法 并生成了 nlopt 對象實例

  //設置下限
  opt.set_lower_bounds(lb);
  //設置上限
  opt.set_upper_bounds(ub);
  opt.set_maxeval(config_.max_evals);//設置最大估計次數
  opt.set_xtol_abs(config_.xtol);//設置x容差停止值
  opt.set_min_objective(LidarOdomMinimizer, opt_data);//設置目標函數  LidarOdomMinimizer函數名 opt_data外部數據

nlopt的相關設置 其中 LidarOdomMinimizer 是目標函數需要重點看的

目標函數里面

  //通過 kdtree 的 方法 求的 每個點的 最近鄰距離 總和
  double error = d->aligner->lidarOdomKNNError(*(d->lidar));

在這里 通過 kdtree 的 方法 求的 每個點的 最近鄰距離 總和

然后不斷的迭代讓這個距離最小，得到的優化向量就是 校準信息了

主要的核心就是這些了，當然這個里面的小細節太多了，有問題的童鞋可以私信我。

結果

最后上一個測試時的結果把

Active Transformation Vector (x,y,z,rx,ry,rz) from the Pose Sensor Frame to the Lidar Frame: [0.770924, -0.25834, 0.105557, 1.67974, -1.88141, -1.40085]

Active Transformation Matrix from the Pose Sensor Frame to the Lidar Frame: -0.300413 -0.623731 -0.721603 0.770924 -0.870125 -0.130671 0.475193 -0.25834 -0.390685 0.770639 -0.503468 0.105557 0 0 0 1

Active Translation Vector (x,y,z) from the Pose Sensor Frame to the Lidar Frame: [0.770924, -0.25834, 0.105557]

Active Hamiltonen Quaternion (w,x,y,z) the Pose Sensor Frame to the Lidar Frame: [-0.127913, -0.577435, 0.646763, 0.481564]

Time offset that must be added to lidar timestamps in seconds: -0.00263505

ROS Static TF Publisher:

在txt里面功能包輸出的結果就是這樣的。

到此，相信大家對“C語言lidar_align雷達里程計校準功能怎么用”有了更深的了解，不妨來實際操作一番吧！這里是億速云網站，更多相關內容可以進入相關頻道進行查詢，關注我們，繼續學習！