KITTI資料集測試 ：MATLAB繪制groundtruth 真實地圖

在poses目錄下，包含00.txt-10.txt 11個序列，每一個檔案包換Nx12個表格，N代表幀數。每一行利用3x4轉移矩陣代表左邊相機系統位姿，轉移矩陣将目前幀左邊相機系統中的一個點映射到第0幀的坐标系統中。轉移矩陣中平移的部分表示目前相機位置(相對于第0幀)。

Groundtruth

sequence 00的地面地圖如下圖所示：

%% display groundtruth of KITTI poses
% include sequence from 00-10.txt
 
% read from poses
filename = '/home/qinhaidong/桌面/data_odometry_gray/poses/00.txt';
fid = fopen(filename);
fseek(fid, 0, 'bof');
lastposition = ftell(fid);
disp(['start position:',num2str(lastposition)]);
 
groundtruth = [];
 
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);
 
    groundtruth = [groundtruth; [transform(1,4), transform(3,4)]];
    lastposition = ftell(fid);
    disp(['lastposition:',num2str(lastposition)]);
 
end
 
% display ground truth
scatter(groundtruth(:,1),groundtruth(:,2));
 
fclose(fid);