减少漂移在3 d视觉测程法使用构成图形轨迹

打开生活的脚本

这个例子展示了如何减少估计的漂移轨迹(位置和姿态)的单眼相机使用3 d构成图优化。视觉测程法估计当前全球带来的相机(当前帧)。因为可怜的匹配或错误的3 d点三角,机器人轨迹往往倾向于漂移从地面真理。环路闭合检测和姿势图优化减少这种漂移和纠正错误。

对负载估计姿势图优化

负载估计摄像机姿态和循环闭合边缘。估计相机使用视觉测程法提出了计算。循环闭合边缘被发现前一帧,计算当前场景和评估当前帧之间的相对姿态和循环关闭候选人。相机帧取样于[1]。

%估计姿势负载(“estimatedpose.mat”);% Loopclosure边缘负载(“loopedge.mat”);% Groundtruth相机位置负载(“groundtruthlocations.mat”);

构建三维构成图

创建一个空构成图。

pg3D = poseGraph3D;

将节点添加到构成图,定义相对姿态和边缘信息矩阵的构成图。把估计姿势,给出转换相对姿态的[x yθqw qx qy求)向量。一个单位矩阵用于信息矩阵为每个姿势。

len =大小(estimatedPose, 2);informationmatrix = [1 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 1);%插入所有连续帧之间的相对姿态为k = 2:兰%相对当前和前一帧之间的姿势relativePose = estimatedPose {k - 1} / estimatedPose {k};%相对取向用四元数表示relativeQuat = tform2quat (relativePose);%相对冒充[x yθqw qx qy求)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);optimizedposes =节点(optimizedPosegraph);%相机轨迹图图estimatedposes =节点(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”);持有从传奇(“估计姿势图”,“地面实况姿势图”,“优化构成图”);视图(-20.8,-56.4);

引用

[1]Galvez-Lopez D。和j . d .缓慢的。“袋二进制字快速识别在图像序列的地方。”IEEE机器人。5号卷28日,2012年,页1188 - 1197。