Livox Lidar+海康Camera实时生成彩色点云

Livox Lidar  + HIKROBOT Camera系列

最近在开发相机和激光雷达融合的slam算法，主要用于三维重建，想实时的得到彩色点云地图，传感器选择了海康威视的工业相机和大疆的固态激光雷达。

海康Camera MVS Linux SDK二次开发封装ROS packge过程记录（c++）

Livox Lidar+海康Camera实时生成彩色点云

Livox Lidar+海康Camera实时三维重建生成RGB彩色点云地图

---

前言

大部分基于点云的三维重建算法，输出的mesh或点云都是不带rgb信息，我希望重建后的地图是带rgb信息的，因为不论是为了在重建的地图上做分割（引入纹理信息）还是人的直接观测（直观比对），rgb信息都很重要。

已封装为Ros package，地址：https://github.com/luckyluckydadada/LIVOX_COLOR

硬件环境

• 一个livox的激光雷达
• 一个hikrobot的工业相机
• 一台linux系统的电脑

软件环境

我的测试环境是：

• Ubuntu18.04
• ros（melodic）
• livox sdk（https://github.com/Livox-SDK/Livox-SDK）
• livox ros driver（https://github.com/Livox-SDK/livox_ros_driver）
• hikrobot camera sdk （mvs sdk 安装参考海康Camera MVS Linux SDK二次开发ROS packge过程记录（c++））  
• hikrobot camera ros driver （camera ros driver是我自己开发的，安装参考海康Camera MVS Linux SDK二次开发ROS packge过程记录（c++））  

联合标定

我们要得到两个传感器的坐标系的变换关系，以及相机的内参，才可以将相机的彩色信息赋值给点云。

标定过程参考livox的官方文档：https://github.com/Livox-SDK/livox_camera_lidar_calibration，文档对标定过程描述比较清晰，下面是对文档中没说明的做一些补充。

1. 对相机内参标定时，要根据自己使用的标定A4纸修改cameraCalib.launch中的row_number、col_number，number不是格子的个数而是交点的个数(下图是横六数九)。width和height是格子的大小，单位是mm，但是感觉没什么用。
   

2. 在获得标定板（泡棉板）的四个角点时，跳出的窗口并没有显示像素值，我将每副去畸变的照片都save下来，用windows的画图软件找的角点的像素值。
   只需在corner_photo.cpp：101行增加 cv::imwrite(photo_path+".bak.bmp",src_img); 即可。
   注意不要在原照片上直接采集角点，一定要先运行这个cpp对应的launch，得到去畸变后的照片，再在新的照片上获得像素值，再次运行这个launch执行后续操作。

3. 修改cornerPhoto.launch文件（可选操作）：

   <?xml version="1.0" encoding="UTF-8"?>
   <launch>
       <arg name="arg1" default="$(find camera_lidar_calibration)/../../data/photo/0.bmp"/>
       <param name="intrinsic_path"    value="$(find camera_lidar_calibration)/../../data/parameters/intrinsic.txt" />  <!-- intrinsic file -->
        <param name="input_photo_path"  value="$(arg arg1)" /> <!-- photo to find the corner -->
       <param name="ouput_path"        value="$(find camera_lidar_calibration)/../../data/corner_photo.txt" />  <!-- file to save the photo corner -->
       
       <node pkg="camera_lidar_calibration" name="cornerPhoto" type="cornerPhoto" output="screen"></node>
   
   </launch>

   修改后可以直接在命令行指定文件：roslaunch camera_lidar_calibration cornerPhoto.launch arg1:=/home/yijiankeji/data/photo/0.bmp 

4. pcdTransfer.launch中只有一个参数要修改：data_num，修改为你录制的bag包的数量。

生成彩色点云

进过标定得到intrinsic.txt和extrinsic.txt文件后就可以使用我的ros包进行彩色点云的生成了。

安装过程参看：https://github.com/luckyluckydadada/LIVOX_COLOR

需要注意的是catkin_make 执行前需要修改main.cpp中的void CalibrationData(void)函数，将你的标定结果写入对应的位置。

例如：

    // extrinsic
	// 0.0451423  -0.998715  0.0230348  0.00925535
	// 0.0558064  -0.0205011  -0.998231  0.0499455
	// 0.997421  0.046348  0.0548092  0.42788
	// 0  0  0  1
	extrinsicMat_RT.at<double>(0, 0) = 0.0451423;
	extrinsicMat_RT.at<double>(0, 1) = -0.998715;
	extrinsicMat_RT.at<double>(0, 2) = 0.0230348;
	extrinsicMat_RT.at<double>(0, 3) = 0.00925535;
	extrinsicMat_RT.at<double>(1, 0) = 0.0558064;
	extrinsicMat_RT.at<double>(1, 1) = -0.0205011;
	extrinsicMat_RT.at<double>(1, 2) = -0.998231;
	extrinsicMat_RT.at<double>(1, 3) = 0.0499455;
	extrinsicMat_RT.at<double>(2, 0) = 0.997421;
	extrinsicMat_RT.at<double>(2, 1) = 0.046348;
	extrinsicMat_RT.at<double>(2, 2) = 0.0548092;
	extrinsicMat_RT.at<double>(2, 3) = 0.42788;
	extrinsicMat_RT.at<double>(3, 0) = 0.0;
	extrinsicMat_RT.at<double>(3, 1) = 0.0;
	extrinsicMat_RT.at<double>(3, 2) = 0.0;
	extrinsicMat_RT.at<double>(3, 3) = 1.0;

	// intrinsic
	// 2875.097131590431  0  1369.668059923329;
	//  0  2896.420251825658  1114.244269170673;
	//  0  0  1
	// ditortion
	// -0.008326874784366894 -0.06967846599874981 0.006185220615585947 -0.01133018681519818  0.5462976722456516
	intrisicMat.at<double>(0, 0) = intrisic.at<double>(0, 0) = 2875.097131590431;
	intrisicMat.at<double>(0, 1) = 0.000000e+00;
	intrisicMat.at<double>(0, 2) = intrisic.at<double>(0, 2) = 1369.668059923329;
	intrisicMat.at<double>(0, 3) = 0.000000e+00;
	intrisicMat.at<double>(1, 0) = 0.000000e+00;
	intrisicMat.at<double>(1, 1) = intrisic.at<double>(1, 1) = 2896.420251825658;
	intrisicMat.at<double>(1, 2) = intrisic.at<double>(1, 2) = 1114.244269170673;
	intrisicMat.at<double>(1, 3) = 0.000000e+00;
	intrisicMat.at<double>(2, 0) = 0.000000e+00;
	intrisicMat.at<double>(2, 1) = 0.000000e+00;
	intrisicMat.at<double>(2, 2) = 1.000000e+00;
	intrisicMat.at<double>(2, 3) = 0.000000e+00;
	distCoeffs.at<double>(0) = -0.008326874784366894;
	distCoeffs.at<double>(1) = -0.06967846599874981;
	distCoeffs.at<double>(2) = 0.006185220615585947;
	distCoeffs.at<double>(3) = -0.01133018681519818;
	distCoeffs.at<double>(4) = 0.5462976722456516;

彩色点云对比原始livox点云

原始livox：

color livox：