ROS+Gazebo+Kinect+MATLAB显示深度图 sensor_msgs::PointCloud2 ROS点提取

1.前言

最近使用ros+gazebo+kinect，并在matlab中订阅处理数据显示kinect RGB图和深度图。由于使用的是仿真kinect模型，非真实kinect硬件，不能获取直接的深度图显示。为此这段时间对获取matlab显示RGB和Depth图做了学习。

2.matlab订阅话题

1. RGB topic订阅
   ‘/camera/rgb/image_raw’ 为Gazebo中仿真kinect关于RGB图的topic。

   subI = rossubscriber('/camera/rgb/image_raw');%订阅
   I = readImage(receive(subI));%获取数据
   imshow(I);%显示
   

2. Depth topic订阅
   同上

   subP = rossubscriber(depth_topic_name);
   D = readXYZ(receive(subP));
   

3.深度图说明

1. RGB topic简单直接的RGB图，显示为正常的图片。但Depth获取的到是空间的三维坐标信息，用在储存上为65536X3的数据结构。由于kinect获取信息大小也就是图大小为640X480的，故大小为65536，其中对应为（x,y,z）的坐标。
   2.深度图中的数据为深度信息，但在显示上往往用灰度图显示。其处理过程即将Z坐标转换为灰度图中的数据。
   订阅得到的x,y，z对应是在坐标系下（x，y）下的深度信息，x，y并不代表图像中的位置，三个数据均为double类型。但在存储上显示为对应像素点的信息，做默认对应点的深度值。

• 数据处理
  将65536X3的数据中提取为480X640的数据结构，对应像素点储存深度值转换的值：

    function depth = displaydepth(xyz)
    scale = 65536/900;%转换比例
    for i =1:480%数据格式转换
        x = (i-1)*640 + 1;
        y = i * 640;
        temp = xyz(x:y, 3)';
        depth(i,1:640) = temp;
    end
  

4.结果显示

RGB图和深度图仿真中的结果：

5. sensor_msgs::PointCloud2 (x,y,z)提取

在ROS获取得到的sensor_msgs::PointCloud2的格式比较复杂.
理论上数据存在data[]数组中,但是:
在本人的一次数据中,显示(H,W)=(480,640),显示data[]长度为:98300400,是480*640=307200的32倍,且均为整数,不是double的,因此其有自己的编码格式.

• 解决:需要通过sensor_msgs::PointCloud进行转换才可以得到数据,代码如下:
• C++

#include 
#include 
#include 
 
double pointCloud2ToZ(const sensor_msgs::PointCloud2 &msg)
{
	sensor_msgs::PointCloud out_pointcloud;
	// 转换为point格式
	sensor_msgs::convertPointCloud2ToPointCloud(msg, out_pointcloud);
	for (int i=0; i<out_pointcloud.points.size(); i++) {
		cout << out_pointcloud.points[i].x << ", " << out_pointcloud.points[i].y << ", " << out_pointcloud.points[i].z << endl;
	}
	cout << "------" << endl;
	return out_pointcloud.points[0].z;
}

• python

#coding:utf-8
import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2
 
def callback_pointcloud(data):
    assert isinstance(data, PointCloud2)
    gen = point_cloud2.read_points(data)
    print type(gen)
    for p in gen:
        print p 

6.点云转换为深度灰度图

 Mat depth(480,640,CV_8UC1);
    int x,y;
    double min,max;
    min = 2; max =0.0;
    for(int i=0;i<out_pointcloud.points.size(); i++)
    {
        if(out_pointcloud.points[i].z < min)
            min = out_pointcloud.points[i].z;
        if(out_pointcloud.points[i].z > max)
            max = out_pointcloud.points[i].z;
    }
    double scale = 255 / (max-min);
    for (int i=0; i<out_pointcloud.points.size(); i++) {
		// cout << out_pointcloud.points[i].x << ", " << out_pointcloud.points[i].y << ", " << out_pointcloud.points[i].z << endl;
        x= (int)(i/640);
        y = i % 640;
        cout<<out_pointcloud.points[i].z<<" ";
        // convert to 0-255
        depth.at<uchar>(x,y)= round((out_pointcloud.points[i].z - min) * scale);
        if((i % 640) == 639)
            cout<<endl<<endl;
    }
    string str("/home/txt/data/");
    str = str + to_string(flag) + ".jpg";
    imwrite(str,depth);