立体视觉同步定位与测绘

打开实时脚本

视觉同步定位与测绘(Visual simultaneous localization and mapping, vSLAM)，是指在同时测绘环境的同时，计算相机相对于其周围环境的位置和方向的过程。这个过程只使用摄像机的视觉输入。vSLAM的应用包括增强现实、机器人技术和自动驾驶。

vSLAM可以仅使用单目摄像机来执行。但是，由于无法使用单个摄像机精确计算深度，因此地图的比例和估计轨迹未知且随时间漂移。此外，为了引导系统，需要多个视图来生成初始地图，因为无法从fir对其进行三角剖分使用立体相机解决了这些问题，并提供了更可靠的vSLAM解决方案。

这个例子展示了如何处理来自立体相机的图像数据，以建立一个户外环境的地图，并估计相机的轨迹。本例使用了ORB-SLAM2的一个版本［1］算法，它是基于特征的，支持立体相机。金宝app

处理管线概述

立体vSLAM的管道与单目vSLAM的管道非常相似单目视觉同步定位与绘图的例子。主要的区别在于地图初始化阶段三维地图点由同一立体图像对的一对立体图像生成，而不是由两幅不同帧的图像生成。

地图初始化:管道首先使用视差图从一对立体图像初始化三维点的映射。左边的图像被存储为第一个关键帧。
跟踪一旦地图被初始化，对于每一对新的立体图像，相机的姿态是通过匹配左边图像的特征到最后一个关键帧的特征来估计的。通过跟踪局部地图，对估计的相机姿态进行优化。
当地的地图：如果当前左侧图像被识别为关键帧，则根据立体对的视差计算新的三维贴图点。在此阶段，使用束调整通过调整相机姿势和三维点来最小化重投影错误。
循环关闭:通过使用特征包方法将每个关键帧与之前的所有关键帧进行比较，从而检测出每个关键帧的循环。一旦检测到一个闭环，姿态图将被优化，以细化所有关键帧的相机姿态。

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

本例中使用的数据来自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.zip (818 MB)。这次下载可能需要几分钟。”) mget (ftpObj“/2020 vtr数据集/UTIAS暗中运行/run_000005.zip”, tempFolder);%提取下载文件的内容disp ('正在提取run_000005.zip（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管道从初始化包含三维世界点的地图开始。这一步骤至关重要，对最终SLAM结果的准确性有重大影响。使用matchFeatures在一对立体图像之间。匹配的配对必须满足以下约束条件:

校正后的立体图像中两个对应特征点之间的水平位移小于最大视差。你可以确定近似的最大视差值从立体声对图像的立体浮雕。有关更多信息，请参见选择视差范围。
校正后的立体图像中两个对应特征点的垂直位移小于阈值。
匹配特征的尺度几乎是相同的。

与匹配特征点对应的三维世界位置确定如下：

使用disparitySGM采用半全局匹配(SGM)方法计算每对立体图像的视差图。
使用reconstructScene从视差图中计算出三维世界点坐标。
在视差图中找到与特征点对应的位置和它们的三维世界位置。

%为重现性设置随机种子rng (0);载入初始相机姿态。摄像机初始姿态的推导基于相机和车辆之间的转换%:% http://asrl.utias.utoronto.ca/datasets/2020-vtr-dataset/text_files/transform_camera_vehicle.txinitialPoseData =负载(“initialPose.mat”）;initialPose = initialPoseData.initialPose;创建一个stereparameters对象来存储立体相机参数。%可在以下页面找到数据集的内部函数：% 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 = stereparameters (cameraParam, cameraParam, eye(3)， [-baseline, 0 0]);%在这个例子中，图像已经是未变形的。在一般%工作流中，取消注释以下代码以取消对图像的干扰。% currILeft = undistort timage (currILeft, intrinsics);% currIRight = undistortion timage (currIRight, intrinsics);%矫正立体图像[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, max视差);

数据管理和可视化

使用第一个立体声对初始化映射后，可以使用imageviewset，worldpointset和helperViewDirectionAndDepth存储第一个关键帧和对应的映射点:

%创建一个空的imageviewset对象来存储关键帧vSetKeyFrames=图像视图集；%创建空的worldpointset对象以存储三维贴图点mapPointSet = worldpointset;创建一个helperViewDirectionAndDepth对象来存储视图的方向和深度directionAndDepth = helperViewDirectionAndDepth(size(xyzPoints, 1));添加第一个关键帧currKeyFrameId = 1;vSetKeyFrames = addView(vSetKeyFrames, currKeyFrameId, initialPose，“点”currPointsLeft,．..“特性”, currFeaturesLeft.Features);%添加3d地图点[mapPointSet, stereoMapPointsIdx] = addWorldPoints(mapPointSet, xyzPoints);增加地图点的观察值mapPointSet = addinterfaces (mapPointSet, currKeyFrameId, stereoMapPointsIdx, matchedPairs(:， 1));%在第一个关键帧中可视化匹配的特征featurePlot = helperVisualizeMatchedFeaturesStereo(currILeft, currIRight, currPointsLeft，．..电流点右，匹配对）；可视化初始地图点和摄像机轨迹mapPlot = helperVisualizeMotionAndStructureStereo(vSetKeyFrames, mapPointSet);%显示传奇显示图例（地图）；

初始化位置识别数据库

环路检测是使用词袋方法进行的。用A表示的视觉词汇巴戈菲特酒店对象使用从数据集中的大量图像中提取的ORB描述符离线创建，方法是调用:

bag = bagfeatures (imds，'CustomExtractor'， @helperORBFeatureExtractorFunction， 'TreeProperties'， [2, 20]， ' stronggestfeatures '， 1);

在哪里洛桑国际管理发展学院是一个imageDatastore对象存储训练图像和helperORBFeatureExtractorFunction是ORB特性提取器函数。看到图像检索与袋视觉词为更多的信息。

循环关闭过程增量地构建一个数据库，表示为invertedImageIndex对象，该对象存储基于ORB特性包的可视文字到图像映射。

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

跟踪

跟踪过程使用每对并决定何时插入新的关键帧。

上一个关键帧的% ViewIdlastKeyFrameId = currKeyFrameId;%具有最多共可见的参考关键帧的ViewId%使用当前关键帧映射点refKeyFrameId = currKeyFrameId;%输入图像序列中最后一个关键帧的索引lastKeyFrameIdx = currFrameIdx;%输入图像序列中所有关键帧的索引addedFramesIdx = lastKeyFrameIdx;currFrameIdx = 2;isLoopClosed = false;

每帧的处理如下:

为每个新的立体图像对提取ORB特征，然后进行匹配(使用matchFeatures)，最后一个关键帧中的特征具有已知的相应三维贴图点。
使用透视n点算法估计摄像机姿态estimateWorldCameraPose．
给定摄影机姿势，将最后一个关键帧观察到的贴图点投影到当前帧中，并使用matchFeaturesInRadius．
在当前帧中使用3-D到2-D的对应，通过执行仅运动束调整来优化相机的姿态bundleAdjustmentMotion．
将局部地图点投影到当前框架中，以搜索更多的特征对应matchFeaturesInRadius并再次使用优化摄影机姿势bundleAdjustmentMotion．
跟踪的最后一步是决定当前帧是否应该是一个新的关键帧。满足以下两个条件的帧就是关键帧:

自最后一个关键帧或当前帧跟踪少于100个贴图点以来，至少经过了5帧。
当前帧跟踪的地图点少于参考关键帧跟踪的90%。

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

%主循环虽然~isLoopClosed && currFrameIdx <= numel(imdsLeft. files) currILeft = readimage(imdsLeft, currFrameIdx); / /设置当前帧currIRight = readimage(imdsRight, currFrameIdx);[currILeft, currIRight] = rectifyStereoImages(currILeft, currIRight, stereoParams，“OutputView”，“全部”)；[currFeaturesLeft，currPointsLeft]=helperDetectAndExtractFeatures（currILeft，scaleFactor，NumLevel）；[currFeaturesRight，currPointsRight]=helperDetectAndExtractFeatures（currIRight，scaleFactor，NumLevel）；%跟踪最后一个关键帧% trackkedmappointsidx:当前左帧中观察到的映射点的索引% trackkedfeatureidx:当前左帧对应特征点的索引[currPose, trackkedmappointsidx, trackkefeatureidx] = helperTrackLastKeyFrame(mapPointSet，．..vSetKeyFrames。的观点,currFeaturesLeft, currPointsLeft, lastKeyFrameId, intrinsics, scaleFactor);如果isempty(currPose) || numel(trackkedmappointsidx) < 30 currFrameIdx = currFrameIdx + 1;继续结束%跟踪本地地图%refKeyFrameId：具有最大与当前帧共可见的地图点% localKeyFrameIds:当前帧连接的关键帧的ViewId如果currKeyFrameId==1 refKeyFrameId=1；LocalKeyframeId=1；其他的[refKeyFrameId、LocalKeyframeId、currPose、TrackedAppointSidx、trackedFeatureIdx]=．..helperTrackLocalMap(mapPointSet, directionAndDepth, vSetKeyFrames, trackkedmappointsidx，．..trackkedfeatureidx, currPose, currFeaturesLeft, currPointsLeft, intrinsics, scaleFactor, numLevels);结束%匹配立体图像之间的特征点，得到三维世界位置[xyzPoints, matchedPairs] = helperReconstructFromStereo(currILeft, currIRight, currFeaturesLeft，．..currFeaturesRight, currPointsLeft, currPointsRight, stereoParams, currPose, max视差);[untrackkedfeatureidx, ia] = setdiff(matchedPairs(:， 1)， trackkedfeatureidx); / /设置匹配项xyzPoints = xyzPoints(ia，:);%检查当前帧是否为关键帧isKeyFrame = helperIsKeyFrame(mapPointSet, refKeyFrameId, lastKeyFrameIdx，．..currFrameIdx、TrackedAppointSidX）；%在立体图像中可视化匹配的特征updatePlot（featurePlot、currILeft、currIRight、currPointsLeft、currPointsRight、trackedFeatureIdx、matchedPairs）；如果~isKeyFrame && currFrameIdx < numel(imdsLeft.Files) currFrameIdx = currFrameIdx + 1;继续结束%更新当前关键帧IDcurrKeyFrameId = currKeyFrameId + 1;

当地的地图

对每个关键帧进行局部映射。当确定一个新的关键帧时，将其添加到关键帧中，更新新关键帧观察到的地图点的属性。以确保mapPointSet包含尽可能少的异常值，必须在至少3个关键帧中观察到有效的映射点。

通过对当前关键帧及其连接的关键帧中的ORB特征点进行三角化来创建新的映射点。对于当前关键帧中的每一个未匹配的特征点，利用该方法在已连接的关键帧中与其他未匹配的特征点进行匹配matchFeatures．局部束调整优化当前关键帧的位姿、连接关键帧的位姿以及在这些关键帧中观察到的所有地图点。

%添加新的关键帧[mappointment set, vSetKeyFrames] = helperAddNewKeyFrame(mappointment set, vSetKeyFrames，．..currPose、currFeaturesLeft、currPointsLeft、TrackedAppointSidx、trackedFeatureIdx、localKeyFrameIds）；%删除在关键帧内观察到的离群点如果currKeyFrameId == 2 triangulatedMapPointsIdx = [];结束[mapPointSet, directionAndDepth, trackkedmappointsidx] =．..helperCullRecentMapPoints(mapPointSet, directionAndDepth, trackkedmappointsidx, triangulatedMapPointsIdx，)．..stereoMapPointsIdx);从视差中添加新的地图点[mapPointSet, stereoMapPointsIdx] = addWorldPoints(mapPointSet, xyzPoints);mapPointSet = addmappings (mapPointSet, currKeyFrameId, stereoMapPointsIdx，．..未跟踪的特征IDX）；通过三角测量创建新的地图点minNumMatches = 20;minParallax = 0.35;[mapPointSet, vSetKeyFrames, triangulatedMapPointsIdx, stereoMapPointsIdx] = helperCreateNewMapPointsStereo(．..mapPointSet、vSetKeyFrames、currKeyFrameId、内部函数、缩放因子、最小匹配、最小视差、，．..untrackedFeatureIdx stereoMapPointsIdx);%更新视图方向和深度directionAndDepth = update(directionAndDepth, mapPointSet, vSetKeyFrames.)的观点,．..[trackedMapPointsIdx;triangulatedMapPointsIdx;stereoMapPointsIdx),真正的);局部束平差[mapPointSet、方向和深度、VSET关键帧、三角化MapPointSidX、stereoMapPointsIdx]=．..helperLocalBundleAdjustmentStereo (mapPointSet directionAndDepth vSetKeyFrames,．..currKeyFrameId, intrinsics, triangulatedMapPointsIdx, stereoMapPointsIdx);可视化三维世界点和摄像机轨迹updatePlot (mapPlot vSetKeyFrames mapPointSet);

循环关闭

循环结束步骤采用局部映射过程处理的当前关键帧，并尝试检测和关闭循环。循环候选通过查询数据库中与当前使用的关键帧在视觉上相似的图像来识别evaluateImageRetrieval．如果一个候选关键帧没有连接到最后一个关键帧，并且它的三个邻居关键帧是循环候选帧，那么这个候选关键帧是有效的。

当找到一个有效的循环闭包候选帧时，使用算法计算循环闭包候选帧与当前关键帧之间的相对位姿估计几何变换3D．然后添加与相对姿态和更新的循环连接mapPointSet和vSetKeyFrames．

%在一些关键帧被创建后检查循环关闭如果currKeyFrameId > 50%循环边的最小特征匹配数loopEdgeNumMatches = 45;%检测可能的循环关闭关键帧候选[isDetected, validLoopCandidates] = helperCheckLoopClosure(vSetKeyFrames, currKeyFrameId，．..loopDatabase、currILeft、LoopEdgeEnumMatches）；IsToCloseView=currKeyFrameId-max（ValidLoop候选者）<100；如果isDetected & & ~ isTooCloseView%添加循环闭包连接[isLoopClosed, mapPointSet, vSetKeyFrames] = helperAddLoopConnectionsStereo(．..mapPointSet, vSetKeyFrames, validLoopCandidates, currKeyFrameId，．..currFeaturesLeft loopEdgeNumMatches);结束结束%如果没有检测到循环闭包，将当前特性添加到数据库中如果~isLoopClosed addmagefeatures (loopDatabase, currFeaturesLeft, currKeyFrameId);结束%更新id和索引lastKeyFrameId = currKeyFrameId;lastKeyFrameIdx = currFrameIdx;addedFramesIdx = [addedFramesIdx;currFrameIdx];currFrameIdx = currFrameIdx + 1;结束%主循环结束

Loop edge added between keyframe: 1 and 282 Loop edge added between keyframe: 4 and 282

最后，对基本图进行位姿图优化vSetKeyFrames纠正漂移。本质图是通过去除小于的连接而在内部创建的minNumMatches在共可见图中匹配。在姿态图优化后，使用优化后的姿态更新地图点的三维位置。

优化姿势minNumMatches = 30;vSetKeyFramesOptim = optimizePoses(vSetKeyFrames, minNumMatches，“宽容”1 e-16);%在优化姿态后更新地图点mapPointSet = helperUpdateGlobalMap(mapPointSet, directionAndDepth, vSetKeyFrames, vSetKeyFramesOptim);updatePlot (mapPlot vSetKeyFrames mapPointSet);%绘制优化的摄像机轨迹optimizedPoses =姿势(vSetKeyFramesOptim);plotOptimizedTrajectory (mapPlot optimizedPoses)%更新传奇显示图例（地图）；

与地面真理比较

可以将优化后的摄像机轨迹与地面真实值进行比较，以评估解决方案的准确性。下载的数据包含gps.txt文件存储了每一帧的GPS位置。您可以转换GPS位置从地理坐标到本地笛卡尔坐标使用拉丁美洲(自动驾驶工具箱)自动驾驶工具箱或geodetic2enu(映射工具箱)从映射工具。在本例中，您可以简单地从m文件加载转换后的GPS数据。

装载GPS数据gpsData =负载(“gpsLocation.mat”）;gpsLocation = gpsData.gpsLocation;%将GPS位置转换为参考坐标系gTruth = helperTransformGPSLocations(gpsLocation, optimizedpose);%绘制GPS位置plotActualTrajectory (mapPlot gTruth (addedFramesIdx:));%显示传奇显示图例（地图）；

金宝app支持功能

下面列出了简短的辅助函数。较大的函数包含在单独的文件中。

helperDetectAndExtractFeatures从图像中检测、提取和提取特征。

作用[features, validPoints] = helperDetectAndExtractFeatures(Irgb, scaleFactor, numLevels) numPoints = 800;%检测ORB功能Igray=im2gray（Irgb）；点=检测器特征（Igray，“ScaleFactor”，scaleFactor，“NumLevels”, numLevels);%选择均匀分布在整个图像中的特征子集点数= selectUniform(点数，numPoints，大小(Igray, 1:2));%提取特征[features, validPoints] = extractFeatures(Igray, points);结束

helperReconstructFromStereo利用视差图从立体图像重建场景

作用[xyzPoints, indexPairs] = helperReconstructFromStereo(I1, I2，．..indexPairs = helperFindValidFeaturePairs(features1, features2, points1, points2, max悬殊);%计算左边图像中所有像素的视差。在实践中，它是更多通常只对匹配的特征点计算视差。disitymap = disityysgm (rgb2gray(I1)， rgb2gray(I2))，“DisparityRange”，[0，MaxDisparency]）；xyzPointsAll=重建场景（disparityMap，stereoParams）；%找到匹配特征点对应的世界点位置=地板(里。位置(indexPairs(:， 1)， [2 1]);xyzPoints = [];isPointFound = false(大小(里));为point3d = squeeze(xyzPointsAll(locations(i,1)， locations(i, 2)，:)))';isPointValid = all(~isnan(point3d)) && all(isfinite(point3d)) && point3d(3) > 0;isDepthInRange = point3d(3) < 200*norm(stereoParams.TranslationOfCamera2);如果isPointValid && isDepthInRange xyzPoints = [xyzPoints;point3d];% #好< * AGROW >isPointFound (i) = true;结束结束indexPairs = indexPairs(isPointFound，:);xyzPoints = xyzPoints * currPose。旋转+ currPose.Translation;结束

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

作用indexPairs = helperFindValidFeaturePairs(features1, features2, point1, point2, max悬殊)indexPairs = matchFeatures(features1, features2，．..“独特的”,真的,“MaxRatio”, 1“MatchThreshold”, 40);matchedpoint1 = point1 . location (indexPairs(:，1)，:);matchedpoint2 = point2 . location (indexPairs(:，2)，:);= point1 . scale (indexPairs(:，1)，:);= point2 . scale (indexPairs(:，2)，:);dist2EpipolarLine = abs(matchedPoints2(:， 2) - matchedPoints1(:， 2));shiftDist = matchedPoints1(:， 1) - matchedPoints2(:， 1);isCloseToEpipolarline = dist2EpipolarLine < 2*scales2;isdisityvalid = shiftDist > 0 & shiftDist < max悬殊;isScaleIdentical = scales1 == scales2; indexPairs = indexPairs(isCloseToEpipolarline & isDisparityValid & isScaleIdentical, :);结束

helperIsKeyFrame检查帧是否是关键帧。

作用isKeyFrame = helperIsKeyFrame (mappoint,．..numPointsRefKeyFrame = nummel (findWorldPointsInView(mapPoints, refKeyFrameId));%上次插入关键帧后经过的帧超过5帧tooManyNonKeyFrames=currFrameIndex>=lastKeyFrameIndex+5；%追踪少于100个地图点tooFewMapPoints=numel（映射点sindices）<100；%被跟踪的地图点少于90%被跟踪的点%参考关键帧toofewtrackkedpoints = nummel (mapPointsIndices) < 0.9 * numPointsRefKeyFrame;isKeyFrame = (tooManyNonKeyFrames && toofewtrackkedpoints) || tooFewMapPoints;结束

helperCullRecentMapPointsCull最近增加了地图点。

作用[mapPointSet, directionAndDepth, mapPointsIdx] =．..helperCullRecentMapPoints(mapPointSet, directionAndDepth, mapPointsIdx, newPointIdx, stereoMapPointsIndices) = setdiff([newPointIdx;stereoMapPointsIndices], mapPointsIdx);如果~isempty(outlierIdx) mapPointSet = removeWorldPoints(mapPointSet, outlierIdx);directionAndDepth = remove(directionAndDepth, outlierIdx);mappointment sidx = mappointment sidx - arrayfun(@(x) nnz(x>outlierIdx)， mappointment sidx);结束结束

helperUpdateGlobalMap在姿态图优化后更新地图点的三维位置

作用[mapPointSet，Direction and Depth]=helperUpdateGlobalMap(．..posesOld = vSetKeyFrames. views . absolutepose;一直陷于= vSetKeyFramesOptim.Views.AbsolutePose;positionsOld = mapPointSet.WorldPoints;positionsNew = positionsOld;指数= 1:mapPointSet.Count;%更新每个地图点的世界位置基于新的绝对姿态%对应的主要观点为i = 1: mapPointSet。Count majorviewid = directionAndDepth.MajorViewId(i); / /显示主视图tform = posesOld (majorViewIds)。T \一直陷于(majorViewIds)。T;position new (i，:) = positionsOld(i，:) * tform(1:3,1:3) + tform(4, 1:3);结束mapPointSet = updateWorldPoints(mapPointSet, indices, positionsNew);结束

helperTransformGPSLocations将GPS位置转换为参考坐标系统

作用gTruth = helperTransformGPSLocations(gpsLocations, optimizedPoses) initialYawGPS = atan((gpsLocations(1002) - gpsLocations(1,2)) /．..（gpsLocations（100,1）-gpsLocations（1,1））；initialYawSLAM=atan（（优化操作系统。绝对姿势（50）。翻译（2）-．..optimizedPoses.AbsolutePose (1) .Translation) / (2)．..(optimizedPoses.AbsolutePose (59) .Translation (1) -．..optimizedPoses.AbsolutePose (1) .Translation (1)));relYaw = initialYawGPS - initialYawSLAM;relTranslation = optimizedPoses.AbsolutePose (1) .Translation;initialform = rotationVectorToMatrix([0 0 relYaw]);为i = 1:size(gpsLocations, 1) gTruth(i，:) = initialform * gpsLocations(i，:)' + relTranslation';结束结束