十五. 单线激光雷达和视觉信息融合

单线激光雷达和视觉信息融合案例

很多智能应用场景涉及到激光雷达和相机视觉信息融合,一般都是指多线激光雷达,至少也得16线激光吧;但多线激光雷达动不动数万的价格,让很多技术人员无法尝试.我尝试使用微型机器人或扫地机器人常用的单线激光雷达,与相机视觉做个信息融合实践,以作为多线激光雷达和视觉信息融合的补充,共大家参考实践.

本实践基于ROS系统开发,硬件平台采用一款搭载Jetson Nano的四轮机器人.平台搭载一款单线激光雷达,可实现 12M 测距范围， 8000 次/秒的测量频率，扫描频率15HZ;同时还搭载一款深度相机,具有1080P RGB普通相机功能，又有深度相机功能(因为要实践和单线激光雷达做融合,深度相机有此功能咱也不使用).

另外,本实例主要出于熟悉雷达和视觉融合流程考虑,只写了可视化调试代码,并没有实际的业务逻辑代码.
先规划下我们都有哪些工作要做?
一. 获取单线激光雷达外参,并发布激光的外参到tf;
二. 获取相机外参,并发布相机的外参到tf;
三. 对相机内参做标定,并获取相机的内参参数.这样可以把相机坐标系下的3D坐标点无畸变地投影到相机的像素平面;
四. 对相机和单线激光雷达联合标定,目的就是建立单线激光雷达坐标系下3D点云的point和相机坐标系下3D点的对应关系.
这里分两种情况:
1>. 如果你对激光雷达和相机坐标转换精度要求不高,且你在第一步和第二步测量计算的外参精度很有把握,你可以不做专门的联合标定,直接利用第一,二步发布的tf做坐标系间的变换;
2>. 对于商品化产品,一般对激光雷达和相机坐标转换精度要求较高,且考虑到装配误差,需要做独立的联合标定, 详细的联合标定方法请参考其他文献;
五. 相机内参数据获取,及单线激光雷达数据和相机数据的时间线对齐;
六. 将单线激光雷达的LaserScan数据从极坐标系表示转换到激光雷达笛卡尔坐标系表示,并生成需要的点云(PointCloud)数据L(1xn);
七. 对上述单线激光雷达坐标系下的点云数据L(1xn),做水平插值,以保证点云密度,生成新的点云数据L(1xm).
八. 对激光雷达笛卡尔坐标系下的单行点云数据L(1xm)做垂直方向扩展,向下扩展到地面和墙面交界处k,向上扩展约0.5米高j,生成激光雷达笛卡尔坐标系下的点云数据L((k+1+j)xm);
九. 将上一步生成的点云(PointCloud)数据L((k+1+j)xm)利用一,二步发布的tf变换到相机坐标系下,生成相机坐标系下的点云数据C((k+1+j)xm),同时利用获取的相机内参信息,将点云映射到相机像素平面坐标(x,y),并从像素平面(current_image_frame)获取对应颜色RGB值;
十. 根据以上各参数和生成的数据,进行点云数据和视觉信息融合;
1>. 二维相机图像像素数据 --融合到--> 三维空间的点云数据;
2>. 三维空间的点云数据 --融合到--> 二维相机图像像素数据;
十一. 需要细节优化,性能调整的地方.

为了便于对比,我固定选用家里的一个局部场景做演示,这是一个标准的H户型的主次卧和卫生间中间的过廊场景,左右分别是主次卧的卧室门,中间是卫生间门.

直接通过相机,在rqt_image_view查看是这样的:

通过单线激光雷达,在rviz中查看laser scan是这个样的(我在图中用白色选中的倒U形区域,我们称为ROI,这也是相机的可视区域):

融合视觉信息的雷达扫描效果如下图(未做优化):

放大效果(中间门的中间有条黑色竖条,是我的激光雷达在交接处无数据,后期会优化处理):

下面按照前面的规划,一步一步来完成,涉及ROS包,节点的创建,已经ROS初始化等基本操作的内容将忽略.(代码中设计的类名,我统一改为YourClass)

一. 获取激光雷达外参,并发布激光的外参到tf;
从硬件和结构人员那里获取激光雷达在整体机器人上的结构参数,就是相对于机器人整体参考系的坐标信息,在适当的launch文件执行时发布激光雷达的位姿到tf:

<node pkg="tf" type="static_transform_publisher" name="base_foot_print_to_laser" args="0.1 0 0.1 -1.57 0 0 base_footprint laser_link 40"/>

二. 获取相机外参,并发布相机的外参到tf;
同上面激光雷达类似:

<node pkg="tf" type="static_transform_publisher" name="base_footprint_to_camera" args="0.10 0 0.10 1.57 3.14 1.57 base_footprint camera_link 40"/>

三. 对相机内参做标定,并获取相机的内参参数.这样可以把相机坐标系下的3D坐标点无畸变地投影到相机的像素平面;
相机内参标定的方法,相关书籍和网上都有说明,这里不再阐述.通过相机标定会得到相机内参矩阵和畸变系数，畸变系数可以消除相机凸透镜的畸变效应，有了相机内参的信息就可以把相机坐标系下的3维点投影到相机的2维成像像素平面。

ROS系统中一般标定好的相机内参,可以通过订阅"/camera/camera_info"主题(topic)获取.

//订阅相机内参: 
node_h.subscribe("/camera/camera_info", 1, &YourClass::intrinsicValueCallback);

回调函数获取摄像机内参参数及畸变系数,详细查看ROS文档sensor_msgs::CameraInfo定义.
这里我们获取到一些相机内参信息,后面从相机获取到图像数据时,会利用camera_intrinsic_value和distortion_coefficients对图像做去畸变,而fx,fy,cx,cy则用来对相机坐标系下的点云point做投影计算,映射到成像像素平面. 此处变量是对象属性,需要预先定义好.

void YourClass::intrinsicValueCallback(const sensor_msgs::CameraInfo::ConstPtr &intrinsic_value_msg){image_frame_size.height = intrinsic_value_msg->height;image_frame_size.width = intrinsic_value_msg->width;// 相机内参camera_intrinsic_value = cv::Mat(3, 3, CV_64F);//相机内参变换矩阵3x3,把3D点投影到2D像素平面时使用.  后面获取相机图像时,会用来去畸变for (int row = 0; row < 3; row++){for (int col = 0; col < 3; col++){camera_intrinsic_value.at<double>(row, col) = intrinsic_value_msg->K[row * 3 + col];}}// 相机畸变参数. For "plumb_bob"模式, the 5 parameters are: (k1, k2, t1, t2, k3).//k 1  ，k 2  ，k 3  表示径向畸变参数；t 1 ，t 2  表示切向畸变参数；distortion_coefficients = cv::Mat(1, 5, CV_64F);for (int col = 0; col < 5; col++){distortion_coefficients.at<double>(col) = intrinsic_value_msg->D[col];}//Projection/camera matrix 3x4// 投影系数,获取投影矩阵3x4的数组的fx,fy,cx,cy元素,fx = static_cast<float>(intrinsic_value_msg->P[0]);fy = static_cast<float>(intrinsic_value_msg->P[5]);cx = static_cast<float>(intrinsic_value_msg->P[2]);cy = static_cast<float>(intrinsic_value_msg->P[6]);intrinsics_sub_.shutdown(); //关闭不再使用的intrinsics subscriber
}

四. 对相机和单线激光雷达联合标定,目的就是建立单线激光雷达坐标系下3D点云的point和相机坐标系下3D点的对应关系.
考虑到这里是单线激光雷达, 不方便利用现有的联合标定工具包做联合标的,所以只能在单线激光雷达和相机的测量计算精度上下功夫,
确保在第一步和第二步发布到tf的外参尽量精准可靠;
具体使用时,只需要利用ROS系统的TF功能,即可做单线激光雷达坐标系下3D点云的point和相机坐标系下3D点的坐标变换;
举例如下:
1. 获取目标frame和源frame之间的变换关系transform

tf::StampedTransform transform;
try
{transform_listener.lookupTransform("camera", "laser", ros::Time(0), transform);
}
catch (tf::TransformException ex)
{    }

2. 利用transform做坐标系变换

tf::Vector3 tf_point(pc_point.x, pc_point.y, pc_point.z); //点云点转tf点类型
tf::Vector3 tf_point_transformed = transform * tf_point; //对单个点做坐标系变换
return pcl::PointXYZ(tf_point_transformed.x(), tf_point_transformed.y(),tf_point_transformed.z());//tf点类型转点云点

五. 相机内参数据获取,及单线激光雷达数据和相机数据的时间线对齐;
因为单线激光雷达和相机信息融合,依赖于相机内参,所以程序启动后,需要等待订阅的相机内参数据获取. 代码见第三步;
又因为单线激光雷达主题消息和相机图像主题消息,发布的频率不一致,我们需要对两种类型主题消息的stamp做比对;以保证
激光雷达扫描数据和相机图像数据均反应同一时刻的状态,这部分代码比较简单,故略;

//订阅相机主题消息
node_h.subscribe("/camera/image_raw", 1, &YourClass::cameraImageCallback);
//相机主题消息处理函数
void YourClass::cameraImageCallback(const sensor_msgs::Image::ConstPtr &image_msg){...cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(image_msg, "bgr8");cv::Mat image = cv_image->image;//使用上面第三步获取的相机内参信息,利用openCV对图像做去畸变处理. current_image_frame 类型cv::Mat cv::undistort(image, current_image_frame, camera_intrinsic_value,distortion_coefficients);...
}//订阅激光雷达扫描主题消息
node_h.subscribe("scan", 1, &YourClass::laserScanCallback);
//激光雷达扫描主题消息处理函数
void YourClass::laserScanCallback(const sensor_msgs::LaserScan::ConstPtr &laserScan_msg)
{
...略
}

六. 将单线激光雷达的LaserScan数据从极坐标系表示转换到激光雷达笛卡尔坐标系表示,并生成需要的点云(PointCloud)数据L(1xn);
七. 对上述单线激光雷达坐标系下的点云数据L(1xn),做水平插值,以保证点云密度,生成新的点云数据L(1xm).
Note: 第六步和第七步合并处理

单线激光雷达的数据类型为sensor_msgs::LaserScan,这不同于多线激光雷达的点云数据类型PointCloud. sensor_msgs::LaserScan的数据实际上是极坐标的形式. 我们需要代码转换为PointCloud格式.
进行之前,需要先介绍下我使用的这款单线激光雷达的数据特性.
测距范围: 0.15米--12米;
每次测量旋转一周的数据数量是: 720个;
起始角度: -179.013169 度
结束角度: 180.013264 度
相邻测量点角度差: 0.499341度约0.5度
激光雷达的数据采用极坐标的形式发送,极坐标0度,朝向小车正后方,90度朝向在小车右侧,180度朝向小车前方,270度朝向小车左侧,激光雷达逆时针旋转,
测距数据从序列为:[-179.013169,180.013264],实际数据是弧度值.
而小车的载体坐标系是,X轴正向指向小车正前方,Y轴正向小车正左方,Z轴正向垂直向上.

如下图所示,小车的载体坐标系Oxyz,激光雷达的坐标系O'x'y'z',激光雷达的极坐标系:

所以这里需要两个坐标系变换:
1. 激光雷达数据的极坐标系旋转到和激光雷达三维坐标系一致的方位;
2. 旋转后的激光雷达极坐标系数据转换为激光雷达的三维笛卡尔积坐标系数据;

现在,等待相机内参,相机图像数据和激光雷达数据都准备好后,就可以开始:
已知数据:
1. 单线激光雷达坐标系和相机坐标系的变换tf: camera_lidar_tf;
2. 相机内参信息: image_frame_size, camera_intrinsic_value, distortion_coefficients, fx,fy,cx,cy;
3. 来自相机的去畸变图像数据: cv::Mat current_image_frame;
4. 来自单线激光雷达的扫描数据: sensor_msgs::LaserScan::ConstPtr laserScan_msg;
5. 宏定义:
#define H_LIDAR_INTERPOLATION (true) //是否对雷达数据水平方向插值
#define H_LIDAR_INTERPOLATION_IN_RADIAN (0.00349) //弧度
#define H_LIDAR_INTERPOLATION_IN_RADIAN_3 (0.002) //弧度
#define V_LIDAR_INTERPOLATION (true) //是否对雷达数据垂直方向插值
#define V_UP_LIDAR_INTERPOLATION_IN_PIXEL (99) //向上垂直插值个数
#define V_DOWN_LIDAR_INTERPOLATION_IN_PIXEL (30) //向下垂直插值个数

代码: 激光雷达扫描数据转点云数据,同时做了过滤,插值,点云转换,按像素平面顺序重排:

//激光雷达扫描数据转点云数据,同时做了过滤,插值,点云转换,按像素平面顺序重排:pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_msg(new pcl::PointCloud<pcl::PointXYZ>);cloud_msg->header.frame_id = laserScan_msg->header.frame_id;//"laser_link";cloud_msg->height = 1; //垂直扩展前高度为1cloud_msg->width = laserScan_msg->ranges.size(); //水平插值前宽度int pointCount = laserScan_msg->ranges.size();std::vector<pcl::PointXYZ>  pointVector; float lastLeftRadian = -1;float lastRightRadian = -1;//range data [m] (Note: values < range_min or > range_max should be discarded)for(int i = 0; i < pointCount; i++) //遍历激光雷达扫描数据{   float distance = laserScan_msg->ranges[i];//过滤无效距离if( distance < laserScan_msg->range_min || distance > laserScan_msg->range_max )continue;/*小车载体坐标系是前向X轴正向,左侧Y轴正向;而雷达的极坐标系为后侧为0轴,前向180度.逆时针旋转,到180度时,从-179开始累加1直到0轴所以,返回的雷达数据中,前方摄像头可视区域内,雷达数据存储顺序是[-179,-150] + [150,180],单位度.两个坐标系相差180度或一个PI,我们对雷达角度信息加一个PI,以使两个坐标系起始轴一致.*/float radian = laserScan_msg->angle_min+laserScan_msg->angle_increment*i + M_PI;float degree = RAD2DEG(radian);/*雷达数据转换到和载体坐标系一致后,有效区域数据顺序为:[330,360]和[0,30],为了和摄像头拍摄的像素平面X-Y坐标一致,需要重新排序为:[30,0] + [360,330]*///过滤雷达数据,只保留相机拍摄区数据. 左前方雷达数据,倒序存放为[30,0]if( degree > 0 and degree <= 45 ){float x = cos(radian) * distance;//这里可以对cos(),sin()值缓存优化float y = sin(radian) * distance;cloud_msg->points.insert(cloud_msg->points.begin(),pcl::PointXYZ(x, y, 0));//z坐标0,是激光雷达本地坐标系z坐标//水平插值.  Note: 这里暂时采用较粗暴的插值方法,只是按极坐标角度方向插值,后期要做优化.
#ifdef H_LIDAR_INTERPOLATIONfloat step = H_LIDAR_INTERPOLATION_IN_RADIAN;if( distance > 3.0f )step = H_LIDAR_INTERPOLATION_IN_RADIAN_3;if( lastLeftRadian >= 0 && (radian-lastLeftRadian) > step ){for(float rad = lastLeftRadian; rad < radian; rad+= step){float x2 = cos(rad) * distance; //这里可以对cos(),sin()值缓存优化float y2 = sin(rad) * distance;cloud_msg->points.insert(cloud_msg->points.begin(),pcl::PointXYZ(x2, y2, 0));//z坐标0,是激光雷达本地坐标系z坐标}}lastLeftRadian = radian;
#endif}//过滤雷达数据,只保留相机拍摄区数据. 右前方雷达数据,倒序存放为[360,330]if( degree >= 315 && degree <= 360 ){float x = cos(radian) * distance; //这里可以对cos(),sin()值缓存优化float y = sin(radian) * distance;pointVector.insert(pointVector.begin(),pcl::PointXYZ(x, y, 0));//z坐标0,是激光雷达本地坐标系z坐标//水平插值  Note: 这里暂时采用较粗暴的插值方法,只是按极坐标角度方向插值,后期要做优化.
#ifdef H_LIDAR_INTERPOLATIONif( lastRightRadian >= 0 && (radian-lastRightRadian) > H_LIDAR_INTERPOLATION_IN_RADIAN ){for(float rad = lastRightRadian; rad < radian; rad+= H_LIDAR_INTERPOLATION_IN_RADIAN){float x2 = cos(rad) * distance; //这里可以对cos(),sin()值缓存优化float y2 = sin(rad) * distance;pointVector.insert(pointVector.begin(),pcl::PointXYZ(x2, y2, 0));//z坐标0,是激光雷达本地坐标系z坐标}}lastRightRadian = radian;
#endif}}//调整雷达数据顺序和计算笛卡尔坐标后,按相机像素图像顺序,从左到右,保存到cloud_msg->points. 方便后面直接映射到相机像素平面for( int i = 0; i < pointVector.size(); i++ )cloud_msg->points.push_back(pcl::PointXYZ(pointVector.at(i).x, pointVector.at(i).y, pointVector.at(i).z));//z坐标暂存变换后极坐标角度cloud_msg->width = cloud_msg->points.size(); //水平插值后宽度

八. 对激光雷达笛卡尔坐标系下的单行点云数据L(1xm)做垂直方向扩展,向下扩展到地面和墙面交界处k,向上扩展约0.5米高j,生成激光雷达笛卡尔坐标系下的点云数据L((k+1+j)xm);

#ifdef V_LIDAR_INTERPOLATIONpcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>);point_cloud->header.frame_id = laserScan_msg->header.frame_id;//"laser_link";point_cloud->height = V_UP_LIDAR_INTERPOLATION_IN_PIXEL + V_DOWN_LIDAR_INTERPOLATION_IN_PIXEL + 1;point_cloud->width = cloud_msg->width;//向上扩展99行for( int up = 1; up <= V_UP_LIDAR_INTERPOLATION_IN_PIXEL; up++ ){for(int i = 0; i < cloud_msg->points.size(); i++){point_cloud->points.push_back(pcl::PointXYZ(cloud_msg->points[i].x, cloud_msg->points[i].y, up/200.0));//这里z坐标应该为正数}}//这是原始激光雷达数据水平插值后单行点云数据, L(1xm)for(int i = 0; i < cloud_msg->points.size(); i++){point_cloud->points.push_back(pcl::PointXYZ(cloud_msg->points[i].x, cloud_msg->points[i].y, cloud_msg->points[i].z));//这里z坐标应该为0}//向下扩展30行for( int down = 1; down <= V_DOWN_LIDAR_INTERPOLATION_IN_PIXEL; down++ ){for(int i = 0; i < cloud_msg->points.size(); i++){point_cloud->points.push_back(pcl::PointXYZ(cloud_msg->points[i].x, cloud_msg->points[i].y, -down/200.0));//这里z坐标开始为负数}}cloud_msg->height = point_cloud->height;cloud_msg->width = point_cloud->width;cloud_msg->points.clear();for(int i = 0; i < point_cloud->points.size(); i++) cloud_msg->points.push_back(point_cloud->points[i]);
#endif

到这里我们生成了激光雷达本地坐标系下的一系列点云数据L((k+1+j)xm), 针对上述代码是130行,m列,m值大小不固定.

九. 将上一步生成的点云(PointCloud)数据L((k+1+j)xm)利用一,二步发布的tf变换到相机坐标系下,生成相机坐标系下的点云数据C((k+1+j)xm),同时利用获取的相机内参信息,将点云映射到相机像素平面坐标(x,y),并从像素平面(current_image_frame)获取对应颜色RGB值;

//定义新的带3D坐标和RGB的点云数据
pcl::PointXYZRGB colored_3d_point;pcl::PointCloud<pcl::PointXYZRGB>::Ptr outColorPointCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
outColorPointCloud->points.clear();std::vector<pcl::PointXYZ> cam_cloud(cloud_msg->points.size());for(int i = 0; i < cloud_msg->points.size(); i++) 
{//把点云里面的3D坐标从激光雷达坐标系,变换到摄像机的坐标系cam_cloud[i] = transformPoint(cloud_msg->points[i], camera_lidar_tf);// 再使用相机内参将三维空间点投影到像素平面int col = int(cam_cloud[i].x * fx / cam_cloud[i].z + cx);int row = int(cam_cloud[i].y * fy / cam_cloud[i].z + cy);//根据映射后的坐标,从当前摄像机的当前图像帧current_image_frame上获取颜色值,并保存对应的3D位置信息if ((col >= 0) && (col < image_frame_size.width) && (row >= 0) && (row < image_frame_size.height) ) {//生成的点云坐标,在rviz中显示和小车前后位置相反,系前面极坐标旋转导致,这里强制把x,y(分别乘-1)相对原点做旋转colored_3d_point.x = -cloud_msg->points[i].x;  //乘-1colored_3d_point.y = -cloud_msg->points[i].y; //乘-1colored_3d_point.z = cloud_msg->points[i].z;cv::Vec3b rgb_pixel = current_image_frame.at<cv::Vec3b>(row, col);colored_3d_point.r = rgb_pixel[2] * 2;colored_3d_point.g = rgb_pixel[1] * 2;colored_3d_point.b = rgb_pixel[0] * 2;outColorPointCloud->points.push_back(colored_3d_point);}
}
//转换为ROS的点云类型
sensor_msgs::PointCloud2 out_colored_cloud_msg;
pcl::toROSMsg(*outColorPointCloud, out_colored_cloud_msg);
out_colored_cloud_msg.header = laserScan_msg->header;//发布"colored_point_cloud" 主题消息
fusion_cloud_pub_.publish(out_colored_cloud_msg);

十. 根据以上各参数和生成的数据,进行点云数据和视觉信息融合;
1>. 二维相机图像像素数据 --融合到--> 三维空间的点云数据;
因为本案例仅仅出于演示单线激光雷达和视觉融合目的,不涉及具体功能逻辑,做到发布融合视觉信息的点云到rviz显示,已经达到目的;
执行rviz,查看topic: PointCloud2的"colored_point_cloud",即可.
2>. 三维空间的点云数据 --融合到--> 二维相机图像像素数据;
此步骤与上述相似,只需将已经获得的摄像机坐标系下的点云数据,映射并修改相机像素平面(current_image_frame)的坐标(x,y)像素值即可.
只是处理时,点云数据建议做下稀疏处理,因为上面水平插值和垂直扩展后,点云数据太密了.

看看在rviz中效果图(未优化):

一.相机视觉效果(rqt_image_view):

2.单线激光雷达扫描图(rviz):

3.单线雷达和视觉融合效果(rviz: 正视图):

4.单线雷达和视觉融合效果(rviz: 左视图,因为水平插值未优化,有明显片状切片现象):

5.单线雷达和视觉融合效果(rviz: 右视图,因为水平插值未优化,有明显片状切片现象):

6.单线雷达和视觉融合效果(rviz: 后视图):

7.单线雷达和视觉融合效果(rviz: 俯视图,俯视角度问题,雷达点和视觉点云看起来有些错位):

十一. 下一步细节优化,性能调整计划.
1. 由极坐标转笛卡尔坐标过程中的cos()和sin()值,因为每次处理雷达扫描都是720个一样的值,可以缓存做优化,避免每次都重复计算;

2. 单线激光雷达水平插值算法优化,从在极坐标系下基于角度插值改为雷达笛卡尔坐标系下的基于3D坐标插值,这样最终融合视觉信息后,在rviz中不会呈现片状切片.
3. 水平插值和垂直扩展算法,可以改进为根据景物距离采用不同的插值和扩展密度;
4. 下一步试试单线激光雷达和视觉融合是否可以应用到SLAM和导航中,以及配合AMCL做定位.

最后:

应大家要求,我上传了配到的: 单线激光雷达和视觉信息融合 ROS实践功能包

当时随便写着玩玩,代码也写的比较乱, 本不想拿出来献丑, 很多朋友要求,就发出来.