1.前言
最近使用ros+gazebo+kinect,并在matlab中订阅处理数据显示kinect RGB图和深度图。由于使用的是仿真kinect模型,非真实kinect硬件,不能获取直接的深度图显示。为此这段时间对获取matlab显示RGB和Depth图做了学习。
2.matlab订阅话题
-
RGB topic订阅
‘/camera/rgb/image_raw’ 为Gazebo中仿真kinect关于RGB图的topic。subI = rossubscriber('/camera/rgb/image_raw');%订阅 I = readImage(receive(subI));%获取数据 imshow(I);%显示
-
Depth topic订阅
同上subP = rossubscriber(depth_topic_name); D = readXYZ(receive(subP));
3.深度图说明
- 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 <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
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);