用matlab画ORBSLAM2运行保存的轨迹文件（笔记）

ORBSLAM2运行kitti数据集得到的轨迹txt文件是Nx12的表格，N代表帧数。每一行利用3x4转移矩阵代表左边相机系统位姿，转移矩阵将当前帧左边相机系统中的一个点映射到第0帧的坐标系统中。转移矩阵中平移的部分表示当前相机位置(相对于第0帧)。

CameraTrajectory.txt格式如下：

MATLAB代码如下：

clear all;clc;
filename = 'CameraTrajectory.txt';
fid = fopen(filename);
fseek(fid, 0, 'bof');
lastposition = ftell(fid);
disp(['start position:',num2str(lastposition)]);

orb = [];

while fgetl(fid) ~= -1, % end of line check

    fseek(fid, lastposition, 'bof');
    line = textscan(fid,'%f %f %f %f %f %f %f %f %f %f %f %f\n',1);
    line = [line{:}];
    transform = vec2mat(line,4);

    orb = [orb; [transform(1,4), transform(3,4)]];
    lastposition = ftell(fid);
    disp(['lastposition:',num2str(lastposition)]);

end

% display 
scatter(orb(:,1),orb(:,2));

fclose(fid);

ORBSLAM2的轨迹如下：

参考：https://blog.youkuaiyun.com/ykwjt/article/details/88244098

https://blog.youkuaiyun.com/qq_33591712/article/details/83448476