matlab读取realsense bag文件数据

代码参考：https://dev.intelrealsense.com/docs/matlab-wrapper

本示例的功能：

matlab代码：给出bag文件路径，显示realsense相机型号、序列号、ID、RGB图像、D图像、点云以及RGB相机和D相机的内参等信息、基线长度。

clc;clear;close all;
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 指定读取的bag路径
filename = 'E:\\data\\realsense_bag.bag';
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Make Config object to manage pipeline settings
cfg = realsense.config();
validateattributes(filename, {'char','string'}, {'scalartext', 'nonempty'}, '', 'filename', 1);
% Tell pipeline to stream from the given rosbag file
cfg.enable_device_from_file(filename)

% Make Pipeline object to manage streaming
pipe = realsense.pipeline();
% Start streaming from the rosbag with default settings
profile = pipe.start(cfg);

% Get frames. We discard the first couple to allow
% the camera time to settle
for i = 1:5
    fs = pipe.wait_for_frames();
end

% Stop streaming
pipe.stop();

%----显示彩色图、深度图、点云------------
%---------------------------------------
% show rgb img
rgb = fs.get_color_frame();
rgbwidth = rgb.get_width();
rgbheight = rgb.get_height();
rgbdata = rgb.get_data();
I = permute(reshape(rgbdata',[3,rgbwidth,rgbheight]),[3 2 1]);
figure;imshow(uint8(I));title('rgb image');
%---------------------------------------
% show depth img
depth = fs.get_depth_frame();
dwidth = depth.get_width();
dheight = depth.get_height();
units = depth.get_units();
depthdata = (reshape(depth.get_data(),dwidth,dheight))';
depthdata = double(depthdata)*units;
depthdata(depthdata>5.0)=0; % 超过5米的值置0
figure;imshow(depthdata,[]);title('depth image');colormap jet;colorbar;

% Make Colorizer object to prettify depth output
colorizer = realsense.colorizer();
% Colorize depth frame
color = colorizer.colorize(depth);
% Get actual data and convert into a format imshow can use
% (Color data arrives as [R, G, B, R, G, B, ...] vector)
data = color.get_data();
img = permute(reshape(data',[3,color.get_width(),color.get_height()]),[3 2 1]);
% Display image
figure;imshow(img);
title('Colorized depth frame');
%---------------------------------------
% show point cloud
pointcloud = realsense.pointcloud();
points = pointcloud.calculate(depth);

vertices = points.get_vertices();
X = vertices(:,1,1);
Y = vertices(:,2,1);
Z = vertices(:,3,1);

figure;
plot3(X,Y,Z,'.'); 
grid on
hold off;
view([45 30]);

xlim([-0.5 0.7])
ylim([-1 1])
zlim([1.2 1.7])

xlabel('X');
ylabel('Y');
zlabel('Z');

ptCloud = pointCloud(vertices);
figure;pcshow(ptCloud);colormap jet;
xlim([-0.5 0.7])
ylim([-1 1])
zlim([1.2 1.7])

xlabel('X');
ylabel('Y');
zlabel('Z');

%--------   打印相机信息  --------------
%---------------------------------------
% Get streaming device's name
dev = profile.get_device();
camera_name = dev.get_info(realsense.camera_info.name);
serial_number = dev.get_info(realsense.camera_info.serial_number);
product_id = dev.get_info(realsense.camera_info.product_id);
% 在命令行窗口显示相机信息
camera_name
serial_number
product_id

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bagfile = rosbag(filename);
AvailableTopics_D455 = bagfile.AvailableTopics; % 可从该变量查看所有相机信息

% realsense D455:
% sensor_0/Depth_0/: 深度相机
% sensor_0/Infrared_1/：左红外相机
% sensor_0/Infrared_2/：右红外相机
% sensor_1/Color_0: RGB相机
% device_0/sensor_2: IMU

%---------------------------------------
% 读RGB相机信息
Ccamera_info = select(bagfile,'Topic','device_0/sensor_1/Color_0/info/camera_info');
CCamInfo = readMessages(Ccamera_info);
Color_width  = CCamInfo{1}.Width; % 图像宽度
Color_height  = CCamInfo{1}.Height; % 图像高度
Color_Intr = reshape(CCamInfo{1}.K,3,3)'; % 相机内参

Transform_info = select(bagfile,'Topic','device_0/sensor_1/Color_0/tf/0');
tfInfo = readMessages(Transform_info);
color_tf = tfInfo{1,1}.Translation; % 相机位移

Image_info = select(bagfile,'Topic','device_0/sensor_1/Color_0/image/data');
img_Num = Image_info.NumMessages; % 帧数
imgInfo = readMessages(Image_info,5);% 获取第5帧的信息
img = imgInfo{1,1}.Data;
I = permute(reshape(img',[3,Color_width,Color_height]),[3 2 1]);
figure;imshow(I);title('Color Frame 5');
% 在命令行窗口显示RGB相机信息
Color_width
Color_height
Color_Intr
color_tf
img_Num

%---------------------------------------
% 读深度传感器信息
Dcamera_info = select(bagfile,'Topic','device_0/sensor_0/Depth_0/info/camera_info');
DCamInfo = readMessages(Dcamera_info);
Depth_width  = DCamInfo{1}.Width;
Depth_height  = DCamInfo{1}.Height;
Depth_Intr = reshape(DCamInfo{1}.K,3,3)';
DData_info = select(bagfile,'Topic','device_0/sensor_0/Depth_0/image/data');
Dimg_Num = DData_info.NumMessages; % 帧数
DimgInfo = readMessages(DData_info,5);% 获取第5帧的信息
depth = DimgInfo{1,1}.Data;% 注意：depth原是16位无符号整型，这里按uint8取出
% 在命令行窗口显示如下信息：
Depth_Intr
Depth_width
Depth_height

%---------------------------------------
% 读基线宽度
Baseline_info = select(bagfile,'Topic','device_0/sensor_0/option/Stereo_Baseline/value');
BaselineInfo = readMessages(Baseline_info);
baseline  = BaselineInfo{1,1}.Data;
% 在命令行窗口显示如下信息：
baseline