一. 背景介绍
现在很多智能导航场景都涉及到激光(毫米波,固态等)雷达和相机视觉信息融合,这里激光雷达一般都是指多线激光雷达,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<sensor_msgs::PointCloud2>("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<double>(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<double>(col) = cameraInfoMsg->D[col];
}
// 投影系数,获取投影矩阵3x4的数组的fx,fy,cx,cy元素
fx = static_cast<float>(cameraInfoMsg->P[0]);
fy = static_cast<float>(cameraInfoMsg->P[5]);
cx = static_cast<float>(cameraInfoMsg->P[2]);
cy = static_cast<float>(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<pcl::PointXYZ> pointCloud;
pcl::fromROSMsg(*point_cloud2_msg, pointCloud); //ros sensor_msgs::PointCloud2 -> pcl::PointCloud
//下采样后点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());
//体素滤波,稀疏点云
pcl::VoxelGrid<pcl::PointXYZ> 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<pcl::PointXYZ> pointCloud;
pcl::fromROSMsg(*point_cloud2_msg, pointCloud); //ros sensor_msgs::PointCloud2 -> pcl::PointCloud
//下采样后点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());
//体素滤波,稀疏点云
pcl::VoxelGrid<pcl::PointXYZ> 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<pcl::PointXYZ> pointCloud;
pcl::fromROSMsg(*point_cloud2_msg, pointCloud); //ros sensor_msgs::PointCloud2 -> pcl::PointCloud
// 单个彩色点: RGBXYZ
pcl::PointXYZRGB colored_3d_point;
// 存储处理后的彩色点云: [RGBXYZ,,,]
pcl::PointCloud<pcl::PointXYZRGB>::Ptr newRGBPointCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
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<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;
//存储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. 同上,进一步稀疏化,如图 正如前面讲基于结构光的深度相机,容易受环境光干扰,图中强光部分已经无点云分布:文章来源:https://www.toymoban.com/news/detail-400622.html
文章来源地址https://www.toymoban.com/news/detail-400622.html
到了这里,关于十九.在ROS系统基于点云和视觉图像数据融合构建3D点云场景的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!