主要内容

立体视觉同步定位与地图绘制

打开生活的脚本

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

vSLAM可以通过单目相机完成。然而，由于深度不能用单个摄像机精确计算，地图的比例尺和估计的轨迹是未知的，并且会随着时间漂移。此外，为了引导系统，需要多个视图来生成初始地图，因为它不能从第一帧三角剖分。使用立体摄像机解决了这些问题，并提供了更可靠的vSLAM解决方案。

此示例显示如何从立体声相机处理图像数据以构建室外环境的地图并估算相机的轨迹。该示例使用Orb-Slam2的版本[1]算法是基于特征的，支持立体声相机。万博1manbetx

加工管道概述

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

  • 地图初始化管道首先使用视差图从一对立体图像中初始化3-D点的地图。左侧图像存储为第一个关键帧。

  • 跟踪:地图初始化后，对于每一个新的立体对，通过将左边图像中的特征与最后一个关键帧中的特征匹配来估计摄像机的姿态。通过跟踪局部地图，对估计的摄像机姿态进行细化。

  • 本地映射:如果当前的左边图像被确定为一个关键帧，新的三维地图点将从立体对的视差计算。在这一阶段，通过调整相机姿态和三维点，束调整来最小化重投影误差。

  • 循环关闭:循环检测每一个关键帧，通过比较所有之前的关键帧使用袋的特征方法。一旦检测到循环闭合，姿态图将被优化，以细化所有关键帧的摄像机姿态。

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

实例中使用的数据来自UTIA长期本地化和映射数据集由多伦多大学航空航天研究所提供。你可以使用网页浏览器或运行以下代码将数据下载到一个临时目录:

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 -录像机集/ UTIAS-In-The-Dark run_000005.zip”, tempFolder);%解压下载文件的内容disp (解压run_000005.zip (818 MB)…）解压缩（zipfilename，datafolder）;结束

使用两个imageageAtastore.对象来存储立体图像。

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世界点的地图。这一步至关重要，对最终SLAM结果的准确性有重要影响。初始ORB特征点通信使用matchFeatures在一对立体图像之间。匹配的对应满足以下约束:

  • 校正后的立体对图像中对应的两个特征点的水平位移小于最大视差。可以从立体对图像的立体浮雕中确定近似的最大视差值。有关更多信息，请参见选择视差范围。

  • 校正后的立体对图像中对应的两个特征点的垂直位移小于一个阈值。

  • 匹配特征的尺度基本一致。

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

  • 使用disparitySGM采用半全局匹配(SGM)方法计算每对立体图像的视差图。

  • 使用重建科学从视差图计算三维世界点坐标。

  • 在视差图中找到与特征点及其三维世界位置对应的位置。

％设置随机种子以进行再现性RNG（0）;%加载初始相机姿态。初始摄像机姿态是基于相机与车辆之间的变换的百分比：% http://asrl.utias.utoronto.ca/datasets/2020-vtr-dataset/text_files/transform_camera_vehicle.txinitialPoseData =负载(“initialPose.mat”）;initialPose = initialPoseData.initialPose;%创建一个stereparameters对象来存储立体相机参数。%该数据集的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);．..林城（1），校长（2），1];图像智能=尺寸（跨核对级，[1,2]）;%单位为像素[mrows, ncols]cameraParam = cameraParameters (“IntrinsicMatrix”，IntrinsicMatrix，“图象尺寸”、图象尺寸);intrinsic = cameraParam.Intrinsics;stereparams = stereparameter (camerparam, camerparam, eye(3)， [-baseline, 0 0]);在这个例子中，图像已经没有扭曲。在一般% workflow，取消以下代码的注释以恢复图像的扭曲。% currILeft = un畸变图像(currILeft, intrinsics);% currIRight = un畸变图像(currIRight, intrinsics);%立体图像校正[currILeft, currIRight] = rectifyStereoImages(currILeft, currIRight, stereparams，“OutputView”，'满的'）;%从校正后的立体图像中检测并提取ORB特征scalefactor = 1.2;numlevels = 8;[currFeaturesLeft, currPointsLeft] = helperDetectAndExtractFeatures(currILeft, scaleFactor, numLevels);[currFeaturesRight, currPointsRight] = helperDetectAndExtractFeatures(curright, scaleFactor, numLevels);%匹配立体图像之间的特征点，得到三维世界的位置maxDisparity = 48;%以像素指定[xyzPoints, matchedPairs] = helperReconstructFromStereo(currILeft, currIRight，．..CurrfeaturesLeft，CurrfeaturesReight，Curpoptpopleleft，Curpopitssstright，Stereoparams，Iirstale，MaxDisparity）;

数据管理和可视化

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

创建一个空的imageviewset对象来存储关键帧vSetKeyFrames = imageviewset;%创建一个空的worldpointset对象来存储3d地图点mapPointSet = worldpointset;%创建一个helperViewDirectionAndDepth对象来存储视图的方向和深度directionAndDepth = helperViewDirectionAndDepth(size(xyzPoints, 1));添加第一个关键帧currKeyFrameId = 1;vSetKeyFrames = addView(vSetKeyFrames, currKeyFrameId, initialPose，“点”currPointsLeft,．..“特性”, currFeaturesLeft.Features);%添加3d地图点[mapPointSet, stereoMapPointsIdx] = addWorldPoints(mappoint, xyzPoints);%添加地图点的观察mapPointSet = addcontacts (mapPointSet, currKeyFrameId, stereoMapPointsIdx, matchedPairs(:， 1));%在第一个关键帧中可视化匹配的特征Featureplot = HelpervisualizeMatchedFeaturessTereo（威尔，速度，Curpopitsleft，．..currPointsRight matchedPairs);％可视化初始映射点和相机轨迹mapPlot = helperVisualizeMotionAndStructureStereo(vSetKeyFrames, mapPointSet);％显示传奇showLegend (mapPlot);

跟踪

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

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

每一帧的处理如下:

  1. 对每一对新的立体图像提取ORB特征，然后进行匹配(使用matchFeatures)，其最后一个关键帧的特征已知相应的3-D地图点。

  2. 使用Perspective-n-Point算法估计摄像机姿态estimateWorldCameraPose．

  3. 给定相机姿态，将最后一个关键帧观测到的地图点投影到当前帧中，使用搜索特征对应matchFeaturesInRadius．

  4. 使用当前帧中的3-D到2-D对应，通过执行仅运动束调整来改进摄像机姿态bundleAdjustmentMotion．

  5. 将局部地图点投影到当前框架中，以搜索更多的特征对应使用matchFeaturesInRadius再用相机调整姿势bundleAdjustmentMotion．

  6. 跟踪的最后一步是决定当前帧是否应该是一个新的关键帧。当满足以下两个条件时，即为关键帧:

  • 自最后一个关键帧以来，至少有5帧通过，或者当前帧跟踪不到100个地图点。

  • 当前帧所跟踪的地图点少于参考关键帧所跟踪的点的90%。

如果当前帧成为关键帧，则继续进行本地映射过程。否则，开始跟踪下一帧。

%主循环而currFrameIdx < numel(imdsLeft. files) currILeft = readimage(imdsLeft, currFrameIdx);currIRight = readimage(imdsRight, currFrameIdx);[currILeft, currIRight] = rectifyStereoImages(currILeft, currIRight, stereparams，“OutputView”，'满的'）;[currFeaturesLeft, currPointsLeft] = helperDetectAndExtractFeatures(currILeft, scaleFactor, numLevels);[currFeaturesRight, currPointsRight] = helperDetectAndExtractFeatures(curright, scaleFactor, numLevels);％跟踪最后一个关键帧% trackdmappointsidx:当前左帧中观察到的地图点的索引% trackedFeatureIdx:当前左帧中对应特征点的索引[currPose, trackdmappointsidx, trackfeatureidx] = helperTrackLastKeyFrame(mapPointSet，．..vSetKeyFrames。视图，currFeaturesLeft, currPointsLeft, lastKeyFrameId, intrinsics, scaleFactor);如果isempty(currPose) || numel(trackdmappointsidx) < 30 currFrameIdx = currFrameIdx + 1;继续结束％跟踪本地地图% refKeyFrameId:拥有最多的参考关键帧的ViewId%与当前帧共同可见的地图点% localKeyFrameIds:当前帧连接的关键帧的ViewId如果currKeyFrameId == 1 refKeyFrameId = 1;localKeyFrameIds = 1;其他的[refKeyFrameId, localkeyframeid, currPose, trackdmappointsidx, trackfeatureidx] =．..helperTrackLocalMap(mapPointSet, directionAndDepth, vSetKeyFrames, trackdmappointsidx，．..trackedFeatureIdx, currPose, currFeaturesLeft, currPointsLeft, intrinsics, scaleFactor, numLevels);结束%匹配立体图像之间的特征点，得到三维世界的位置[XyzPoints，MatchedPairs] = HelperReconstructFromStereo（跨跨时级，速度，Currfeaturesleft，．..currFeaturesRight, currPointsLeft, currPointsRight, stereparams, currPose, max悬殊);[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 = currFrameIdx + 1;继续结束%更新当前关键帧IDcurrKeyFrameId = currKeyFrameId + 1;

本地映射

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

通过对当前关键帧及其连接的关键帧中的ORB特征点进行三角剖分，可以创建新的地图点。对于当前关键帧中每个未匹配的特征点，使用matchFeatures．局部束调整改进了当前关键帧的姿势，连接关键帧的姿势，以及在这些关键帧中观察到的所有地图点。

%添加新的关键帧[mapPointSet, vSetKeyFrames] = helperAddNewKeyFrame(mapPointSet, vSetKeyFrames, vSetKeyFrames)．..currPose, currFeaturesLeft, currPointsLeft, trackeddmappointsidx, trackedFeatureIdx, localKeyFrameIds);%删除在少于3个关键帧中观察到的异常映射点如果currKeyFrameId == 2 triangulatedMapPointsIdx = [];结束[mappointset，directionanddepth，trackedmappointsidx] =．..helperCullRecentMapPoints(mapPointSet, directionAndDepth, trackdmappointsidx, triangulatedMapPointsIdx，．..stereoMapPointsIdx);%添加新的地图点计算的视差[mapPointSet, stereoMapPointsIdx] = addWorldPoints(mappoint, xyzPoints);mapPointSet = addcontacts (mapPointSet, currKeyFrameId, stereoMapPointsIdx，．..untrackedFeatureIdx);%通过三角测量创建新的地图点minNumMatches = 10;minParallax = 0.35;[mapPointSet, vSetKeyFrames, triangulatedMapPointsIdx, stereoMapPointsIdx] = helpercreatenewmappointstereo (．..mapPointSet, vSetKeyFrames, currKeyFrameId, intrinsics, scaleFactor, minNumMatches, minParallax，．..untrackedFeatureIdx stereoMapPointsIdx);%更新视图方向和深度directionanddepth =更新（directionanddepth，mappointset，vsetkeyframes.views，．..[trackedMapPointsIdx;triangulatedMapPointsIdx;stereoMapPointsIdx),真正的);局部束调整[mapPointSet, directionAndDepth, vSetKeyFrames, triangulatedMapPointsIdx, stereoMapPointsIdx] =．..helperLocalBundleAdjustmentStereo (mapPointSet directionAndDepth vSetKeyFrames,．..currKeyFrameId, intrinsics, triangulatedMapPointsIdx, stereoMapPointsIdx);%可视化3-D世界点和相机轨迹updatePlot (mapPlot vSetKeyFrames mapPointSet);

循环关闭

循环关闭步骤采用本地映射过程处理的当前关键帧，并尝试检测和关闭循环。循环检测使用词袋方法执行。用。表示的视觉词汇bagOfFeatures对象通过调用从数据集中大量图像中提取的SURF描述符离线创建:

袋=袋功能(imds，'CustomExtractor'， @helperSURFFeatureExtractorFunction);

在哪里洛桑国际管理发展学院是一个imageageAtastore.对象存储训练图像和helperSURFFeatureExtractorFunctionSURF功能提取器功能是否。看基于视觉词袋的图像检索为更多的信息。

循环关闭过程递增地构建数据库，表示为一个数据库invertedImageIndex对象，存储基于SURF特征包的视觉文字到图像映射。通过在数据库中查询与当前关键帧在视觉上相似的图像来识别循环候选者evaluateImageRetrieval．如果一个候选关键帧没有连接到最后一个关键帧，并且它的三个相邻关键帧是循环候选帧，那么这个候选关键帧就是有效的。

当找到一个有效的循环闭包候选帧时，使用estimateGeometricTransform3D．然后添加与相对姿态和更新的循环连接mapPointSet和vSetKeyFrames．

初始化循环闭包数据库如果currKeyFrameId = = 2%加载离线创建的特性数据包bofData =负载(“bagOfFeaturesUTIAS.mat”）;初始化位置识别数据库loopCandidates = [1;2);loopDatabase = indexImages(子集(imdsLeft, loopCandidates)， bofData.bof);%在一些关键帧被创建后检查循环闭包elseifCurrkeyFrameID> 50.%循环边特征匹配的最小数目loopEdgeNumMatches = 40;%检测可能的循环闭包关键帧候选[isDetected, validLoopCandidates] = helperCheckLoopClosure(vSetKeyFrames, currKeyFrameId，．..loopDatabase, currILeft, loopcandidate, loopEdgeNumMatches);isTooCloseView = currKeyFrameId - max(validLoopCandidates) < 20;如果isdetected &&〜iStoocloseView%添加循环闭包连接[isLoopClosed, mapPointSet, vSetKeyFrames] = helperAddLoopConnectionsStereo()．..mapPointSet, vSetKeyFrames, validLoopCandidates, currKeyFrameId，．..currFeaturesLeft、currPointsLeft loopEdgeNumMatches);结束结束%如果没有检测到循环闭包，将图像添加到数据库中如果~isLoopClosed addImages(loopDatabase, sub子集(imdsLeft, currFrameIdx)，“详细”、假);loopCandidates = [loopCandidates;currKeyFrameId];结束%更新id和索引lastKeyFrameId = currKeyFrameId;lastKeyFrameIdx = currFrameIdx;AddatedFramesIDX = [已添加FramesIdx;CurrframeIDX];CurrFrameIDX = CurrFrameIDX + 1;结束%主循环结束
使用Bag-Of-Features创建反向图像索引。------------------------------------------------------- 使用Bag-Of-Features编码图像。-------------------------------------- * 编码2图片…。完成图像索引的创建。
在关键框架之间添加循环边缘：2和268

最后，对关键图进行位姿图优化vSetKeyFrames纠正漂移。基本图是通过删除小于的连接在内部创建的minnummatches.匹配在可执行性图中。姿态图形优化后，使用优化的姿势更新地图点的三维位置。

％优化姿势minNumMatches = 10;vSetKeyFramesOptim = optimizepose (vSetKeyFrames, minNumMatches，'宽容'1 e-16);%在优化姿态后更新地图点mapPointSet = helperUpdateGlobalMap(mapPointSet, directionAndDepth, vSetKeyFrames, vSetKeyFramesOptim);updatePlot (mapPlot vSetKeyFrames mapPointSet);%绘制优化后的摄像机轨迹optimizedPoses =姿势(vSetKeyFramesOptim);plotOptimizedTrajectory (mapPlot optimizedPoses)%更新传奇showLegend (mapPlot);

与Ground Truth相比

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

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

万博1manbetx支持功能

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

helperDetectAndExtractFeatures从图像中提取ORB特征。

函数[features, validPoints] = helperDetectAndExtractFeatures(Irgb, scaleFactor, numLevels) numPoints = 800;%检测ORB特性Igray = rgb2gray (Irgb);点= detectORBFeatures (Igray,“ScaleFactor”scaleFactor,“NumLevels”, numLevels);%选择一个特征子集，均匀分布在整个图像中点= SelectUniform（点，Numpoints，大小（Igray，1：2））;%提取特征[功能，有效点] =提取物（IGRAI，点）;结束

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

函数[xyzPoints, indexPairs] = helperReconstructFromStereo(I1, I2，．..特点1，特点2，点，点2，立体声邮件，省略，maxDisparity）IndexPairs = HelperFindValidFeaturePairs（特点1，特征2，点，点2，MaxDisparity）;左图像中所有像素的％计算差异。在实践中，更多% common来计算只匹配特征点的视差。= disparitySGM(rgb2gray(I1)， rgb2gray(I2))，“DisparityRange”[0, maxDisparity]);xyzPointsAll = reconstructScene(disparityMap, stereparams);％找到匹配特征点的相应世界点位置=地板(里。indexPairs(:， 1)， [2 1]);xyzPoints = [];isPointFound = false(大小(里));为i = 1:size(locations, 1) point3d = squeeze(xyzPointsAll(locations(i,1)， locations(i, 2)，:))';isPointValid = all(~isnan(point3d)) && all(isfinite(point3d)) && point3d(3) > 0;isDepthInRange = point3d(3) < 200*norm(stereparams . translationofcamera2);如果isPointValid && isDepthInRange xyzPoints = [xyzPoints;point3d];% #好< * AGROW >isPointFound (i) = true;结束结束indexPairs = indexPairs(isPointFound，:);xyzPoints = xyzPoints * currPose。旋转+ currPose.Translation;结束

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

函数indexPairs = helperFindValidFeaturePairs(featres1, featres2, points1, points2, max悬殊)indexPairs = matchFeatures(featres1, featres2, max悬殊)．..“独特的”， 真的，“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;isdisityvalid = shiftDist > 0 & shiftDist < max视差;isScaleIdentical = scales1 == scales2; indexPairs = indexPairs(isCloseToEpipolarline & isDisparityValid & isScaleIdentical, :);结束

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

函数isKeyFrame = helperIsKeyFrame (mappoint,．..numPointsRefKeyFrame = numel(findWorldPointsInView(mapPoints, refKeyFrameId));从最后一个关键帧插入超过10个帧toomanynonkeyframes = currframeIndex> = lastKeyFrameIndex + 10;%追踪少于100个地图点tooFewMapPoints = nummel (mapPointsIndices) < max(100, 0.25 * numPointsRefKeyFrame);%被跟踪的地图点少于90%被跟踪的点%参考关键帧tooFewTrackedPoints = numel(mappointsinices) < 0.9 * numPointsRefKeyFrame;isKeyFrame = (tooManyNonKeyFrames && toofewtrackpoints) || 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;%更新世界位置的每个地图点基于新的绝对姿态%相应的主要观点为i = 1: mapPointSet。Count majorViewIds = directionAndDepth.MajorViewId(i);tform = posesOld (majorViewIds)。T \一直陷于(majorViewIds)。T;tform(1:3,1:3) + tform(4, 1:3);结束mapPointSet = updateWorldPoints(mapPointSet, indices, positionsNew);结束

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

函数gTruth = helperTransformGPSLocations(gpsLocations, optimizedpositions) initialYawGPS = atan((gpsLocations(100, 2) - gpsLocations(1,2)) /．..(gpsLocations(100, 1) - gpsLocations(1,1)));initialYawSLAM = atan((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:size(gpsLocations, 1) gTruth(i，:) = initialTform * gpsLocations(i，:)' + relTranslation';结束结束

参考

穆尔-阿塔尔、劳尔和胡安D. Tardós。ORB-SLAM2:一种用于单目、立体声和RGB-D相机的开源SLAM系统。机器人上的IEEE交易33岁的没有。5(2017): 1255 - 1262。

计算机视觉工具箱文档

万博1manbetx

与Matlab引入深度学习

与Matlab引入深度学习

下载电子书