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数据，单位是mm
    unsigned 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)