主要内容
利用视觉测程和优化的姿势图从深度图像构建占用地图
这个例子展示了如何使用三维姿态图优化减少单目摄像机估计轨迹(位置和方向)中的漂移。在本例中，您可以从深度图像构建一个占用地图，该地图可用于在该环境中导航时的路径规划。
负载估计的姿态图优化
加载估计的相机姿态和环路闭合边缘。估计的相机姿态是用视觉里程计计算的。循环结束边缘的计算是通过找到看到当前场景的前一帧，并估计当前帧和循环结束候选帧之间的相对姿态来实现的。相机帧从包含深度图像、相机姿态和地面真实位置[1]的数据集中采样。
负载(“estimatedpose.mat”）;%估计姿势负载(“loopedge.mat”）;% Loopclosure边缘负载(“groundtruthlocations.mat”）;地面真实相机位置
构建三维姿态图
创建一个空的姿势图。
pg3D = poseGraph3D;
在位姿图中添加节点，用边定义位姿图的相对位姿和信息矩阵。转换估计的姿态，给出的转换，相对姿态作为一个[x y qw qx qy qz]向量。每个姿态的信息矩阵使用单位矩阵。
len =大小(estimatedPose, 2);信息矩阵= [1 0 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1];在所有连续帧之间插入相对姿态为k = 2:兰当前帧和前一帧之间的相对姿态relativePose = estimatedPose {k - 1} / estimatedPose {k};%用四元数表示的相对方向relativeQuat = tform2quat (relativePose);%相对位姿为[x y 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);optimizedPosegraph = optimizePoseGraph (pg3D);optimizedposes =节点(optimizedPosegraph);图;显示(pg3D);
从深度图像和优化的姿势创建占用地图
从数据集[1]加载深度图像和相机参数。
负载(“depthimagearray.mat”）;%变量depthImages负载(“freburgK.mat”）;%变量K
创建一个3d占用地图，分辨率为50个单元/米。迭代读取深度图像，并使用相机参数和优化的相机姿态转换深度图像中的点。插入点作为点云在优化的姿态，以建立地图。在添加所有点后显示地图。因为有很多深度图像，这个步骤可能需要几分钟。考虑现在流命令打印图像处理的进度。
map3D = occupancyMap3D (50);为k = 1:length(depthImages) points3D = exampleHelperExtract3DPointsFromDepthImage(depthImages{k}， k);% fprintf('处理图像%d\n'， k);insertPointCloud (map3D optimizedposes (k,:), points3D, 1.5);结束
显示地图。
图;显示(map3D);xlim (2 [2]) ylim (2 [1]) zlim([0 4])视图([-201 47])
参考文献
[1] Galvez-López, D，和J. D. Tardós。用于图像序列中快速位置识别的二进制词袋。IEEE机器人学报。第28卷第5期，2012年，第1188-1197页。
你也可以从以下列表中选择一个网站: