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

这篇具有很好参考价值的文章主要介绍了十九.在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());

三.主要思路和步骤

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

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

结合上图思路:

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. 先看原始点云,由于点云过于密集即全部白色,我切换个角度,以便可以看出立体感.

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

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

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

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

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

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

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

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

 

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

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

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

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

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

十九.在ROS系统基于点云和视觉图像数据融合构建3D点云场景十九.在ROS系统基于点云和视觉图像数据融合构建3D点云场景文章来源地址https://www.toymoban.com/news/detail-400622.html

到了这里,关于十九.在ROS系统基于点云和视觉图像数据融合构建3D点云场景的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • ROS系统读取USB相机图像数据

    usb_cam功能包简介 为了丰富机器人与外界的交互方式,已经增加了与机器人的语音交互方式,不仅使机器人能够说话发声,还能听懂我们说的话,但是如果只有语音交互的话机器人就是一个盲人,无法看到这个色彩斑斓的大千世界,因此我们就需要为机器人增加视觉识别功能

    2024年04月08日
    浏览(48)
  • 基于 ZYNQ 的双目视觉图像采集系统设计(四)

    1、axi_hp0_wr.v 模块代码解析         该模块实现 AXI HP 总线写入数据到 DDR3 的操作。该模块的接口如下。 rst_n 为系统复位信号; i_clk 、 i_data_rst_n 、 i_data_en 和 i_data 为 FPGA 逻辑需要写入到 DDR3 的数据输入接口。 i_clk 为同步时钟信号, i_data_rst_n 用于复位 FIFO , i_data_en 拉高表

    2024年02月04日
    浏览(36)
  • 2.ROS机器视觉——ROS图像(imgmsg)与opencv(cv2)对接

    参考: 古月学院和ROS机器人开发实践 目标 :实现ROS系统读取摄像头的图像,ROS读取的图像数据转化为opencv中的图像,opencv对接受的图像进行处理,最后返回给ROS系统可视化输出。 安装opencv库与相关的接口包 由于我用的ROS-Melodic版本,其中roscore只能在python2中执行,而视觉部

    2024年02月16日
    浏览(33)
  • 【图像分类】基于计算机视觉的坑洼道路检测和识别(ResNet网络,附代码和数据集)

    写在前面: 首先感谢兄弟们的关注和订阅,让我有创作的动力,在创作过程我会尽最大能力,保证作品的质量,如果有问题,可以私信我,让我们携手共进,共创辉煌。 (专栏订阅用户订阅专栏后免费提供数据集和源码一份,超级VIP用户不在服务范围之内,不想订阅专栏的

    2024年02月06日
    浏览(53)
  • 【图像融合】Dif-Fusion:基于扩散模型的红外/可见图像融合方法

    颜色在人类的视觉感知中起着重要的作用,反映了物体的光谱。然而, 现有的红外和可见光图像融合方法很少探索如何直接处理多光谱/通道数据,并实现较高的彩色保真度 。本文提出了一种 利用扩散模型diffusion来生成多通道输入数据的分布 ,提高了多源信息聚合的能力和

    2024年02月09日
    浏览(80)
  • 基于小波变换的图像融合(附加视频融合)代码

    小波分析是一个比较难的分支,用户采用小波变换,可以实现图像压缩,振动信号的分解与重构等,因此在实际工程上应用较广泛。小波分析与Fourier变换相比,小波变换是空间域和频率域的局部变换,因而能有效地从信号中提取信息。小波变换通过伸缩和平移等基本运算,实

    2023年04月21日
    浏览(49)
  • 图像融合论文阅读:CoCoNet: 基于多层特征集成的耦合对比学习网络多模态图像融合

    @article{liu2023coconet, title={Coconet: Coupled contrastive learning network with multi-level feature ensemble for multi-modality image fusion}, author={Liu, Jinyuan and Lin, Runjia and Wu, Guanyao and Liu, Risheng and Luo, Zhongxuan and Fan, Xin}, journal={International Journal of Computer Vision}, pages={1–28}, year={2023}, publisher={Springer} } 论文级

    2024年02月04日
    浏览(51)
  • 图像融合论文阅读:CrossFuse: 一种基于交叉注意机制的红外与可见光图像融合方法

    @article{li2024crossfuse, title={CrossFuse: A novel cross attention mechanism based infrared and visible image fusion approach}, author={Li, Hui and Wu, Xiao-Jun}, journal={Information Fusion}, volume={103}, pages={102147}, year={2024}, publisher={Elsevier} } 论文级别:SCI A1 影响因子:18.6 📖[论文下载地址] 💽[代码下载地址] 以往的交

    2024年01月15日
    浏览(52)
  • 雷达视觉融合算法RODNet数据处理代码解读

    论文RODNet:A Real-Time Radar Object Detection Network Cross-Supervised by Camera-Radar Fused Object 3D 原论文地址:https://arxiv.org/abs/2102.05150 代码地址:https://github.com/yizhou-wang/RODNet 摘要 在目标检测中,雷达虽然在恶劣天气下有精准采集信号的优势,但缺乏语义信息,若对采集的雷达信号进行手

    2024年02月11日
    浏览(36)
  • 基于Python+OpenCV的图像搜索引擎(CBIR+深度学习+机器视觉)含全部工程源码及图片数据库下载资源

    本项目旨在开发一套完整高效的图像搜索引擎,为用户提供更加便捷的图片搜索体验。为了实现这一目标,我们采用了 CBIR(Content-based image retrieval)技术,这是目前主流的图像搜索方法之一。CBIR 技术基于图像内容的相似性来检索相似的图像,相比于传统的图像搜索方法,

    2024年02月08日
    浏览(60)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包