立体视觉同步定位和映射
视觉同时定位和地图(vSLAM),指的是计算摄像机的位置和姿态的过程对周围环境,同时环境的映射。这个过程只使用视觉输入的相机。申请vSLAM包括增强现实、机器人、自动驾驶。
vSLAM可以由使用单眼相机。然而,由于深度不能准确计算使用一个相机,地图和的规模估计未知和漂移轨迹。此外,引导系统,多个视图需要产生一个初始地图,因为它不能从第一帧的三角形。使用立体相机解决了这些问题,并提供一个更可靠的vSLAM解决方案。
这个例子展示了如何处理图像数据从一个立体相机来构建一个映射的户外环境,估计摄像机的轨迹。ORB-SLAM2的示例使用一个版本[1]算法,它是基于功能,支持立体相机。金宝app
处理管道的概述
立体的管道vSLAM非常类似于单眼vSLAM管道单眼视觉同步定位和映射的例子。的主要区别在于地图初始化阶段3 d地图点创建相同的一对立体影像立体而不是两个不同的图像帧。
地图初始化:管道从初始化开始的3 d分一对立体影像地图使用差异映射。左边的图像存储为第一个关键帧。
跟踪:一旦初始化地图,为每一个新的立体相机的姿势估计通过匹配特性在左边图像特征在过去的关键帧。估计相机姿势精制通过跟踪当地地图。
当地的地图:如果当前左图像被确定为一个关键帧,新的3 d地图点计算从立体的差距。在这个阶段,包调整用于最小化reprojection错误通过调整相机的姿势和3 d点。
循环关闭:为每个关键帧检测到循环通过比较它与以前的所有关键帧使用bag-of-features方法。一旦检测到循环闭合,构成图进行了优化完善相机带来的所有关键帧。
下载并探索立体图像序列的输入
在这个例子中是使用的数据UTIAS长期定位和数据集的映射由多伦多大学航空航天研究所提供。你可以下载数据到一个目录使用web浏览器或通过运行下面的代码:
filesep dataFolder = [fullfile (pwd),“stereoImageData”];zipFileName = [dataFolder filesep,“run_000005.zip”];folderExists =存在(dataFolder,“dir”);%在当前目录中创建一个文件夹来保存下载文件如果~ folderExists mkdir (dataFolder);disp (“下载run_000005。邮政编码(818 MB)。这个下载可以花几分钟”。)! wget ftp://asrl3.utias.utoronto.ca/2020-vtr-dataset/UTIAS-In-The-Dark/run_000005。zip - p。/ stereoImageData nv%提取下载的文件的内容disp (“提取run_000005。邮政编码(818 MB)……”)解压缩(zipFileName dataFolder);结束
使用两个imageDatastore
对象存储立体图像。
imgFolderLeft = [dataFolder,“/图片/左/”];imgFolderRight = [dataFolder,“/图片/正确/”];imdsLeft = imageDatastore (imgFolderLeft);imdsRight = imageDatastore (imgFolderRight);%检查第一双图像currFrameIdx = 1;currILeft = readimage (imdsLeft currFrameIdx);currIRight = readimage (imdsRight currFrameIdx);imshowpair (currILeft currIRight,“蒙太奇”);
地图初始化
ORB-SLAM管道从初始化开始,3 d世界地图点。这一步是至关重要的,最后一个大满贯的准确性产生重大影响的结果。最初的圆形特征点对应使用matchFeatures
两个一对立体的图像。配对应满足以下约束条件:
两者之间的横向转移相应特征点纠正立体图像小于最大的差距。您可以确定近似最大差异值的立体浮雕立体图像。有关更多信息,请参见选择范围的差异
两者之间的垂直变化相应特征点纠正立体图像小于一个阈值。
匹配的尺度特性是几乎相同的。
3 d世界位置对应匹配的特征点确定如下:
使用选择范围的差异计算每一对立体影像地图差距通过semi-global匹配(SGM)方法。
使用
reconstructScene
计算的三维世界坐标点差距地图。找到对应的位置差异映射特征点及其三维世界的位置。
%制定再现性随机种子rng (0);%加载初始相机的姿势。基于最初的相机的姿势是派生的%在相机和汽车之间的转换:% http://asrl.utias.utoronto.ca/datasets/2020-vtr-dataset/text_files/transform_camera_vehicle.txinitialPoseData =负载(“initialPose.mat”);initialPose = initialPoseData.initialPose;% reprojection矩阵构造3 d重建。%的intrinsic数据集可以在以下页面:% http://asrl.utias.utoronto.ca/datasets/2020-vtr-dataset/text_files/camera_parameters.txtfocalLength = (387.777 - 387.777);%中指定的像素principalPoint = (257.446 - 197.718);% (x, y)中指定的像素基线= 0.239965;%在米中指定图象尺寸大小= (currILeft [1, 2]);%在像素(mrows, ncols)intrinsic = cameraIntrinsics (focalLength principalPoint图象尺寸);reprojectionMatrix = (1,0,0, -principalPoint (1);0 1 0,-principalPoint (2);0,0,0,focalLength (1);0,0,1 /基线,0);%在这个例子中,图像已经不失真和纠正。一般工作流程,%取消注释以下代码undistort和纠正图像。% currILeft = undistortImage (currILeft intrinsic);% currIRight = undistortImage (currIRight intrinsic);% stereoParams = stereoParameters (intrinsic intrinsic,眼睛(3),[基线,0 0]);% (currILeft currIRight] = rectifyStereoImages (currILeft、currIRight stereoParams, OutputView =“完整的”);%的检测和提取ORB功能纠正立体图像scaleFactor = 1.2;numLevels = 8;[currFeaturesLeft, currPointsLeft] = helperDetectAndExtractFeatures (im2gray (currILeft) scaleFactor, numLevels);[currFeaturesRight, currPointsRight] = helperDetectAndExtractFeatures (im2gray (currIRight) scaleFactor, numLevels);%匹配特征点之间的立体图像和三维世界的位置disparityRange = [0 48];%中指定的像素[xyzPoints, matchedPairs] = helperReconstructFromStereo (im2gray (currILeft) im2gray (currIRight),…currFeaturesLeft、currFeaturesRight currPointsLeft、currPointsRight reprojectionMatrix, initialPose, disparityRange);
数据管理和可视化
地图初始化后使用第一个立体模型,您可以使用imageviewset
和
worldpointset
存储第一个关键帧和相应的地图点:
%创建一个空imageviewset对象存储关键帧vSetKeyFrames = imageviewset;%创建一个空worldpointset对象来存储3 d地图点mapPointSet = worldpointset;%添加第一个关键帧currKeyFrameId = 1;vSetKeyFrames = addView (vSetKeyFrames currKeyFrameId initialPose,分= currPointsLeft,…特点= currFeaturesLeft.Features);%添加3 d地图点[mapPointSet, stereoMapPointsIdx] = addWorldPoints (mapPointSet xyzPoints);%添加观察地图上的点mapPointSet = addCorrespondences (mapPointSet currKeyFrameId、stereoMapPointsIdx matchedPairs (: 1));%更新视图方向和深度mapPointSet = updateLimitsAndDirection (mapPointSet stereoMapPointsIdx vSetKeyFrames.Views);%更新代表视图mapPointSet = updateRepresentativeView (mapPointSet stereoMapPointsIdx vSetKeyFrames.Views);在第一个关键帧%可视化匹配特性featurePlot = helperVisualizeMatchedFeaturesStereo (currILeft currIRight currPointsLeft,…currPointsRight matchedPairs);%可视化地图初始点和相机轨迹mapPlot = helperVisualizeMotionAndStructureStereo (vSetKeyFrames mapPointSet);%显示传奇showLegend (mapPlot);
初始化位置识别数据库
循环使用bags-of-words方法执行检测。视觉词汇表示为bagOfFeatures
对象创建脱机使用ORB描述符提取大量的图像数据集通过调用:
袋= bagOfFeatures (imd, CustomExtractor = @helperORBFeatureExtractorFunction TreeProperties = [3, 10], StrongestFeatures = 1);
在哪里洛桑国际管理发展学院
是一个imageDatastore
对象存储图像和训练helperORBFeatureExtractorFunction
ORB功能器功能。看到图像检索与袋的视觉语言为更多的信息。
关闭循环过程逐步构建一个数据库,表示为一个invertedImageIndex
对象,存储视觉word-to-image映射基于ORB的包功能。
%负荷特性数据的包创建离线bofData =负载(“bagOfFeaturesDataSLAM.mat”);%初始化位置识别数据库loopDatabase = invertedImageIndex (bofData.bof SaveFeatureLocations = false);%添加特性的第一个关键帧到数据库addImageFeatures (loopDatabase currFeaturesLeft currKeyFrameId);
跟踪
跟踪执行过程使用每一对和决定何时插入一个新的关键帧。
% ViewId最后的关键帧lastKeyFrameId = currKeyFrameId;%的索引输入图像序列中的最后一个关键帧lastKeyFrameIdx = currFrameIdx;%指数的关键帧输入图像序列addedFramesIdx = lastKeyFrameIdx;currFrameIdx = 2;isLoopClosed = false;
每一帧处理如下:
ORB功能是为每个新立体图像,然后提取匹配(使用
matchFeatures
),在最后关键帧特性已知相应的3 d地图点。估计摄像机构成Perspective-n-Point算法使用
estworldpose
。
考虑到相机的姿势,项目地图点观察到最后关键帧到当前帧和通讯使用搜索功能matchFeaturesInRadius
。
在当前帧与三维二维通讯,细化相机姿势通过执行motion-only束调整使用
bundleAdjustmentMotion
。项目当地的点映射到当前帧搜索通讯使用更多的功能
matchFeaturesInRadius
和完善摄像机构成再次使用bundleAdjustmentMotion
。跟踪的最后一步是决定如果当前框架应该是一个新的关键帧。一个框架是一个关键帧如果满足下面两个条件:
至少5帧已经过去了自从上次关键帧和当前帧跟踪不到80点地图。
当前帧的映射点追踪不到90%的点追踪的参考关键帧。
如果当前帧成为一个关键帧,继续的当地的地图的过程。否则,开始跟踪为下一个框架。
%主循环isLastFrameKeyFrame = true;而~ isLoopClosed & & currFrameIdx < =元素个数(imdsLeft.Files) currILeft = readimage (imdsLeft currFrameIdx);currIRight = readimage (imdsRight currFrameIdx);currILeftGray = im2gray (currILeft);currIRightGray = im2gray (currIRight);[currFeaturesLeft, currPointsLeft] = helperDetectAndExtractFeatures (currILeftGray、scaleFactor numLevels);[currFeaturesRight, currPointsRight] = helperDetectAndExtractFeatures (currIRightGray、scaleFactor numLevels);%跟踪最后关键帧% trackedMapPointsIdx:指数映射点观察到在当前帧% trackedFeatureIdx:指数相应的特征点在当前帧[currPose, trackedMapPointsIdx trackedFeatureIdx] = helperTrackLastKeyFrame (mapPointSet,…vSetKeyFrames。视图、currFeaturesLeft currPointsLeft, lastKeyFrameId, intrinsic scaleFactor);如果isempty (currPose) | |元素个数(trackedMapPointsIdx) < 30 currFrameIdx = currFrameIdx + 1;继续结束%跟踪当地地图和检查当前帧是一个关键帧。% localKeyFrameIds: ViewId连接的当前帧的关键帧numSkipFrames = 5;numPointsKeyFrame = 80;[localKeyFrameIds, currPose trackedMapPointsIdx、trackedFeatureIdx isKeyFrame] =…helperTrackLocalMap (mapPointSet vSetKeyFrames trackedMapPointsIdx,…trackedFeatureIdx, currPose、currFeaturesLeft currPointsLeft, intrinsic scaleFactor, numLevels,…isLastFrameKeyFrame、lastKeyFrameIdx currFrameIdx、numSkipFrames numPointsKeyFrame);%匹配特征点之间的立体图像和三维世界的位置[xyzPoints, matchedPairs] = helperReconstructFromStereo (currILeftGray、currIRightGray currFeaturesLeft,…currFeaturesRight、currPointsLeft currPointsRight、reprojectionMatrix currPose, disparityRange);%可视化匹配特性的立体形象updatePlot (featurePlot、currILeft currIRight、currPointsLeft currPointsRight, trackedFeatureIdx, matchedPairs);如果~ isKeyFrame & & currFrameIdx <元素个数(imdsLeft.Files) currFrameIdx = currFrameIdx + 1;isLastFrameKeyFrame = false;继续其他的[untrackedFeatureIdx, ia] = setdiff (matchedPairs (: 1), trackedFeatureIdx);:xyzPoints = xyzPoints (ia);isLastFrameKeyFrame = true;结束%更新当前关键帧IDcurrKeyFrameId = currKeyFrameId + 1;
当地的地图
本地映射执行每一个关键帧。当一个新的确定关键帧,将其添加到关键帧和更新地图的属性点观察到新的关键帧。以确保mapPointSet
包含尽可能少的离群值,一个有效的地图点必须在至少3关键帧。
新地图点是由呈三角形ORB在当前关键帧特征点及其连接的关键帧。对于每一个无与伦比的特征点在当前关键帧,寻找匹配与其他无与伦比的点连接关键帧的使用matchFeatures
。当地包调整改进当前的关键帧的构成,提出了连接的关键帧,所有地图点观察到这些关键帧。
%添加新的关键帧[mapPointSet, vSetKeyFrames] = helperAddNewKeyFrame (mapPointSet vSetKeyFrames,…currPose、currFeaturesLeft currPointsLeft、trackedMapPointsIdx trackedFeatureIdx, localKeyFrameIds);%去除离群值映射点,观察不到3关键帧如果currKeyFrameId = = 2 triangulatedMapPointsIdx = [];结束mapPointSet = helperCullRecentMapPoints (mapPointSet,…trackedMapPointsIdx、triangulatedMapPointsIdx stereoMapPointsIdx);从差异%添加新地图点计算[mapPointSet, stereoMapPointsIdx] = addWorldPoints (mapPointSet xyzPoints);mapPointSet = addCorrespondences (mapPointSet currKeyFrameId stereoMapPointsIdx,…untrackedFeatureIdx);%由三角创建新地图点minNumMatches = 20;minParallax = 0.35;[mapPointSet, vSetKeyFrames triangulatedMapPointsIdx stereoMapPointsIdx] = helperCreateNewMapPointsStereo (…mapPointSet、vSetKeyFrames currKeyFrameId, intrinsic、scaleFactor minNumMatches, minParallax,…untrackedFeatureIdx stereoMapPointsIdx);%当地束调整[refinedViews dist] = connectedViews (vSetKeyFrames, currKeyFrameId MaxDistance = 2);refinedKeyFrameIds = refinedViews.ViewId;%总是修复前两个关键帧fixedViewIds = refinedKeyFrameIds (dist = = 2);fixedViewIds = fixedViewIds (1: min(10,元素个数(fixedViewIds)));%改进当地关键帧和地图点[mapPointSet, vSetKeyFrames mapPointIdx] = bundleAdjustment (…mapPointSet vSetKeyFrames, [refinedKeyFrameIds;intrinsic currKeyFrameId),…FixedViewIDs = FixedViewIDs PointsUndistorted = true, AbsoluteTolerance = 1 e -,…RelativeTolerance = 1 e-16,解算器=“preconditioned-conjugate-gradient”,MaxIteration = 10);%更新视图方向和深度mapPointSet = updateLimitsAndDirection (mapPointSet mapPointIdx,…vSetKeyFrames.Views);%更新代表视图mapPointSet = updateRepresentativeView (mapPointSet mapPointIdx,…vSetKeyFrames.Views);%点和相机轨迹可视化三维世界updatePlot (mapPlot vSetKeyFrames mapPointSet);
循环关闭
循环步骤需要关闭当前关键帧处理当地的映射过程,并试图发现并关闭循环。循环候选人确定通过查询数据库中的图像,视觉上类似于当前使用关键帧evaluateImageRetrieval
。候选关键帧是有效的,如果它不是连接到邻国的最后关键帧和三个关键帧循环的候选人。
当一个有效的循环关闭找到候选人,计算循环关闭候选帧之间的相对姿态和当前使用关键帧estgeotform3d
。然后添加循环与相对姿态和更新mapPointSet
和vSetKeyFrames
。
%检查循环创建了一些关键帧后关闭如果currKeyFrameId > 50%的最小数量的特征匹配循环边缘loopEdgeNumMatches = 50;%检测可能的循环关闭关键帧的候选人[isDetected, validLoopCandidates] = helperCheckLoopClosure (vSetKeyFrames currKeyFrameId,…loopDatabase、currILeft loopEdgeNumMatches);isTooCloseView = currKeyFrameId - max (validLoopCandidates) < 100;如果isDetected & & ~ isTooCloseView%添加循环关闭连接[isLoopClosed, mapPointSet vSetKeyFrames] = helperAddLoopConnectionsStereo (…mapPointSet、vSetKeyFrames validLoopCandidates currKeyFrameId,…currFeaturesLeft、currPointsLeft loopEdgeNumMatches);结束结束%如果没有检测到,关闭循环电流特性添加到数据库中如果~ isLoopClosed addImageFeatures (loopDatabase currFeaturesLeft currKeyFrameId);结束%更新id和指数lastKeyFrameId = currKeyFrameId;lastKeyFrameIdx = currFrameIdx;addedFramesIdx = [addedFramesIdx;currFrameIdx];currFrameIdx = currFrameIdx + 1;结束%主循环的结束
循环之间的边缘添加关键帧:2和285
最后,应用基本图形构成图优化vSetKeyFrames
正确的漂移。基本图形创建内部通过删除联系不到minNumMatches
covisibility图匹配。构成图优化后,更新地图的三维位置点使用优化的姿势。
%优化提出了vSetKeyFramesOptim = optimizePoses (vSetKeyFrames minNumMatches,公差= 1 e-16);%更新地图点优化后的姿势mapPointSet = helperUpdateGlobalMap (mapPointSet vSetKeyFrames vSetKeyFramesOptim);updatePlot (mapPlot vSetKeyFrames mapPointSet);%画出优化的相机轨迹optimizedPoses =姿势(vSetKeyFramesOptim);plotOptimizedTrajectory (mapPlot optimizedPoses)%更新传奇showLegend (mapPlot);
比较与地面真理
你可以比较优化的相机轨迹与地面真理评估解决方案的准确性。包含一个下载的数据gps.txt
文件存储每一帧的GPS位置。你可以把GPS定位地理当地笛卡尔坐标使用latlon2local
(自动驾驶工具箱)从自动驾驶工具箱或geodetic2enu
(映射工具箱)从映射工具。在本例中,您可以简单地加载GPS数据转换从一个m文件。
%的GPS数据加载gpsData =负载(“gpsLocation.mat”);gpsLocation = gpsData.gpsLocation;%转换GPS位置参考坐标系gTruth = helperTransformGPSLocations (gpsLocation optimizedPoses);%画出GPS的位置plotActualTrajectory (mapPlot gTruth (addedFramesIdx:));%显示传奇showLegend (mapPlot);
从立体影像密度重建
鉴于精制相机的姿势,可以执行密集的重建与关键帧相对应的立体影像。
% pointCloud对象创建一个数组来存储3 d世界点%与关键帧相关联ptClouds = repmat (pointCloud(0(1、3)),元素个数(addedFramesIdx), 1);为i = 1:元素个数(addedFramesIdx) ILeft = readimage (imdsLeft, addedFramesIdx(我));IRight = readimage (imdsRight addedFramesIdx(我));%重建场景的差距disparityMap = disparitySGM (im2gray (ILeft) im2gray (IRight) DisparityRange = DisparityRange);xyzPoints = reconstructScene (disparityMap reprojectionMatrix);%忽略图像的上半部分,主要显示了天空xyzPoints(1:地板(图象尺寸(1)/ 2),:,:)=南;%忽略下方的图像显示xyzPoints(图象尺寸(1)-50:,:,:)=南;xyzPoints =重塑(xyzPoints,图象尺寸(1)*图象尺寸(2),3));%获得彩色图像的颜色颜色=重塑(ILeft,图象尺寸(1)*图象尺寸(2),3));%去除世界点太远离相机validIndex = xyzPoints (: 3) > 0 & xyzPoints (:, 3) < 100 / reprojectionMatrix (4,3);xyzPoints = xyzPoints (validIndex:);颜色=颜色(validIndex:);%变换指出世界坐标currPose = optimizedPoses.AbsolutePose(我);xyzPoints = transformPointsForward (currPose xyzPoints);ptCloud = pointCloud (xyzPoints颜色=颜色);% Downsample点云ptClouds (i) = pcdownsample (ptCloud,随机= 0.5);结束%连接的点云pointCloudsAll = pccat (ptClouds);%可视化点云图ax = pcshow (pointCloudsAll VerticalAxis =“y”VerticalAxisDir =“向下”);包含(“X”)ylabel (“Y”)zlabel (“Z”)标题(“密集的重建”)%显示场景的鸟瞰视图(ax, [0 0 1);camroll (ax, 90);
金宝app支持功能
下面列出了辅助函数。更大的功能都包含在单独的文件中。
helperDetectAndExtractFeatures
检测并从图像中提取和ORB功能。
函数(特性,validPoints) = helperDetectAndExtractFeatures (Igray、scaleFactor numLevels) numPoints = 600;%检测ORB功能点= detectORBFeatures (Igray ScaleFactor = ScaleFactor NumLevels = NumLevels);%选择特性的一个子集,均匀分布在整个图像点= selectUniform(点、numPoints大小(Igray 1:2));%提取特征[特性,validPoints] = extractFeatures (Igray点);结束
helperReconstructFromStereo
使用差异地图重建场景的立体图像
函数[xyzPoints, indexPairs] = helperReconstructFromStereo (I1、I2…里,features1, features2 points2 reprojectionMatrix, currPose, disparityRange) indexPairs = helperFindValidFeaturePairs (features1 features2,里,points2, disparityRange);%计算差异左边图像的所有像素。在实践中,它是更多%计算常见的差距只是为了匹配的特征点。disparityMap = disparitySGM (I1、I2 DisparityRange = DisparityRange);xyzPointsAll = reconstructScene (disparityMap reprojectionMatrix);%找到对应点匹配的特征点的世界位置=地板(里。位置(indexPairs (: 1), (2 - 1)));xyzPoints = [];isPointFound = false(大小(里));为i = 1:尺寸(位置1)point3d =挤压(xyzPointsAll(位置(我,1),地点(我,2):))';isPointValid = point3d (3) > 0 & point3d (3) < 200 / reprojectionMatrix (4,3);如果isPointValid xyzPoints = [xyzPoints;point3d];% #好< * AGROW >isPointFound (i) = true;结束结束indexPairs = indexPairs (isPointFound:);xyzPoints = xyzPoints * currPose。旋转+ currPose.Translation;结束
helperFindValidFeaturePairs
一对立体图像之间的匹配特性
函数里,indexPairs = helperFindValidFeaturePairs (features1 features2 points2, disparityRange) indexPairs = matchFeatures (features1 features2,…独特的= true, MaxRatio = 1, MatchThreshold = 40);matchedPoints1 = points1.Location (indexPairs (: 1):);matchedPoints2 = points2.Location (indexPairs (:, 2):);scales1 = points1.Scale (indexPairs (: 1):);scales2 = points2.Scale (indexPairs (:, 2):);dist2EpipolarLine = abs (matchedPoints2 (:, 2)——matchedPoints1 (:, 2));shiftDist = matchedPoints1 (: 1)——matchedPoints2 (: 1);isCloseToEpipolarline = dist2EpipolarLine < 2 * scales2;isDisparityValid = shiftDist > 0 & shiftDist < disparityRange (2);isScaleIdentical = scales1 = = scales2; indexPairs = indexPairs(isCloseToEpipolarline & isDisparityValid & isScaleIdentical, :);结束
helperCullRecentMapPoints
剔除最近添加地图点。
函数mapPointSet = helperCullRecentMapPoints (mapPointSet mapPointsIdx、newPointIdx stereoMapPointsIndices) outlierIdx = setdiff ([newPointIdx;stereoMapPointsIndices], mapPointsIdx);如果~ isempty (outlierIdx) mapPointSet = removeWorldPoints (mapPointSet outlierIdx);结束结束
helperUpdateGlobalMap
更新三维地图的位置后点构成图优化
函数mapPointSet = helperUpdateGlobalMap (mapPointSet vSetKeyFrames vSetKeyFramesOptim) posesOld = vSetKeyFrames.Views.AbsolutePose;一直陷于= vSetKeyFramesOptim.Views.AbsolutePose;positionsOld = mapPointSet.WorldPoints;positionsNew = positionsOld;指数= 1:mapPointSet.Count;%更新世界每个映射点的位置根据新的绝对的姿势%的主要观点为我= 1:mapPointSet。计数majorViewIds = mapPointSet.RepresentativeViewId(我);tform = rigidtform3d(一直陷于(majorViewIds)。/ posesOld (majorViewIds)。);positionsNew(我)= transformPointsForward (tform, positionsOld(我,:));结束mapPointSet = updateWorldPoints (mapPointSet,指标,positionsNew);结束
helperTransformGPSLocations
变换的GPS位置参考坐标系
函数gTruth = helperTransformGPSLocations (gpsLocations optimizedPoses) initialYawGPS =每股((gpsLocations (100 2) - gpsLocations (1、2)) /…(gpsLocations (100,1) - gpsLocations (1,1)));initialYawSLAM = ((optimizedPoses.AbsolutePose每股(50).Translation (2) -…optimizedPoses.AbsolutePose (1) .Translation) / (2)…(optimizedPoses.AbsolutePose (59) .Translation (1) -…optimizedPoses.AbsolutePose (1) .Translation (1)));relYaw = initialYawGPS - initialYawSLAM;relTranslation = optimizedPoses.AbsolutePose (1) .Translation;initialTform = rotationVectorToMatrix ([0 0 relYaw]);为i = 1:尺寸(gpsLocations, 1) gTruth(我:)= initialTform * gpsLocations我:' + relTranslation ';结束结束
引用
[1]Mur-Artal,劳尔和胡安·d·缓慢的。“ORB-SLAM2:单眼的开源大满贯系统,音响,和RGB-D相机。”IEEE机器人33岁的没有。5 (2017):1255 - 1262。