使用视觉测程法构建入住率从深度图像映射和优化构成图
这个例子展示了如何减少估计的漂移轨迹(位置和姿态)的单眼相机使用3 d构成图优化。在本例中,您构建一个入住率从深度图像地图,可以用于路径规划,在这样的环境中导航。
对负载估计姿势图优化
负载估计摄像机姿态和循环闭合边缘。估计摄像机视觉测程法提出了计算使用。循环闭合边缘被找到前一帧,计算当前场景和评估当前帧之间的相对姿态和循环关闭候选人。相机帧从一个数据集,其中包含采样深度图像,相机的姿势,和地面真理位置[1]。
负载(“estimatedpose.mat”);%估计姿势负载(“loopedge.mat”);% Loopclosure边缘负载(“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);optimizedPosegraph = optimizePoseGraph (pg3D);optimizedposes =节点(optimizedPosegraph);图;显示(pg3D);
从深度图像和优化提出了创建入住率地图
负载的深度图像和相机参数数据集[1]。
负载(“depthimagearray.mat”);%变量depthImages负载(“freburgK.mat”);%变量K
创建一个3 d入住率映射分辨率为50细胞每米。阅读的深度图像迭代和转换分深度图像使用相机参数的优化提出了相机。插入点的点云优化提出了构建地图。显示地图添加所有的点之后。因为有很多深度图像,这一步可能要占用几分钟的时间。考虑现在流命令打印图像处理的进展。
map3D = occupancyMap3D (50);为k = 1:长度(depthImages) points3D = exampleHelperExtract3DPointsFromDepthImage (depthImages} {k, k);%流(处理图像% d \ n, k);insertPointCloud (map3D optimizedposes (k,:), points3D, 1.5);结束
显示地图。
图;显示(map3D);xlim (2 [2]) ylim (2 [1]) zlim([0 4])视图([-201 47])
引用
[1]Galvez-Lopez D。和j . d .缓慢的。“袋二进制字快速识别在图像序列的地方。”IEEE机器人。5号卷28日,2012年,页1188 - 1197。
