立体视觉同步定位和映射

视觉同时定位和地图(vSLAM),指的是计算摄像机的位置和姿态的过程对周围环境,同时环境的映射。这个过程只使用视觉输入的相机。申请vSLAM包括增强现实、机器人、自动驾驶。

vSLAM可以由使用单眼相机。然而,由于深度不能准确计算使用一个相机,地图和的规模估计未知和漂移轨迹。此外,引导系统,多个视图需要产生一个初始地图,因为它不能从第一帧的三角形。使用立体相机解决了这些问题,并提供一个更可靠的vSLAM解决方案。

这个例子展示了如何处理图像数据从一个立体相机来构建一个映射的户外环境,估计摄像机的轨迹。ORB-SLAM2的示例使用一个版本[1]算法,它是基于功能,支持立体相机。金宝app

处理管道的概述

立体的管道vSLAM非常类似于单眼vSLAM管道单眼视觉同步定位和映射的例子。的主要区别在于地图初始化阶段3 d地图点创建相同的一对立体影像立体而不是两个不同的图像帧。

地图初始化:管道从初始化开始的3 d分一对立体影像地图使用差异映射。左边的图像存储为第一个关键帧。
跟踪:一旦初始化地图,为每一个新的立体相机的姿势估计通过匹配特性在左边图像特征在过去的关键帧。估计相机姿势精制通过跟踪当地地图。
当地的地图:如果当前左图像被确定为一个关键帧,新的3 d地图点计算从立体的差距。在这个阶段,包调整用于最小化reprojection错误通过调整相机的姿势和3 d点。
循环关闭:为每个关键帧检测到循环通过比较它与以前的所有关键帧使用bag-of-features方法。一旦检测到循环闭合,构成图进行了优化完善相机带来的所有关键帧。

下载并探索立体图像序列的输入

在这个例子中是使用的数据UTIAS长期定位和数据集的映射由多伦多大学航空航天研究所提供。你可以下载数据到一个临时目录中使用一个web浏览器或通过运行下面的代码:

ftpObj = ftp (“asrl3.utias.utoronto.ca”);tempFolder = fullfile (tempdir);dataFolder = [tempFolder,“2020 -录像机集/ UTIAS-In-The-Dark /”];zipFileName = [dataFolder,“run_000005.zip”];folderExists =存在(dataFolder,“dir”);%在一个临时目录中创建一个文件夹来保存下载文件如果~ folderExists mkdir (dataFolder);disp (“下载run_000005。邮政编码(818 MB)。这个下载可以花几分钟”。)mget (ftpObj' / 2020 -录像机集/ UTIAS-In-The-Dark run_000005.zip”,tempFolder);%提取下载的文件的内容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世界位置对应匹配的特征点确定如下:

使用disparitySGM计算每一对立体影像地图差距通过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;%创建stereoParameters对象存储立体相机参数。%的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;%在米中指定intrinsicMatrix = [focalLength (1), 0, 0;…0 0,focalLength (2);…principalPoint principalPoint (1) (2), 1);图象尺寸大小= (currILeft [1, 2]);%在像素(mrows, ncols)cameraParam = cameraParameters (“IntrinsicMatrix”intrinsicMatrix,“图象尺寸”、图象尺寸);intrinsic = cameraParam.Intrinsics;stereoParams = stereoParameters (cameraParam cameraParam,眼睛(3),[基线,0 0]);%在这个例子中,图像已经不失真。在一般%的工作流程,取消以下代码undistort图像。% currILeft = undistortImage (currILeft intrinsic);% currIRight = undistortImage (currIRight intrinsic);%整顿立体影像[currILeft, currIRight] = rectifyStereoImages (currILeft、currIRight stereoParams,“OutputView”,“全部”);%的检测和提取ORB功能纠正立体图像scaleFactor = 1.2;numLevels = 8;[currFeaturesLeft, currPointsLeft] = helperDetectAndExtractFeatures (currILeft、scaleFactor numLevels);[currFeaturesRight, currPointsRight] = helperDetectAndExtractFeatures (currIRight、scaleFactor numLevels);%匹配特征点之间的立体图像和三维世界的位置maxDisparity = 48;%中指定的像素[xyzPoints, matchedPairs] = helperReconstructFromStereo (currILeft currIRight,…currFeaturesLeft、currFeaturesRight currPointsLeft、currPointsRight stereoParams, initialPose, maxDisparity);

数据管理和可视化

地图初始化后使用第一个立体模型,您可以使用imageviewset,worldpointset和helperViewDirectionAndDepth存储第一个关键帧和相应的地图点:

%创建一个空imageviewset对象存储关键帧vSetKeyFrames = imageviewset;%创建一个空worldpointset对象来存储3 d地图点mapPointSet = worldpointset;%创建helperViewDirectionAndDepth对象来存储视图方向和深度directionAndDepth = helperViewDirectionAndDepth(大小(xyzPoints, 1));%添加第一个关键帧currKeyFrameId = 1;vSetKeyFrames = addView (vSetKeyFrames currKeyFrameId initialPose,“点”currPointsLeft,…“特性”,currFeaturesLeft.Features);%添加3 d地图点[mapPointSet, stereoMapPointsIdx] = addWorldPoints (mapPointSet xyzPoints);%添加观察地图上的点mapPointSet = addCorrespondences (mapPointSet currKeyFrameId、stereoMapPointsIdx matchedPairs (: 1));在第一个关键帧%可视化匹配特性featurePlot = helperVisualizeMatchedFeaturesStereo (currILeft currIRight currPointsLeft,…currPointsRight matchedPairs);%可视化地图初始点和相机轨迹mapPlot = helperVisualizeMotionAndStructureStereo (vSetKeyFrames mapPointSet);%显示传奇showLegend (mapPlot);

初始化位置识别数据库

循环使用bags-of-words方法执行检测。视觉词汇表示为bagOfFeatures对象创建脱机使用ORB描述符提取大量的图像数据集通过调用:

袋= bagOfFeatures (@helperORBFeatureExtractorFunction imd,‘CustomExtractor’,‘TreeProperties’, (2, 20),“StrongestFeatures”, 1);

在哪里洛桑国际管理发展学院是一个imageDatastore对象存储图像和训练helperORBFeatureExtractorFunctionORB功能器功能。看到图像检索与袋的视觉语言为更多的信息。

关闭循环过程逐步构建一个数据库,表示为一个invertedImageIndex对象,存储视觉word-to-image映射基于ORB的包功能。

%负荷特性数据的包创建离线bofData =负载(“bagOfFeaturesUTIAS.mat”);%初始化位置识别数据库loopDatabase = invertedImageIndex (bofData.bof,“SaveFeatureLocations”、假);%添加特性的第一个关键帧到数据库addImageFeatures (loopDatabase currFeaturesLeft currKeyFrameId);

跟踪

跟踪执行过程使用每一对和决定何时插入一个新的关键帧。

% ViewId最后的关键帧lastKeyFrameId = currKeyFrameId;%的ViewId最co-visible参考关键帧%地图点与当前关键帧refKeyFrameId = currKeyFrameId;%的索引输入图像序列中的最后一个关键帧lastKeyFrameIdx = currFrameIdx;%指数的关键帧输入图像序列addedFramesIdx = lastKeyFrameIdx;currFrameIdx = 2;isLoopClosed = false;

每一帧处理如下:

ORB功能是为每个新立体图像,然后提取匹配(使用matchFeatures),在最后关键帧特性已知相应的3 d地图点。
估计摄像机构成Perspective-n-Point算法使用estimateWorldCameraPose。
考虑到相机的姿势,项目地图点观察到最后关键帧到当前帧和通讯使用搜索功能matchFeaturesInRadius。
在当前帧与三维二维通讯,细化相机姿势通过执行motion-only束调整使用bundleAdjustmentMotion。
项目当地的点映射到当前帧搜索通讯使用更多的功能matchFeaturesInRadius和完善摄像机构成再次使用bundleAdjustmentMotion。
跟踪的最后一步是决定如果当前框架应该是一个新的关键帧。一个框架是一个关键帧如果满足下面两个条件:

至少5帧已经过去了自从上次关键帧和当前帧跟踪不到100点地图。
当前帧的映射点追踪不到90%的点追踪的参考关键帧。

如果当前帧成为一个关键帧,继续的当地的地图的过程。否则,开始跟踪为下一个框架。

%主循环而~ isLoopClosed & & currFrameIdx < =元素个数(imdsLeft.Files) currILeft = readimage (imdsLeft currFrameIdx);currIRight = readimage (imdsRight currFrameIdx);[currILeft, currIRight] = rectifyStereoImages (currILeft、currIRight stereoParams,“OutputView”,“全部”);[currFeaturesLeft, currPointsLeft] = helperDetectAndExtractFeatures (currILeft、scaleFactor numLevels);[currFeaturesRight, currPointsRight] = helperDetectAndExtractFeatures (currIRight、scaleFactor numLevels);%跟踪最后关键帧% trackedMapPointsIdx:指数映射点观察到在当前帧% trackedFeatureIdx:指数相应的特征点在当前帧[currPose, trackedMapPointsIdx trackedFeatureIdx] = helperTrackLastKeyFrame (mapPointSet,…vSetKeyFrames。视图、currFeaturesLeft currPointsLeft, lastKeyFrameId, intrinsic scaleFactor);如果isempty (currPose) | |元素个数(trackedMapPointsIdx) < 30 currFrameIdx = currFrameIdx + 1;继续结束%跟踪当地地图% refKeyFrameId: ViewId参考最关键帧% co-visible地图点与当前帧% localKeyFrameIds: ViewId连接的当前帧的关键帧如果currKeyFrameId = = 1 refKeyFrameId = 1;localKeyFrameIds = 1;其他的[refKeyFrameId, localKeyFrameIds currPose、trackedMapPointsIdx trackedFeatureIdx] =…helperTrackLocalMap (mapPointSet directionAndDepth、vSetKeyFrames trackedMapPointsIdx,…trackedFeatureIdx, currPose、currFeaturesLeft currPointsLeft, intrinsic scaleFactor, numLevels);结束%匹配特征点之间的立体图像和三维世界的位置[xyzPoints, matchedPairs] = helperReconstructFromStereo (currILeft、currIRight currFeaturesLeft,…currFeaturesRight、currPointsLeft currPointsRight、stereoParams currPose, maxDisparity);[untrackedFeatureIdx, ia] = setdiff (matchedPairs (: 1), trackedFeatureIdx);:xyzPoints = xyzPoints (ia);%检查当前帧是一个关键帧isKeyFrame = helperIsKeyFrame (mapPointSet refKeyFrameId lastKeyFrameIdx,…currFrameIdx trackedMapPointsIdx);%可视化匹配特性的立体形象updatePlot (featurePlot、currILeft currIRight、currPointsLeft currPointsRight, trackedFeatureIdx, matchedPairs);如果~ isKeyFrame & & currFrameIdx <元素个数(imdsLeft.Files) currFrameIdx = currFrameIdx + 1;继续结束%更新当前关键帧IDcurrKeyFrameId = currKeyFrameId + 1;

当地的地图

本地映射执行每一个关键帧。当一个新的确定关键帧,将其添加到关键帧和更新地图的属性点观察到新的关键帧。以确保mapPointSet包含尽可能少的离群值,一个有效的地图点必须在至少3关键帧。

新地图点是由呈三角形ORB在当前关键帧特征点及其连接的关键帧。对于每一个无与伦比的特征点在当前关键帧,寻找匹配与其他无与伦比的点连接关键帧的使用matchFeatures。当地包调整改进当前的关键帧的构成,提出了连接的关键帧,所有地图点观察到这些关键帧。

%添加新的关键帧[mapPointSet, vSetKeyFrames] = helperAddNewKeyFrame (mapPointSet vSetKeyFrames,…currPose、currFeaturesLeft currPointsLeft、trackedMapPointsIdx trackedFeatureIdx, localKeyFrameIds);%去除离群值映射点,观察不到3关键帧如果currKeyFrameId = = 2 triangulatedMapPointsIdx = [];结束[mapPointSet, directionAndDepth trackedMapPointsIdx] =…helperCullRecentMapPoints (mapPointSet directionAndDepth、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);%更新视图方向和深度directionAndDepth =更新(directionAndDepth mapPointSet vSetKeyFrames.Views,…[trackedMapPointsIdx;triangulatedMapPointsIdx;stereoMapPointsIdx),真正的);%当地束调整[mapPointSet, directionAndDepth vSetKeyFrames、triangulatedMapPointsIdx stereoMapPointsIdx] =…helperLocalBundleAdjustmentStereo (mapPointSet directionAndDepth vSetKeyFrames,…currKeyFrameId, intrinsic triangulatedMapPointsIdx stereoMapPointsIdx);%点和相机轨迹可视化三维世界updatePlot (mapPlot vSetKeyFrames mapPointSet);

循环关闭

循环步骤需要关闭当前关键帧处理当地的映射过程,并试图发现并关闭循环。循环候选人确定通过查询数据库中的图像,视觉上类似于当前使用关键帧evaluateImageRetrieval。候选关键帧是有效的,如果它不是连接到邻国的最后关键帧和三个关键帧循环的候选人。

当一个有效的循环关闭找到候选人,计算循环关闭候选帧之间的相对姿态和当前使用关键帧estimateGeometricTransform3D。然后添加循环与相对姿态和更新mapPointSet和vSetKeyFrames。

%检查循环创建了一些关键帧后关闭如果currKeyFrameId > 50%的最小数量的特征匹配循环边缘loopEdgeNumMatches = 45;%检测可能的循环关闭关键帧的候选人[isDetected, validLoopCandidates] = helperCheckLoopClosure (vSetKeyFrames currKeyFrameId,…loopDatabase、currILeft loopEdgeNumMatches);isTooCloseView = currKeyFrameId - max (validLoopCandidates) < 100;如果isDetected & & ~ isTooCloseView%添加循环关闭连接[isLoopClosed, mapPointSet vSetKeyFrames] = helperAddLoopConnectionsStereo (…mapPointSet、vSetKeyFrames validLoopCandidates currKeyFrameId,…currFeaturesLeft loopEdgeNumMatches);结束结束%如果没有检测到,关闭循环电流特性添加到数据库中如果~ isLoopClosed addImageFeatures (loopDatabase currFeaturesLeft currKeyFrameId);结束%更新id和指数lastKeyFrameId = currKeyFrameId;lastKeyFrameIdx = currFrameIdx;addedFramesIdx = [addedFramesIdx;currFrameIdx];currFrameIdx = currFrameIdx + 1;结束%主循环的结束

循环之间的边缘添加关键帧:1 282循环之间的边缘添加关键帧:4和282循环之间的边缘添加关键帧:2 - 282

最后,应用基本图形构成图优化vSetKeyFrames正确的漂移。基本图形创建内部通过删除联系不到minNumMatchescovisibility图匹配。构成图优化后,更新地图的三维位置点使用优化的姿势。

%优化提出了minNumMatches = 30;vSetKeyFramesOptim = optimizePoses (vSetKeyFrames minNumMatches,“宽容”1 e-16);%更新地图点优化后的姿势mapPointSet = helperUpdateGlobalMap (mapPointSet directionAndDepth、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);

金宝app支持功能

下面列出了辅助函数。更大的功能都包含在单独的文件中。

helperDetectAndExtractFeatures检测并从图像中提取和ORB功能。

函数(特性,validPoints) = helperDetectAndExtractFeatures (Irgb、scaleFactor numLevels) numPoints = 800;%检测ORB功能Igray = im2gray (Irgb);点= detectORBFeatures (Igray,“ScaleFactor”scaleFactor,“NumLevels”,numLevels);%选择特性的一个子集,均匀分布在整个图像点= selectUniform(点、numPoints大小(Igray 1:2));%提取特征[特性,validPoints] = extractFeatures (Igray点);结束

helperReconstructFromStereo使用差异地图重建场景的立体图像

函数[xyzPoints, indexPairs] = helperReconstructFromStereo (I1、I2…里,features1, features2 points2 stereoParams, currPose, maxDisparity) indexPairs = helperFindValidFeaturePairs (features1 features2,里,points2, maxDisparity);%计算差异左边图像的所有像素。在实践中,它是更多%计算常见的差距只是为了匹配的特征点。disparityMap = disparitySGM (rgb2gray (I1) rgb2gray (I2),“DisparityRange”[0,maxDisparity]);xyzPointsAll = reconstructScene (disparityMap stereoParams);%找到对应点匹配的特征点的世界位置=地板(里。位置(indexPairs (: 1), (2 - 1)));xyzPoints = [];isPointFound = false(大小(里));为i = 1:尺寸(位置1)point3d =挤压(xyzPointsAll(位置(我,1),地点(我,2):))';所有(~ isnan isPointValid = (point3d) & &所有(isfinite (point3d)) & & point3d (3) > 0;isDepthInRange = point3d(3) < 200 *规范(stereoParams.TranslationOfCamera2);如果isPointValid & & isDepthInRange xyzPoints = [xyzPoints;point3d];% #好< * AGROW >isPointFound (i) = true;结束结束indexPairs = indexPairs (isPointFound:);xyzPoints = xyzPoints * currPose。旋转+ currPose.Translation;结束

helperFindValidFeaturePairs一对立体图像之间的匹配特性

函数里,indexPairs = helperFindValidFeaturePairs (features1 features2 points2, maxDisparity) indexPairs = matchFeatures (features1 features2,…“独特的”,真的,“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 < maxDisparity;isScaleIdentical = scales1 = = scales2; indexPairs = indexPairs(isCloseToEpipolarline & isDisparityValid & isScaleIdentical, :);结束

helperIsKeyFrame检查是否一个框架是一个关键帧。

函数isKeyFrame = helperIsKeyFrame (mappoint,…refKeyFrameId、lastKeyFrameIndex currFrameIndex mapPointsIndices) numPointsRefKeyFrame =元素个数(findWorldPointsInView (mappoint refKeyFrameId));%超过5帧从去年通过关键帧插入tooManyNonKeyFrames = currFrameIndex > = lastKeyFrameIndex + 5;%地图追踪不到100点tooFewMapPoints =元素个数(mapPointsIndices) < 100;%追踪地图点跟踪不到90%的点%参考关键帧tooFewTrackedPoints =元素个数(mapPointsIndices) < 0.9 * numPointsRefKeyFrame;isKeyFrame = (tooManyNonKeyFrames & & tooFewTrackedPoints) | | tooFewMapPoints;结束

helperCullRecentMapPoints剔除最近添加地图点。

函数[mapPointSet, directionAndDepth mapPointsIdx] =…helperCullRecentMapPoints (mapPointSet、directionAndDepth mapPointsIdx、newPointIdx stereoMapPointsIndices) outlierIdx = setdiff ([newPointIdx;stereoMapPointsIndices], mapPointsIdx);如果~ isempty (outlierIdx) mapPointSet = removeWorldPoints (mapPointSet outlierIdx);directionAndDepth =删除(directionAndDepth outlierIdx);mapPointsIdx = mapPointsIdx - arrayfun (@ (x) nnz (x > outlierIdx) mapPointsIdx);结束结束

helperUpdateGlobalMap更新三维地图的位置后点构成图优化

函数[mapPointSet, directionAndDepth] = helperUpdateGlobalMap (…mapPointSet、directionAndDepth vSetKeyFrames vSetKeyFramesOptim) posesOld = vSetKeyFrames.Views.AbsolutePose;一直陷于= vSetKeyFramesOptim.Views.AbsolutePose;positionsOld = mapPointSet.WorldPoints;positionsNew = positionsOld;指数= 1:mapPointSet.Count;%更新世界每个映射点的位置根据新的绝对的姿势%的主要观点为我= 1:mapPointSet。计数majorViewIds = directionAndDepth.MajorViewId(我);tform = posesOld (majorViewIds)。T \一直陷于(majorViewIds)。T;positionsNew(我:)= positionsOld(我,:)* tform (1:3, 1:3) + tform (1:3);结束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 ';结束结束