ROS深度图转化为点云
自己编写C++的ROS代码,订阅D435i深度图像,转化为点云数据,并发布出去。
说明:D435i本身就可以输出点云,不需要自己编写代码。本博客的目的是通过自己编写深度图转点云代码来熟悉ROS工程的创建。
Step1:创建ROS工程
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/
catkin_make
cd ./src
catkin_create_pkg depth2cloud std_msgs roscpp pcl_conversions pcl_ros
# depth2cloud是创建的包名,后面的是依赖
Step2:源码编写
代码的整体由三部分组成:
- 订阅D435i的深度图 ( /camera/depth/image_rect_raw )
- 订阅D435i的深度相机参数 ( /camera/depth/camera_info )
- 利用深度图和相机参数,将深度图转换为点云数据并发布出去 ( /d435i_point_cloud )
在depth2cloud/src/下新建文件depth2cloud_node.cpp,进行源码编写:
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <iostream>
using namespace std;ros::Publisher pub_point_cloud2;bool is_K_empty = 1;
double K[9];
// [fx 0 cx]
// K = [ 0 fy cy]
// [ 0 0 1]void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
{// Step1: 读取深度图//ROS_INFO("image format: %s %dx%d", img_msg->encoding.c_str(), img_msg->height, img_msg->width);int height = img_msg->height;int width = img_msg->width;// 通过指针强制转换,读取为16UC1数据,单位是mmunsigned short *depth_data = (unsigned short*)&img_msg->data[0];// Step2: 深度图转点云sensor_msgs::PointCloud2 point_cloud2;pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);for(int uy=0; uy<height; uy++){for(int ux=0; ux<width; ux++){float x, y, z;z = *(depth_data + uy*width + ux) / 1000.0;if(z!=0){x = z * (ux - K[2]) / K[0];y = z * (uy - K[5]) / K[4];pcl::PointXYZ p(x, y, z);cloud->push_back(p);}}}// Step3: 发布点云pcl::toROSMsg(*cloud, point_cloud2);point_cloud2.header.frame_id = "world";pub_point_cloud2.publish(point_cloud2);
}void camera_info_callback(const sensor_msgs::CameraInfoConstPtr &camera_info_msg)
{// 读取相机参数if(is_K_empty){for(int i=0; i<9; i++){K[i] = camera_info_msg->K[i];}is_K_empty = 0;}
}int main(int argc, char **argv)
{ros::init(argc, argv, "ros_tutorial_node");ros::NodeHandle n;// 订阅D435i的深度图,在其回调函数中把深度图转化为点云,并发布出来ros::Subscriber sub_img = n.subscribe("/camera/depth/image_rect_raw", 100, img_callback);// 订阅D435i的深度相机参数ros::Subscriber sub_cmara_info = n.subscribe("/camera/depth/camera_info", 1, camera_info_callback);pub_point_cloud2 = n.advertise<sensor_msgs::PointCloud2>("/d435i_point_cloud", 1000);ROS_INFO("Runing ...");ros::spin();return 0;
}
Step3:修改CMakeLists.txt
执行catkin_create_pkg命令时,自动为我们生成了CMakeLists.txt,只需取消这两处注释:
# 第一处
add_executable(${PROJECT_NAME}_node src/depth2cloud_node.cpp)# 第二处
target_link_libraries(${PROJECT_NAME}_node${catkin_LIBRARIES})
Step4:编译
cd ~/catkin_ws
catkin_make
Step5:运行
先在另一个终端中打开D435i相机驱动程序:
roslaunch realsense2_camera rs_camera.launch
然后运行自己编写的depth2cloud节点:
source devel/setup.bash
rosrun depth2cloud depth2cloud_node
#Step6:可视化
打开rviz,点击左下角<add>,选择<By display type>中的<PointCloud2>,点击<OK>添加。
然后在<Displays>面板中,点开<PointCloud2>,将<Topic>选择为<d435i_point_cloud>,然后就就可以看到点云的可视化了。
参考:
[1] 动手学ROS(13):点云(PointCloud2)的发送与接收--python和c++示例 - 知乎
[2] D435i camera obtains the depth value of a certain point depth image (ROS implementation and official API call)
ROS深度图转化为点云相关推荐
- 利用深度图建立三维点云地图笔记
前言:这几天在独立地研究对RGBD图像序列,建立其三维点云地图.这是我研究生期间,毕业论文中的一点小工作.由于我并没有借鉴像RTAB-MAP等SLAM方法,所以本文仅仅能够帮助学习和理解是三维建图 ...
- python 将16位 png 深度图转化为伪彩色图
python 将16位 png 深度图转化为伪彩色图: 全部代码如下: import cv2 import os.path import glob import numpy as np from PI ...
- 基于Python深度图生成3D点云
文章目录 前言 二维RGB图像 成像原理 数据准备 图片加载 算法实现 生成点云 点云显示 完整代码 总结 前言 废话不多说,直接开造.这里的话我们有两个目标,第一个是如何把一个2维图片上的点映射到3 ...
- RGB深度图转换成点云-三维重建001
最近做实验,想实现深度估计到三维点云再到网格的生成,第一步做出深度图结合RGB图生成三维点云,感觉效果凑合, 供大家欣赏! RGB室内图: Depth Image:(隐隐约约能过看到点什么) 生成的结 ...
- ROS系统中实现点云聚类(realsense数据源)
本文主要介绍ROS系统中如何订阅并解码realsense点云数据,并对点云进行稀疏.去噪.聚类. 环境配置见<ROS系统中从零开始部署YoloV4目标检测算法(3种方式)> 需要安装的第三 ...
- pcl点云库python实现_如何有效地将ROS PointCloud2转换为pcl点云并在python中将其可视化...
我正在尝试对ROS中的kinect中的pointcloud进行一些分段.截至目前我有这个: import rospy import pcl from sensor_msgs.msg import Po ...
- 促进企业流量高质量转化,华为云CDN加速方案值得选择
伴随着互联网和数字化的发展,企业对于数据安全.需求响应等均提出了极高的要求.而为应对不断变化的企业需求,华为云基于CDN加速方案和Web应用防火墙(WAF)技术,为企业带来了速度和安全方面更好的联动体 ...
- ROS激光雷达信息、点云信息和PCL信息之间的转换
一.消息类型 sensor_msgs::LaserScan // ROS激光雷达信息 sensor_msgs::PointCloud ...
- 1/3 Ubuntu更换ROS的源(阿里云)
一 打开源的文件 命令如下: sudo gedit /etc/apt/sources.list 打开后删除或注释里面所有代码. 二 更换下载源 1 .进入阿里云源的网址:阿里云源 选择你的电脑Ubun ...
最新文章
- jsp error-page没有生效
- miui12 android版本,miui12基于安卓几版本开发的?miui12是安卓11吗
- 35.angularJS的ng-repeat指令
- 分享一个多线程实现[冒泡][选择][二分法]排序的例子
- 信息学奥赛一本通(C++)在线评测系统——基础(一)C++语言——1093:计算多项式的值
- mysql 全文本检索的列_Mysql 全文本检索
- 天啦噜!知道硬盘很慢,但没想到比 CPU Cache 慢 10000000 倍
- 使用LiteOS Studio图形化查看LiteOS在STM32上运行的奥秘
- 新疆大学c语言期末考试题库,2016年新疆师范大学教育科学学院C语言程序设计考研复试题库...
- Redhat7没有安装ifconfig命令的解决方法
- php重定向http请求
- 设计某高校学生选课管理系统
- keras-迁移学习-resnet101-踩过的坑
- 数字双极点低通滤波器-二阶巴特沃斯滤波器
- CISA要求联邦机构修复被震网病毒攻击利用的漏洞
- 单位自建网站服务器,企业自建网站的方式
- SQLServer中如何高效解析JSON格式数据
- 关闭 C4996 警告(_CRT_SECURE_NO_DEPRECATE)方法
- 康得新董事长是谁?_您人生董事会中的谁?
- 动态规划(DP)算法介绍