lidar_align--雷达和里程计校准--详解

功能包名称：lidar_align
git网址：链接

功能介绍

一种 校准 3D 激光雷达和 6 自由度位姿传感器 外参 的 方法

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

准确的结果需要 大量 非平面的运动，这使得该方法不适合 校准 安装在汽车上的传感器
以为只有在那个方向上有数据的变化才能计算出最终的结果。

整个功能包大体上实现了下面的功能：
1、读取lidar和位姿传感器的数据
2、通过时间戳匹配lidar每帧里面的每个点和位姿传感器的坐标变换
2、通过上面的变换矩阵将 利用位姿信息将lidar的每帧拼接成点云
3、每个点和它最近邻点的距离总和求出，在优化中就是不断的迭代找到坐标变换使这个距离最小

功能包算法的整体思想：
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里面功能包输出的结果就是这样的。