十九.在ROS系统基于点云和视觉图像数据融合构建3D点云场景

一. 背景介绍

        现在很多智能导航场景都涉及到激光(毫米波,固态等)雷达和相机视觉信息融合,这里激光雷达一般都是指多线激光雷达,16线,64线,甚至更多线数. 但多线激光雷达动不动数万的价格,让很多技术人员无法尝试.我前面写过一篇使用单线激光雷达和相机视觉融合的文章, 但那个过于基础,纯是技术学习目的. 这里我使用一款深度相机来做3D点云和相机视觉的融合, 构建一个彩色3D点云场景. 并基于该点云做稀疏化处理,以模拟出多线激光雷达的效果.

         本实践基于ROS系统开发,硬件平台采用一款搭载Jetson Nano的四轮机器人.平台搭载一款深度相机,该深度相机是基于奥比中光 Astra Pro 方案的定制版相机，相机具备 1080P RGB 普通相机功能和基于结构光原理深度相机功能。

具体参数见下表。

内容	参数
RGB 像素	1080P
深度分辨率	640*480/320*240
深度最大帧率	30FPS
视频分辨率	1280*720
视频最大帧率	30FPS
视场角（FOV）	H 58.4° x V 45.7°
精度	1m: ±3mm
工作范围	0.6-8m

        由于这是一款基于结构光原理的的深度相机,决定了其与基于TOF原理的深度相机存在较大的差异,缺点主要表现在:

1）容易受环境光干扰，室外体验差;

2）随检测距离增加，精度会变差;

3)  检测距离近.
以上3点,确定了其只能应用小场景环境,比如室内. 由上表也可以看出,其有效距离为:0.6-8m.

二.准备工作

除了最基础的系统平台工作外,我们需要做一些必要准备工作:

1.RGB相机的内参标定,用以实现将相机3D坐标系中某点,映射到相机的成像像素平面;

2.深度相机和RGB相机的联合标定, 虽然他们在一个结构设备上,但仍存在位置差,比如我的相机拍出的RGB图像和点云图像实际是错位的,需要联合标定. 这一步的目的是为了实现将基于深度相机的点云通过坐标系变换, 转换到RGB相机的3D坐标系下;

3.基于步骤2,在ROS系统准备RGB相机和深度相机点云的TF坐标系, 即这两个物理设备在空间上相对于车辆质心的坐标. 以保证可以在相机坐标系和点云坐标系间利用ROS接口做坐标系变换(lookupTransform());

三.主要思路和步骤

       有了上面的准备工作, 还不能着急编写代码, 我们需要捋清楚为了实现我们的目标还需要哪些更细节的步骤, 先想清楚再动手做. 这里先给出一张流程图:

 

结合上图思路:

1. 利用ROS接口订阅(camera_info,image,point_cloud)消息: 

cameraInfo_sub_ = node_h.subscribe("/camera/camera_info", 1, &EntryClass::cameraInfoCallback, this);

cameraImage_sub_ = node_h.subscribe("/camera/image_raw", 1, &EntryClass::cameraImageCallback, this);

pointCloud_sub_ = node_h.subscribe("/camera/depth/points", 1, &EntryClass::pointCloudCallback, this);

//用于发布融合视觉图像后的彩色点云
fusion_cloud_pub_ = node_h.advertise("camera/depth/rgb_points", 1);

//---------------------------------
void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr &intrinsic_value_msg);
void cameraImageCallback(const sensor_msgs::Image::ConstPtr &image_msg);
void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &point_cloud2_msg);

        这里"/camera/camera_info"消息用于获取相机内参相关信息(消息名称请以你平台实际为准),主要包括图像尺寸,畸变系数,投影系数等参数.

详细信息,请参考sensor_msgs::CameraInfo: sensor_msgs/CameraInfo Documentation

    image_frame_size.height = cameraInfoMsg->height;
    image_frame_size.width = cameraInfoMsg->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(row, col) = cameraInfoMsg->K[row * 3 + col];
        }
    }

    // 相机畸变参数. For "plumb_bob"模式, the 5 parameters are: (k1, k2, t1, t2, k3).
    distortion_coefficients = cv::Mat(1, 5, CV_64F);
    for (int col = 0; col < 5; col++)
    {
        distortion_coefficients.at(col) = cameraInfoMsg->D[col];
    }

    // 投影系数,获取投影矩阵3x4的数组的fx,fy,cx,cy元素
    fx = static_cast(cameraInfoMsg->P[0]);
    fy = static_cast(cameraInfoMsg->P[5]);
    cx = static_cast(cameraInfoMsg->P[2]);
    cy = static_cast(cameraInfoMsg->P[6]);

2. 在cameraImageCallback()回调函数中获取相机原始RGB图像,并利用获取到的相机内参,对原始RGB图像做去畸变处理.

cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(image_msg, "bgr8");
cv::Mat image = cv_image->image;

//图像去畸变, 使用相机内参和畸变系数可以图像去畸变
if( true )
    cv::undistort(image, current_image_frame, camera_intrinsic_value, distortion_coefficients);
else //如果接口获取的图像已经做了去畸变处理,不用在处理
    current_image_frame = image;

        处理后的图片current_image_frame作为当前图片帧,稍后用于和最近的同时间序列的点云做融合处理. 也可以publish出去用rqt_image_view测试观察使用.

3.在pointCloudCallback回调函数中处理点云信息.  主要工作是: 利用pcl的体素滤波,稀疏化点云;利用前期系统发布的RGB相机和深度相机的坐标系变换关系,将基于深度相机3D坐标系的点云坐标变换到RGB相机3D坐标系下的点云坐标:

pcl::PointCloud pointCloud;
pcl::fromROSMsg(*point_cloud2_msg, pointCloud); //ros sensor_msgs::PointCloud2 -> pcl::PointCloud

//下采样后点云
pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud());

//体素滤波,稀疏点云
pcl::VoxelGrid voxel_filter;
voxel_filter.setInputCloud(pointCloud);
voxel_filter.setLeafSize(0.1f,0.1f,0.1f);
voxel_filter.filter(*cloud_filtered);

pcl::PointXYZ camera_point;
for(int i = 0; i < cloud_filtered->points.size(); i++) 
{
    //把点云里面的3D坐标从点云坐标系,变换到摄像机的坐标系[x',y',z']
    camera_point = transformPoint(cloud_filtered->points[i], camera_depth_tf);
    ...
    ...
}

4. 基于第3步,将转换后的RGB相机坐标系下的点云坐标,利用前期获取的相机内参的投影系数,进一步将点云3D坐标,映射到RGB相机的成像像素平面:

pcl::PointCloud pointCloud;
pcl::fromROSMsg(*point_cloud2_msg, pointCloud); //ros sensor_msgs::PointCloud2 -> pcl::PointCloud

//下采样后点云
pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud());

//体素滤波,稀疏点云
pcl::VoxelGrid voxel_filter;
voxel_filter.setInputCloud(pointCloud);
voxel_filter.setLeafSize(0.1f,0.1f,0.1f);
voxel_filter.filter(*cloud_filtered);

pcl::PointXYZ camera_point;
for(int i = 0; i < cloud_filtered->points.size(); i++) 
{
    //把点云里面的3D坐标从点云坐标系,变换到摄像机的坐标系[x',y',z']
    camera_point = transformPoint(cloud_filtered->points[i], camera_depth_tf);
    
    // 再使用相机内参将相机三维空间点投影到相机像素平面[x,y]
    int col = int(camera_point.x * fx / camera_point.z + cx);
    int row = int(camera_point.y * fy / camera_point.z + cy);
    ...
}

        此处得到的(col,row),为RGB相机成像像素平面的2D坐标(x,y), 也就是上面第2步保存的当前图片帧current_image_frame中的2D坐标(x,y).  注意时序上保持两者同步, 代码中同步相关代码已略.

5.  接上一步,从当前图片帧中提取出每个2D坐标(x,y)的像素颜色值(r,g,b,), 保存到该2D点对应的3D点云坐标点内, 形成基于RGB相机3D坐标系下的3D坐标彩色点云.

pcl::PointCloud pointCloud;
pcl::fromROSMsg(*point_cloud2_msg, pointCloud); //ros sensor_msgs::PointCloud2 -> pcl::PointCloud

// 单个彩色点: RGBXYZ
pcl::PointXYZRGB colored_3d_point;

// 存储处理后的彩色点云: [RGBXYZ,,,]
pcl::PointCloud::Ptr newRGBPointCloud(new pcl::PointCloud);
newRGBPointCloud->points.clear();

pcl::PointXYZ camera_point;
for(int i = 0; i < pointCloud.points.size(); i++) 
{
    //把点云里面的3D坐标从点云坐标系,变换到摄像机的坐标系[x',y',z']
    camera_point = transformPoint(pointCloud.points[i], camera_depth_tf);
    
    // 再使用相机内参将相机三维空间点投影到相机像素平面[x,y]
    int col = int(camera_point.x * fx / camera_point.z + cx);
    int row = int(camera_point.y * fy / camera_point.z + cy);
    
    //根据映射后的坐标(col,row),从当前相机的当前图像帧current_image_frame上获取颜色值,并保存对应的3D位置信息
    if ((col >= 0) && (col < image_frame_size.width) && (row >= 0) && (row < image_frame_size.height) ) {
        colored_3d_point.x = camera_point.x;
        colored_3d_point.y = camera_point.y;
        colored_3d_point.z = camera_point.z;
            
        //RGB
        cv::Vec3b rgb_pixel = current_image_frame.at(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;
        //存储RGBXYZ点到点云
        newRGBPointCloud->points.push_back(colored_3d_point);
    }
}

6. 至此, 我们已经得到一系列基于RGB相机坐标系的3D彩色点云, 下一步将其转化为ROS系统点云消息sensor_msgs::PointCloud2,发布即可.

sensor_msgs::PointCloud2 out_colored_cloud_msg;
pcl::toROSMsg(*newRGBPointCloud, out_colored_cloud_msg); //pcl::PointCloud --> sensor_msgs::PointCloud2

//设置Header
out_colored_cloud_msg.header = point_cloud2_msg->header;

//发布"colored_point_cloud" Topic
fusion_cloud_pub_.publish(out_colored_cloud_msg);

7. 第3步中的变换处理(transformPoint(point, camera_depth_tf)) 代码如下:

片段1:
tf::StampedTransform camera_depth_tf;

//这里用于获取RGB相机和深度相机的坐标系变换关系
try
{
    transform_listener.lookupTransform(image_frame_id, depth_frame_id, ros::Time(0), camera_depth_tf);
}
catch (tf::TransformException ex)
{
    ROS_INFO("FindTransform : %s", ex.what());
}


片段2:
//利用获取的camera_depth_tf变换关系,对点云坐标进行变换,  第2个参数为片段1的:camera_depth_tf
pcl::PointXYZ MyClass名::transformPoint(const pcl::PointXYZ& in_point, const tf::StampedTransform& transform)
{
    tf::Vector3 tf_point(in_point.x, in_point.y, in_point.z);
    tf::Vector3 tf_point_transformed = transform * tf_point;

    return pcl::PointXYZ(tf_point_transformed.x(), tf_point_transformed.y(), tf_point_transformed.z());
}

四.实现效果

        1. 先看原始点云,由于点云过于密集即全部白色,我切换个角度,以便可以看出立体感.

正如前面讲基于结构光的深度相机,容易受环境光干扰,图中强光部分已经无点云分布.

2. 输出为RGB相机坐标系下的彩色点云,效果如下:

 3. 换个角度,查看立体感效果, 如果深度相机不可见(盲区)的地方,比如桌面,呈空洞状态.

4. 由于这样的彩色点云过于密集, 对于下一步的任务处理说来,计算量较大,我们对彩色点云做稀疏化处理,使其更接近多线雷达点云. 方法一,利用pcl库的体素滤波下采样原始点云, 使其稀疏化,效果如下:

 

5. 方法二,在原始点云的3D空间做稀疏化,效果如下:

5. 方法三,从当前图片帧的2D空间,即相机成像平面, 做稀疏化,效果如下:

 6. 同上,进一步稀疏化,如图 正如前面讲基于结构光的深度相机,容易受环境光干扰,图中强光部分已经无点云分布: