用matlab从rosbag中提取rgb图片和depth图片

前情提要

要从ros bagfile中提取depth图片，网络上找了个很久提取depth图片的教程，都没找到，自己胡乱折腾出来了，做个记录。

以下是matlab的程序：

matlab读取bag文件

clear all
clc
bag = rosbag('rgbd.bag');

从bag中读取特定的topic

bSel_rgb = select(bag,'Topic','/camera/color/image_raw');
bSel_depth = select(bag,'Topic','/camera/aligned_depth_to_color/image_raw');

读取image信息

rgb_message=select(bSel_rgb,'MessageType','sensor_msgs/Image');
depth_message=select(bSel_depth,'MessageType','sensor_msgs/Image');

rgb_message信息中包含了其他非图片类别的信息，比如height等，提取其中的图片信息：

data_rgb=readMessages(rgb_message);
data_depth=readMessages(depth_message);

提取第一帧图片的信息

image_rgb=data_rgb{1,1};
image_depth=data_depth{1,1};

image_chen_rgb=readImage(image_rgb);
image_chen_depth=readImage(image_depth);

显示提取的图片

figure;imshow(image_chen_rgb,[]);
figure;imshow(image_chen_depth,[]);

这里提取的是registered depth image和对应的rgb image, 感觉有的地方对应得上，有的地方就不一定。
rgb图像中小手指跟depth图片中对应得不好
然而茶杯还是对应得很好。

没有可能是因为小手指那边有太多电线，分布在不同的深度，所以导致错误；而茶杯的背景相对整洁，所以算法没有出错。
？？？搞不懂，请大神指教。我也自己再试一下。

试了一下简单的背景，桌子上撒了几颗钉子
左图是钉子的rgb图片，右边是钉子的深度图片，坐标原点不一样，看上去没对齐，但是实际上可以看到四颗钉子的顶点在两幅图中在差不多的位置，这说明深度图与彩图对齐了