从深度图像使用Visual里程计和优化的姿态图形建设占用地图
此示例示出了如何减少使用3- d姿态图形优化单目摄像机的估计的轨迹（位置和取向）的漂移。在这个例子中，你建立从深度图像，它可以同时在环境中导航可用于路径规划的入住地图。
荷载推算体式的姿态图形优化
负载估计摄像机姿态和循环闭合边缘。估计的相机姿态计算使用视觉里程计。通过查找看到当前场景的前一帧并估计当前帧与候选环闭包之间的相对位姿来计算环闭包边缘。摄像机帧从包含深度图像、摄像机姿态和地面真实位置[1]的数据集中采样。
加载（'estimatedpose.mat'）;％估算姿势加载（'loopedge.mat'）;％Loopclosure边缘加载（'groundtruthlocations.mat'）;％地面实测摄像头位置
构建3 d姿态图形
创建一个空的姿态图形。
pg3D = poseGraph3D;
在位姿图中添加节点，以边定义位姿图的相对位姿和信息矩阵。将估计的姿态(给定为转换)转换为相对姿态(an)[X Y THETA QW QX QY QZ]向量。每个位姿的信息矩阵使用单位矩阵。
len =大小(estimatedPose, 2);信息矩阵= [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 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）;optimizedPosegraph = optimizePoseGraph（pg3D）;optimizedposes =节点（optimizedPosegraph）;数字;显示（pg3D）;
创建入住率地图从深度图像和优化的姿态
从数据集[1]装入深度图像和相机参数。
加载（'depthimagearray.mat'）;％变量depthImages加载（'freburgK.mat'）;％变量K
创建3-d占用地图的每米50个细胞的分辨率。阅读深度图像迭代和使用该照相机参数和照相机的优化姿势转换点在深度图像。插入点作为点云在优化的姿态打造的地图。将显示所有点后的地图。因为有很多的深度图像，这一步可能需要几分钟。考虑取消对fprintf中命令打印的进展，图像处理。
map3d的= occupancyMap3D（50）;对于K = 1：长度（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]加尔韦斯-洛佩兹，D.，和J. D.Tardós。“图像序列中的二进制字的快速到位识别包。”IEEE交易上的机器人。卷。28，第5号，2012，第1188至1197年。
您还可以选择从下面的列表中的网站：