matlab绘制slam轨迹

首先录制integrated_to_init话题数据，生成bag文件，然后在matlab中处理，处理代码：

%打开bag文件
filepath=fullfile('D:','data','transform1.bag');
bag=rosbag(filepath);

%提取bag文件中的odometry数据,并读取到data中
geometry_message=select(bag,'MessageType','nav_msgs/Odometry');
data=readMessages(geometry_message);
size_d = size(data,1);

%观察data中数据量大小，建立一个三维数组，将所有数据读入position数组
%注意y和z坐标需要调换
position=zeros(size_d,3);
for i=1:size_d
    position(i,1)=data{i,1}.Pose.Pose.Position.X;
    position(i,2)=data{i,1}.Pose.Pose.Position.Z;
    position(i,3)=data{i,1}.Pose.Pose.Position.Y;
end

%绘制三维图像
for i=1:size_d
    plot3(position(i,1),position(i,2),position(i,3),'r.','markersize',5);
    hold on
end

%绘制二维图像
for i=1:size_d
    plot(position(i,1),position(i,2),'r.','markersize',5);
    hold on
end