sudo apt-get install libnlopt-dev

class Scan {
 public:
  struct Config {
    float min_point_distance = 80;//lidar數(shù)據(jù)里面點的距離的最小值 大于此值被使用 0.0;
    float max_point_distance = 200;// lidar數(shù)據(jù)里面點的距離的最大值 小于此值被使用  默認(rèn)100.0;
    float keep_points_ratio = 0.01;// 用于優(yōu)化點的比例 （此值越大會極大增加運行時間） 默認(rèn)0.01
    float min_return_intensity = -1.0;//lidar數(shù)據(jù)里面點的強度的最小值  大于此值被使用  默認(rèn) -1.0
    bool estimate_point_times = false;//是否估計lidar每個點的時間 用下面那倆變量   默認(rèn) false
    bool clockwise_lidar = false;//lidar旋轉(zhuǎn)的方向  順時針還是逆時針              默認(rèn)  false（逆時針）
    bool motion_compensation = true;//是不是需要運行補償    默認(rèn) true
    float lidar_rpm = 600.0;// lidar的轉(zhuǎn)速  僅用于  estimate_point_times   默認(rèn) 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;//執(zhí)行局部優(yōu)化還是全局優(yōu)化 初始化為false， 執(zhí)行全局的優(yōu)化，并且結(jié)果用于初始估計 然后執(zhí)行局部優(yōu)化
    std::vector<double> inital_guess{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};//校準(zhǔn)的初始估計   僅用于運行 local 模式  
    double max_time_offset = 0.1;//最大的時間對準(zhǔn)偏差  x[6]的上下浮動范圍
    double angular_range = 0.5;    //  在初始估計位置的搜索范圍 （弧度）  角度偏移上下限 +-0.5弧度
    double translation_range = 1.0;//在初始估計位置的搜索范圍 （位置）  位置偏移上下限 +-1米
    double max_evals = 200;// 最大的評估次數(shù)
    double xtol = 0.0001;//x的容差
    int knn_batch_size = 1000;//點的偏移數(shù)量 被用于  尋找最近鄰點時
    int knn_k = 1;//尋找最近鄰點的個數(shù) ， 程序了 自加1了  因為在一個點云里面找其中一個點的最近鄰一個是自己一個才是真的最近鄰
    float local_knn_max_dist = 0.1;//局部優(yōu)化的時候 最近鄰點的最大距離
    float global_knn_max_dist = 1.0;//全局優(yōu)化的時候 最近鄰點的最大距離
    bool time_cal = true;   //是否執(zhí)行時間補充計算  就是在找每個點的位姿時根據(jù)時間對里程計插值
    std::string output_pointcloud_path = "";  //點云文件保存路徑
    std::string output_calibration_path = ""; // 校準(zhǔn)信息文件保存路徑
  };

aligner.lidarOdomTransform(&lidar, &odom);//執(zhí)行校準(zhǔn)

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

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

  //設(shè)置 參數(shù)向量 的下限   ，默認(rèn)參數(shù) -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};
  //設(shè)置 參數(shù)向量 的上限    默認(rèn)參數(shù) 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};
  //將  全局優(yōu)化得到的 角度的 初始估計值  疊加到 上下限中   也就是說  初始估計 roll 為0.1弧度，則 優(yōu)化的范圍是 0.1 +- 0.5 弧度  
  for (size_t i = 0; i < 6; ++i) {
    lb[i] += x[i];//疊加下限
    ub[i] += x[i];//疊加上限
  }
    //將時間補償?shù)?優(yōu)化 范圍 加 到 上下限中
  if (config_.time_cal) {
    ub.push_back(config_.max_time_offset);
    lb.push_back(-config_.max_time_offset);
  }

 /*執(zhí)行局部優(yōu)化*/
  optimize(lb, ub, &opt_data, &x);

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

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

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