姿勢グラフを使用した 3 次元ビジュアル オドメトリの軌跡におけるドリフトの削減

この例では、3 次元姿勢グラフの最適化を使用して単眼カメラの推定軌跡 (位置と向き) におけるドリフトを減らす方法を説明します。ビジュアル オドメトリでは、カメラの現在のグローバルな姿勢 (現在のフレーム) を推定します。3 次元の点の三角形分割におけるマッチングが不適切であるか、誤差があることが原因で、ロボットの軌跡がグラウンド トゥルースからドリフトすることがよくあります。ループ閉じ込み検出と姿勢グラフの最適化により、このドリフトが減って誤差が補正されます。

姿勢グラフを最適化するための推定姿勢の読み込み

推定カメラ姿勢とループ閉じ込みエッジを読み込みます。推定カメラ姿勢はビジュアル オドメトリを使用して計算されます。ループ閉じ込みエッジは、現在のシーンを認識した前のフレームを検出して現在のフレームとループ閉じ込み候補の間の相対姿勢を推定することで計算されます。カメラ フレームは [1] からサンプリングされます。

% Estimated poses
load('estimatedpose.mat');
% Loopclosure edge
load('loopedge.mat');
% Groundtruth camera locations 
load('groundtruthlocations.mat');

3 次元姿勢グラフの作成

空の姿勢グラフを作成します。

pg3D = poseGraph3D;

相対姿勢を定義するエッジと姿勢グラフの情報行列を使用して、姿勢グラフにノードを追加します。変換として与えられている推定姿勢を、[x y theta qw qx qy qz] ベクトルとしての相対姿勢に変換します。各姿勢の情報行列には単位行列が使用されます。

len = size(estimatedPose,2);
informationmatrix = [1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1]; 
% Insert relative poses between all successive frames 
for k = 2:len
    % Relative pose between current and previous frame
    relativePose = estimatedPose{k-1}/estimatedPose{k};
    % Relative orientation represented in quaternions
    relativeQuat = tform2quat(relativePose);
    % Relative pose as [x y theta qw qx qy qz] 
    relativePose = [tform2trvec(relativePose),relativeQuat];
    % Add pose to pose graph
    addRelativePose(pg3D,relativePose,informationmatrix);
end

ループ閉じ込みエッジを追加します。このエッジは、現在のフレームから前のフレームへの既存の 2 つのノード間に追加します。

% Convert pose from transformation to pose vector.
relativeQuat = tform2quat(loopedge);
relativePose = [tform2trvec(loopedge),relativeQuat];
% Loop candidate
loopcandidateframeid = 1;
% Current frame
currentframeid = 100;

addRelativePose(pg3D,relativePose,informationmatrix,...
                loopcandidateframeid,currentframeid);

figure
show(pg3D);

姿勢グラフを最適化します。エッジの制約に基づいてノードが調整され、姿勢グラフ全体が改善されます。ドリフトの変化を確認するには、推定姿勢と新たに最適化された姿勢をグラウンド トゥルースに対してプロットします。

% Pose graph optimization
optimizedPosegraph = optimizePoseGraph(pg3D);
optimizedposes = nodes(optimizedPosegraph);
% Camera trajectory plots
figure
estimatedposes = nodes(pg3D);
plot3(estimatedposes(:,1),estimatedposes(:,2),estimatedposes(:,3),'r');
hold on
plot3(groundtruthlocations(:,1),groundtruthlocations(:,2),groundtruthlocations(:,3),'g');
plot3(optimizedposes(:,1),optimizedposes(:,2),optimizedposes(:,3),'b');
hold off
legend('Estimated pose graph','Ground truth pose graph', 'Optimized pose graph');
view(-20.8,-56.4);

参考文献

[1] Galvez-López, D., and J. D. Tardós. "Bags of Binary Words for Fast Place Recognition in Image Sequences." "IEEE Transactions on Robotics." Vol. 28, No. 5, 2012, pp. 1188-1197.