利用姿态图减少三维视觉测程轨迹的漂移

打开实时脚本

这个例子展示了如何使用三维姿态图优化来减少单目摄像机估计轨迹(位置和方向)的漂移。视觉里程计估计相机的当前全局姿态(当前帧)。由于三维点三角剖分的不匹配或存在误差，机器人轨迹往往会偏离地面真相。环路闭合检测和位姿图优化减少了这种漂移并纠正了错误。

用于姿态图优化的负载估计姿态

加载估计的相机姿势和循环关闭边。估计的相机姿势计算使用视觉里程计。环路闭合边缘的计算方法是通过查找之前看到当前场景的帧，并估计当前帧与环路闭合候选帧之间的相对位置。摄像机帧从[1]采样。

%估计姿势负载(“estimatedpose.mat”）;%环闭边负载(“loopedge.mat”）;% Groundtruth摄影机位置负载(“groundtruthlocations.mat”）;

建立三维姿态图

创建一个空的姿态图。

pg3D = poseGraph3D;

向姿态图中添加节点，用边定义姿态图的相对姿态和信息矩阵。将估计的姿态，以转换的形式给出，转换为相对的姿态[x y theta qw qx qy qz]向量。每个姿态的信息矩阵使用一个单位矩阵。

len = size(estimatedPose,2);信息矩阵= [1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 1 0 0 1];在所有连续帧之间插入相对姿态为K = 2:len当前帧与前一帧之间的相对位姿relativePose = estimatedPose{k-1}/estimatedPose{k};%用四元数表示的相对方向relativeQuat = tform2quat(relativePose);%相对位姿为[x y theta qw qx qy qz]relativePose = [tform2trvec(relativePose)，relativeQuat];将姿态添加到姿态图addRelativePose (pg3D relativePose informationmatrix);结束

添加一个循环闭合边。将这条边添加到当前帧到前一帧的两个现有节点之间。

将姿态从变换转换为姿态向量。relativeQuat = tform2quat(loopedge);relativePose = [tform2trvec(loopedge)，relativeQuat];%循环候选Loopcandidateframeid = 1;%当前帧Currentframeid = 100;addRelativePose (pg3D relativePose informationmatrix,.．.loopcandidateframeid currentframeid);图显示(pg3D);

优化姿态图。根据边缘约束对节点进行调整，改善整体位姿图。为了看到漂移的变化，将估计的姿态和新的优化姿态绘制成与地面真实相对应的图。

%姿态图优化optimizedPosegraph = optimizePoseGraph(pg3D);optimizedpose =节点(optimizedPosegraph);%相机轨迹图figure estimatedpose = nodes(pg3D);plot3 (estimatedposes (: 1) estimatedposes (:, 2), estimatedposes (:, 3),“r”）;持有在plot3 (groundtruthlocations (: 1) groundtruthlocations (:, 2), groundtruthlocations (:, 3),‘g’）;plot3 (optimizedposes (: 1) optimizedposes (:, 2), optimizedposes (:, 3),“b”）;持有从传奇(“估计姿势图”，“Ground truth pose graph”，“优化的姿态图”）;视图(-20.8,-56.4);

参考文献

[1] Galvez-López, D.和J. D. Tardós。“用于图像序列中快速位置识别的二进制单词袋。”IEEE机器人汇刊。Vol. 28 No. 5, 2012, pp. 1188-1197。