轨迹绘制(Matlab)

在使用rgbdslam的时候,发现三维建图效果并不好,事实上是很糟糕,原因还在查找中…

对产生的轨迹进行绘制,保存的trajectory_estimate.txt格式如下所示:

保存的都是很基本的格式,时间+平移+旋转.绘制轨迹其实只要有平移就行了,因为在图上体现不出一个点的方向

具体代码如下, 很简单,仅做个记录:

clear all;
clc;

fidin=fopen('trajectory_estimate.txt');                               % 打开test2.txt文件             
fidout=fopen('trajectory_deal.txt','w');                       % 创建MKMATLAB.txt文件
while ~feof(fidin)                                      % 判断是否为文件末尾               
    tline=fgetl(fidin);                                 % 从文件读行   
    if double(tline(1))>=48&&double(tline(1))<=57       % 判断首字符是否是数值
       fprintf(fidout,'%s\n\n',tline);                  % 如果是数字行，把此行数据写入文件MKMATLAB.txt
       continue                                         % 如果是非数字继续下一次循环
    end
end
fclose(fidout);
transformation=importdata('trajectory_deal.txt');      % 将生成的MKMATLAB.txt文件导入工作空间，变量名为MK，实际上它不显示出来 

plot3(transformation(:, 2), transformation(:, 3), transformation(:, 4), 'ro-', 'Linewidth', 2);
grid on;

可以看到轨迹跑来跑去,也有可能是实验室的场景太过于相似,反正结果不理想…继续加油↖(^ω^)↗