Lidar与Odom外参标定：lidar_align代码学习

本文仅用于记录自己在学习lidar_align代码过程中的一些总结。本文首先发表于公众号推送：Lidar与IMU标定代码实战。有修改与增加。

重要说明：lidar_align不适用于纯IMU与Lidar进行标定！

来自lidar_align维护者Alex：
Unfortunately our toolbox is not suitable for pure imu-to-lidar calibration. I have seen other papers about this problem, but to be honest I’m not up to date with that literature so I don’t know if there is an open-source toolbox available.
Our typical use case is one where we have a camera-imu-lidar system. We typically perform visual-inertial odometry and use this as the odom transform to which you’re referring. If you don’t have an option for obtaining odometry in a similar manner you might be out of luck with this toolbox…

纯IMU进行积分，无法得到准确的Odom数据。所以无法使用纯IMU获得odom，进行与Lidar标定。

0. 参考资料

原版代码github：https://github.com/ethz-asl/lidar_align
个人修改（格式）与注释版：https://github.com/LarryDong/lidar_align
参考博客1：https://www.cnblogs.com/gangyin/p/13366683.html
参考博客2：https://blog.csdn.net/miracle629/article/details/87854450
NLOPT优化库介绍：https://nlopt.readthedocs.io/en/latest/
Lidar和IMU标定需要标什么？https://blog.csdn.net/tfb760/article/details/108532974

1. 代码整体一览

1.1 代码结构

代码主要包括：头文件、实现cpp、以及ROS的launch启动文件
头文件包括：
aligner.h：Lidar和Odom对齐（外参计算）时用到的类
loader.h：从ROS的Bag或CSV格式载入数据的相关函数
sensors.h：主要包括：里程计Odom，以及雷达Lidar相关接口
transform.h：一些SO3变化的计算以及转换，在插值、优化时使用

1.2 方法基本思想

方法本质解决的是一个优化问题，即在外参参数(6DoF)如何选择时，使Lidar采集到的数据转化到Odom系下后，前后两次scan的数据点能够尽可能的重合。

详细一点儿来说，方法将Lidar数据根据当前假设的状态变量（6DoF参数）变换到Odom系下，构成点云PointCloud，之后对每一次scan时的数据，在下一次scan中通过kdtree的方式寻找最近邻的数据点并计算距离，当总距离最小时可以认为完全匹配，即计算的外参参数正确。

1.3 主要流程

代码主要有两部分：载入数据与优化计算。
载入数据：从Bag数据载入雷达的数据(sensor_msgs/PointCloud2)，并从CSV或Bag数据中载入Odom获得的6DoF位置信息。具体的位置信息如何获得将在后面进行介绍。

2. 详细介绍

2.1 主要数据类型

Odom数据：主要包括两个数据：时间戳timestamp_us_与从当前时刻到初始时刻的变换T_o0_ot_，具体定义如下：

class OdomTformData{
public:
    OdomTformData(Timestamp timestamp_us, Transform T_o0_ot);
    const Transform &getTransform() const;
    const Timestamp &getTimestamp() const;
private:
    Transform T_o0_ot_;
    Timestamp timestamp_us_;
};

class Odom{
public:
    void addTransformData(const Timestamp &timestamp_us, const Transform &transform);
    Transform getOdomTransform(const Timestamp timestamp_us, const size_t start_idx = 0, size_t *match_idx = nullptr) const;
    bool empty() { return data_.empty(); }
private:
    std::vector<OdomTformData> data_;
};

Lidar数据：主要包括两个参数：从Lidar到Odom的外参T_o_l_与每次扫描的数据scans_，而每次的扫描scan数据类型主要包括：扫描起始时间timestamp_us_，本次扫描的点云raw_points_，某个点在Odom的变换（用于去畸变）T_o0_ot_，以及相关配置参数等。具体定义如下：

class Scan{
public:
    struct Config{
        float min_point_distance = 0.0;
        float max_point_distance = 100.0;
        float keep_points_ratio = 0.01;
        float min_return_intensity = -1.0;
        bool estimate_point_times = false;
        bool clockwise_lidar = false;
        bool motion_compensation = true;
        float lidar_rpm = 600.0;
    };
    Scan(const LoaderPointcloud &pointcloud, const Config &config);
    static Config getConfig(ros::NodeHandle *nh);
    void setOdomTransform(const Odom &odom, const double time_offset, const size_t start_idx, size_t *match_idx);
    const Transform &getOdomTransform() const;
    const Pointcloud &getRawPointcloud() const;
    void getTimeAlignedPointcloud(const Transform &T_o_l, Pointcloud *pointcloud) const;
private:
    Timestamp timestamp_us_;            // signed to allow simpler comparisons
    Pointcloud raw_points_;
    std::vector<Transform> T_o0_ot_;    // absolute odom transform to each point in pointcloud
    bool odom_transform_set_;
};

class Lidar{
public:
    Lidar(const LidarId &lidar_id = "Lidar");
    const size_t getNumberOfScans() const;
    const size_t getTotalPoints() const;
    // note points are appended so any points in *pointcloud are preserved
    void getCombinedPointcloud(Pointcloud *pointcloud) const;
    const LidarId &getId() const;
    void addPointcloud(const LoaderPointcloud &pointcloud, const Scan::Config &config = Scan::Config());
    void setOdomOdomTransforms(const Odom &odom, const double time_offset = 0.0);
    void setOdomLidarTransform(const Transform &T_o_l);
    void saveCombinedPointcloud(const std::string &file_path) const;        // used for debugging frames
    const Transform &getOdomLidarTransform() const;
private:
    LidarId lidar_id_;
    Transform T_o_l_;               // transform from lidar to odometry
    std::vector<Scan> scans_;       //~ contains all scans.
};

Aligner的数据：Aligner首先包含了需要优化的数据OptData（其中包括Lidar、Odom等数据），以及相应的配置参数（数据载入路径、初值、优化参数、KNN相关参数等），以及优化计算的相关参数。主要定义如下：

class Aligner{
public:
    struct Config{
        bool local = false;
        std::vector<double> inital_guess{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
        double max_time_offset = 0.1;
        double angular_range = 0.5;
        double translation_range = 1.0;
        double max_evals = 200;
        double xtol = 0.0001;
        int knn_batch_size = 1000;
        int knn_k = 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 = "";
    };
    struct OptData{     //~ optimize data
        Lidar *lidar;
        Odom *odom;
        Aligner *aligner;
        bool time_cal;
    };
private:
	// 省略相关计算参数
}

2.2 优化过程详细介绍

在载入了Odom和Lidar数据之后，进行优化求解6个位姿参数。主要求解函数为：lidarOdomTransform

Aligner::lidarOdomTransform()

首先进行相关的优化配置。默认优化参数是6个，但可以考虑两个传感器传输造成的时间差，如果考虑这个因素，参数数量将变为7。但最终优化结果好像并没有这个时间差。

优化时，采用NLOPT优化库，默认首先全局优化$[rx,ry,rz]$这三个参数。如果提供的初值与真值相差较大，或完全没有设置初值（默认为全0），则需要进行全局优化获得旋转参数。在局部优化$[x,y,z,rx,ry,rz]$这6个参数，局部优化开始时的初值就是3个为0的平移参数，以及全局优化计算出来的旋转参数。全局优化、局部优化，都是调用的optimize函数。

Aligner::optimize()

在这个函数设置了NLOPT优化的相关参数，包括：是全局优化还是局部优化、优化问题的上下界、最大迭代次数、求解精度以及目标函数等。最重要的是目标函数LidarOdomMinimizer

LidarOdomMinimizer()

这个函数在优化中会不断调用，迭代计算。首先会通过上一时刻的状态，计算新的从Lidar到Odom的变换（这里用到了Transform.h中定义的一些变换），误差是由lidarOdomKNNError函数获得。

lidarOdomKNNError()

这个是一个重载函数，具有两种重载类型。首先调用的是lidarOdomKNNError(const Lidar)，处理的是Lidar的数据，首先根据估计的Lidar到Odom的变化，对完整的scans_数据计算出每次scan时每个点在Odom下的坐标（getTimeAlignedPointcloud函数，相当于点云去畸变），得到一个结合的点云(CombinedPointcloud)，之后从这个点云中寻找每个点的最近邻，在利用另一个重载类型的lidarOdomKNNError(const Pointcloud, const Pointcloud)函数进行计算最近邻误差。

计算最近邻误差时，构建了一个KD-Tree，并行计算kNNError函数，利用pcl库的nearestKSearch函数搜索一定范围（全局优化时是1m，局部优化时是0.1m）的最近邻，计算最近2个点的误差（作者使用2个点，认为完全匹配时应该和自己重合，所以1-近邻是自己，但我感觉没有太大区别）

小结
优化的目标函数是每次scan的每个点在完整点云中的最近邻的距离，首先通过粗的全局优化估计一部分参数，再局部优化求解精细的6DoF参数。

3. 配置与运行

3.1 安装

首先在安装时需要安装NLOPT：

sudo apt-get install libnlopt-dev

之后把代码拷贝到ros的工作空间，使用 catkin_make进行编译。

3.2 编译可能遇到的问题

1. typedef struct LZ4_stream_t LZ4_stream_t

这个代码是个人编写使用，没有在大多数的ubuntu和ros版本进行测试，所以可能会遇到各种各样的问题。以Ubuntu18与ROS-melodic为例，首先会遇到一个定义冲突的报错：

error: conflicting declaration ‘typedef struct LZ4_stream_t LZ4_stream_t’ typedef struct { long long table[LZ4_STREAMSIZE_U64]; } LZ4_stream_t;

这个原因是ROS版本下有两个头文件定义发生冲突，github的issue中给出了两种解决办法，之一是重命名头文件避免冲突：

sudo mv /usr/include/flann/ext/lz4.h /usr/include/flann/ext/lz4.h.bak
sudo mv /usr/include/flann/ext/lz4hc.h /usr/include/flann/ext/lz4.h.bak
sudo ln -s /usr/include/lz4.h /usr/include/flann/ext/lz4.h
sudo ln -s /usr/include/lz4hc.h /usr/include/flann/ext/lz4hc.h

详见：https://github.com/ethz-asl/lidar_align/issues/16

2. 找不到"FindNLOPT.cmake"

By not providing “FindNLOPT.cmake” in CMAKE_MODULE_PATH this project has asked CMake to find a package configuration file provided by “NLOPT”, but CMake did not find one. Solutions: Move “NLOPTConfig.cmake” file to `src directory.

解决方法：将"\lidar_align\FindNLOPT.cmake"文件移动到工作路径下中的"\src"文件夹下，再次编译即可。

3.3 测试运行

在github的issue中，由于存在准备数据（尤其是Odom数据）有错误的问题，造成运行失败。作者上传了测试数据https://drive.google.com/open?id=11fUwbVnvej4NZ_0Mntk7XJ2YrZ5Dk3Ub 可以运行测试。

3.4 数据准备

Lidar的数据直接是ros中的sensor_msgs/PointCloud2即可，但位置数据需要提供CSV格式或者ROS下的geometry_msgs/TransformStamped消息类型。后者如何获得？如果是IMU直接进行积分就好，但这样积分势必会不准确，作者也在issue中提到没有考虑noise的问题(https://github.com/ethz-asl/lidar_align/issues/5#issuecomment-432232087)，所以目前看来对IMU进行积分，凑合使用就好。

参考博客1给出了一种可能的计算方法：https://www.cnblogs.com/gangyin/p/13366683.html

3.5 数据采集要求

作者在issue和readme中指出，该方法存在的局限性是，必须要求采集数据时系统进行非平面运动，对平移要求不高但要求旋转必须充分。但对数据量、运动范围没有经过严格的测试。这个局限性也限制了不能用于给无人车这种系统标定。