C語言lidar_align雷達裡程計校準功能詳解

sudo apt-get install libnlopt-dev

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
  };

    <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)"/>

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.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);

  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的個數在前面配置的是三個 
  }

  //設置下限
  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外部數據

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