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