Livox Lidar + HIKROBOT Camera系列
最近在开发相机和激光雷达融合的slam算法,主要用于三维重建,想实时的得到彩色点云地图,传感器选择了海康威视的工业相机和大疆的固态激光雷达。
海康Camera MVS Linux SDK二次开发封装ROS packge过程记录(c++)
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,文档对标定过程描述比较清晰,下面是对文档中没说明的做一些补充。
- 对相机内参标定时,要根据自己使用的标定A4纸修改cameraCalib.launch中的row_number、col_number,number不是格子的个数而是交点的个数(下图是横六数九)。width和height是格子的大小,单位是mm,但是感觉没什么用。
-
在获得标定板(泡棉板)的四个角点时,跳出的窗口并没有显示像素值,我将每副去畸变的照片都save下来,用windows的画图软件找的角点的像素值。
只需在corner_photo.cpp:101行增加 cv::imwrite(photo_path+".bak.bmp",src_img); 即可。
注意不要在原照片上直接采集角点,一定要先运行这个cpp对应的launch,得到去畸变后的照片,再在新的照片上获得像素值,再次运行这个launch执行后续操作。 -
修改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
-
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: