ubuntu16.04 lidar_align实现三维激光雷达和Imu联合标定

实现环境：

系统环境： Ubuntu16.04
激光雷达： velodyne16线
imu品牌： xsens

一简介

lidar-imu标定方法为瑞士苏黎世理工大学-自动驾驶实验室开源的一种校准 3D 激光雷达和 6 自由度位姿传感器外参的方法。该方法需要大量非平面的运动，因此不适合校准安装在汽车上的传感器标定。

lidar_align工程代码实现了以下功能：
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和位姿都要在里面。

二环境安装

（1）安装依赖

sudo apt-get install libnlopt-dev

（2）编译

mkdir -p lidar_align_ws/src
cd lidar_align_ws/src
git clone https://github.com/ethz-asl/lidar_align
cd ..
catkin_make

若报错:

解决办法：
将 lidar_align 文件夹下的 NLOPTConfig.cmake 复制到 ROS工作空间lidar_align_ws/src路径下面，再次编译

编译完成:

（3）改写imu接口

这一工具原本不是用来标定激光雷达和IMU的而是用来标定激光雷达和里程计的。所以需要改写IMU接口来替换掉里程计接口。所以这一工具一定程度上并不是精确标定上述两种传感器的，但不精确不代表不可用。

找到以下odom部分注释删掉都可
/*  types.push_back(std::string("geometry_msgs/TransformStamped"));rosbag::View view(bag, rosbag::TypeQuery(types));size_t tform_num = 0;for (const rosbag::MessageInstance& m : view) {std::cout << " Loading transform: \e[1m" << tform_num++<< "\e[0m from ros bag" << '\r' << std::flush;geometry_msgs::TransformStamped transform_msg =*(m.instantiate<geometry_msgs::TransformStamped>());Timestamp stamp = transform_msg.header.stamp.sec * 1000000ll +transform_msg.header.stamp.nsec / 1000ll;Transform T(Transform::Translation(transform_msg.transform.translation.x,transform_msg.transform.translation.y,transform_msg.transform.translation.z),Transform::Rotation(transform_msg.transform.rotation.w,transform_msg.transform.rotation.x,transform_msg.transform.rotation.y,transform_msg.transform.rotation.z));odom->addTransformData(stamp, T);}
*/将以上部分替换为：types.push_back(std::string("sensor_msgs/Imu"));rosbag::View view(bag, rosbag::TypeQuery(types));size_t imu_num = 0;double shiftX=0,shiftY=0,shiftZ=0,velX=0,velY=0,velZ=0;ros::Time time;double timeDiff,lastShiftX,lastShiftY,lastShiftZ;for (const rosbag::MessageInstance& m : view){std::cout <<"Loading imu: \e[1m"<< imu_num++<<"\e[0m from ros bag"<<'\r'<< std::flush;sensor_msgs::Imu imu=*(m.instantiate<sensor_msgs::Imu>());Timestamp stamp = imu.header.stamp.sec * 1000000ll +imu.header.stamp.nsec / 1000ll;if(imu_num==1){time=imu.header.stamp;Transform T(Transform::Translation(0,0,0),Transform::Rotation(1,0,0,0));odom->addTransformData(stamp, T);}else{timeDiff=(imu.header.stamp-time).toSec();time=imu.header.stamp;velX=velX+imu.linear_acceleration.x*timeDiff;velY=velX+imu.linear_acceleration.y*timeDiff;velZ=velZ+(imu.linear_acceleration.z-9.801)*timeDiff;lastShiftX=shiftX;lastShiftY=shiftY;lastShiftZ=shiftZ;shiftX=lastShiftX+velX*timeDiff+imu.linear_acceleration.x*timeDiff*timeDiff/2;shiftY=lastShiftY+velY*timeDiff+imu.linear_acceleration.y*timeDiff*timeDiff/2;shiftZ=lastShiftZ+velZ*timeDiff+(imu.linear_acceleration.z-9.801)*timeDiff*timeDiff/2;Transform T(Transform::Translation(shiftX,shiftY,shiftZ),Transform::Rotation(imu.orientation.w,imu.orientation.x,imu.orientation.y,imu.orientation.z));odom->addTransformData(stamp, T);}}并在开头添加头文件：#include <sensor_msgs/Imu.h>

修改完成之后再次catkin_make 编译一次即可。

三录制数据

联合标定需要录制的bag包中包含激光数据和imu的数据，因此首先需要固定好激光雷达和imu。将二者的相对位置固定好，然后在录制bag包。

需要注意的是，标定的效果是很影响建图效果的。因此标定需要严谨小心。

较错误的方式：手持固定好的设备，在室外环境录制2分钟左右的数据包，移动过程中夹杂原地转圈。这种数据包标定出来的误差有14000-16500不等。这个还是比较大了，我看有的标定让误差降到几百的都有。然后取经学习了下。

较正确的方式：以移动平台为介质，承载固定好的设备。然后在层次分明的室内环境，特征较多，尽量避免环境中有移动物体（也需要考虑自己）来进行录制。遥控移动平台完成移动和转圈的操作。然后尝试了一下，误差直接降到2000以内，效果明显。可以多以该模式录制几次，以便找到最优的方法。

（1）修改launch文件

打开lidar_align.launch文件，将两分钟的数据包路经copy到第4行的位置：

改写了接口以后就不用以表格形式导入数据了，直接播放在launch文件里面修改你的数据包的路径即可，下面是一些过程展示，标定时间可能较长，一个小时多点吧。

（2）开始标定

source devel/setup.bash 
roslaunch lidar_align lidar_align.launch

标定完以后:

结果会保存到lidar_align/results文件下:

至此,联合标定就结束了

参考文献:

LIO-SAM运行自己数据包遇到的问题解决--SLAM不学无数术小问题_^摆渡人^的博客-CSDN博客

lidar-imu calibration---lidar_align+运行liosam算法测试标定效果_陆枫先森的博客-CSDN博客

激光雷达和IMU联合标定并运行LIOSAM_cyx610481953的博客-CSDN博客_liosam