C语言lidar_align雷达里程计校准功能怎么用(c语言,开发技术)

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

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

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

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

整个功能包大体上实现了下面的功能：

1、读取lidar和位姿传感器的数据

2、通过时间戳匹配lidar每帧里面的每个点和位姿传感器的坐标变换

3、通过上面的变换矩阵将 利用位姿信息将lidar的每帧拼接成点云

4、每个点和它最近邻点的距离总和求出，在优化中就是不断的迭代找到坐标变换使这个距离最小

功能包算法的整体思想：

1、为每帧lidar的每个点通过时间戳去匹配一个位姿数据，并通过插值的方式得到更准确的位姿值，每个点的时间偏移也是优化因素之一

2、利用NLopt的库的非线性优化方法

3、目标函数：让拼接后的点云的每个点的最近邻点最小

4、优化向量：x、y 、z、roll、pitch、yaw、time_offset 总体来说就是不断的迭代，找到一个合适的优化向量（也就是lidar到里程计的坐标变换）使得拼在一起的点云每个点的最近邻点距离最小。这个功能包的优点就是可以离线计算，录一个rosbag，然后跑一下就可以求的外参 它只会读取一个bag ，所以 lidar和位姿都要在里面

最终的结果： 在运行的时候，功能包会输出当前的估计变化 优化结束的时候 坐标变换的参数会打印在终端上 也会在路径下面保存一个txt文件和ply文件。

可以查看ply文件，就是拼接后的点云和场景是否一致

从git上下载后需要 下载 nlopt的库

然后编译还是会报错

Could not find a package configuration file provided by "NLOPT"

解决办法 将 lidar_align 文件夹下的 NLOPTConfig.cmake 复制到 你ROS工作空间的src路径下面

这些都没有在配置文件里面 想要修改的话需要改源码然后再编译

在每个类里面有个 Config的结构体， 初始化了相关参数

和lidar相关的在 Scan类里面设置的

输入输出的相关参数在launch里面设置的

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类里面设置的

头文件四个：

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和里程计的数据

前面的数据读取处理啥的就不整了，下面分析下主要的代码部分

在面函数里面 调用了 下面的 函数 前面就是数据读取和处理，从这个函数里面开启了校准的过程

这个函数里面

根据 是否使用 时间补偿 来确定 优化参数的 个数

根据配置参数（是否执行全局优化） 执行 初始角度的赋值 ，如果执行全局优化则先进行一个 估计，得到角度 否则 直接附设置的初始值

设置 优化因素的 上限和下限

执行局部优化

下面看下执行局部优化的主要内容

也就是 optimize()函数 里面就是用的nlopt的方法进行的非线性优化

根据执行全局优化还是局部优化 选择不同的算法 并生成了 nlopt 对象实例

nlopt的相关设置 其中 LidarOdomMinimizer 是目标函数需要重点看的

目标函数里面

在这里 通过 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里面功能包输出的结果就是这样的。