视觉SLAM与RGB-D相机
视觉同步定位和映射(vSLAM)，是指计算相机相对于周围环境的位置和方向，同时映射环境的过程。
您可以使用单目摄像机执行vSLAM。然而，深度无法精确计算，估计的轨迹是未知的，并随时间漂移。要生成一个无法从第一帧开始进行三角测量的初始地图，您必须使用单目相机的多个视图。更好、更可靠的解决方案是使用RGB- d相机，它由一张RGB彩色图像和一张深度图像组成。
本例展示了如何处理RGB-D图像数据来构建室内环境的地图并估计摄像机的轨迹。该示例使用ORB-SLAM2[1]算法的一个版本，该算法基于特征，支持RGB-D相机。金宝app
加工管道概述
RGB-D vSLAM的管道与单目vSLAM管道非常相似单目视觉同步定位与作图的例子。主要的区别在于地图初始化阶段，由由一幅彩色图像和一幅深度图像组成的一对图像代替两帧彩色图像创建三维地图点。
地图初始化:从彩色图像中提取ORB特征点，然后从深度图像中计算其三维世界位置，构建初始三维世界点。彩色图像被存储为第一个关键帧。
跟踪:地图初始化后，通过将彩色图像中的特征与最后一帧关键帧中的特征进行匹配，来估计每一幅新的RGB-D图像的相机姿态。
当地的地图:如果当前彩色图像被识别为关键帧，则从深度图像计算新的三维映射点。在这个阶段，通过调整相机姿势和3-D点，使用束调整来最小化重投影误差。
循环关闭:通过使用特征袋方法将每个关键帧与之前的所有关键帧进行比较来检测循环。一旦检测到循环闭合，姿态图将被优化以优化所有关键帧的相机姿态。
下载并浏览输入图像序列
本例中使用的数据来自TUM RGB-D基准[２]．您可以使用web浏览器或运行以下代码将数据下载到临时文件夹:
baseDownloadURL =“https://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.tgz”；dataFolder = fullfile(tempdir，“tum_rgbd_dataset”, filesep);选项= weboptions(“超时”、正);tgzFileName =[数据文件夹，“fr3_office.tgz”];folderExists = exist(数据文件夹“dir”）;在临时目录中创建一个文件夹来保存下载的文件如果~ folderExists mkdir (dataFolder);disp (“下载fr3_office。tgz (1.38 GB)。下载过程可能需要几分钟。”) websave(tgzFileName, baseDownloadURL，选项);提取下载文件的内容disp (“提取fr3_office。tgz (1.38 GB)…') untar(tgzFileName, dataFolder);结束imageFolder =[数据文件夹，“rgbd_dataset_freiburg3_long_office_household /”];
创建两个imageDatastore对象分别存储颜色和深度图像。
imgFolderColor = [imageFolder，“rgb /”];imgFolderDepth = [imageFolder，“深度/”];imdsColor = imageDatastore(imgFolderColor);imdsDepth = imageDatastore(imgFolderDepth);
请注意，颜色和深度图像是在数据集中以非同步的方式生成的。因此，我们需要根据时间戳将彩色图像与深度图像关联起来。
加载彩色图像的时间戳数据timeColor = helperImportTimestampFile([imageFolder，]“rgb.txt”]);加载深度图像的时间戳数据timeDepth = helperImportTimestampFile([imageFolder，]“depth.txt”]);对齐时间戳indexPairs = helperAlignTimestamp(timeColor, timeDepth);%选择已同步的镜像数据imdsColor =子集(imdsColor, indexPairs(:， 1));imdsDepth =子集(imdsDepth, indexPairs(:， 2));检查第一个RGB-D图像currFrameIdx = 1;currIcolor = readimage(imdsColor, currFrameIdx);currIdepth = readimage(imdsDepth, currFrameIdx);imshowpair (currIcolor currIdepth,“蒙太奇”）;
地图初始化
管道首先初始化包含3-D世界点的地图。这一步至关重要，对最终SLAM结果的准确性有重大影响。从第一张彩色图像中提取初始ORB特征点helperDetectAndExtractFeatures．利用特征点的像素坐标和深度值，可以计算出它们对应的三维世界位置helperReconstructFromRGBD．
为重现性设置随机种子rng (0);创建一个cameraIntrinsics对象来存储相机的内在参数。数据集的intrinsic可以在下面的页面中找到:% https://vision.in.tum.de/data/datasets/rgbd-dataset/file_formatsfocalLength = [535.4, 539.2];%，以像素为单位principalPoint = [320.1, 247.6];%，以像素为单位imageSize = size(currIcolor，[1,2]);%(像素)[mrows, ncols]depthFactor = 5e3;intrinsic = cameraIntrinsics(focalLength,principalPoint,imageSize);从彩色图像中检测并提取ORB特征scaleFactor = 1.2;numLevels = 8;[currFeatures, currPoints] = helperDetectAndExtractFeatures(currIcolor, scaleFactor, numLevels);initialPose = rigidtform3d();[xyzPoints, validIndex] = helperReconstructFromRGBD(currPoints, currIdepth, intrinsic, initialPose, depthFactor);
初始化位置识别数据库
循环检测是使用单词袋方法执行的。表示为的可视化词汇表bagOfFeatures对象离线创建，使用从数据集中大量图像中提取的ORB描述符，通过调用:
bag = bagfeatures (imds,CustomExtractor=@helperORBFeatureExtractorFunction, TreeProperties=[5,10]，最强特征=1);
在哪里洛桑国际管理发展学院是一个imageDatastore对象存储训练图像和helperORBFeatureExtractorFunction为ORB特征提取器函数。看到图像检索与袋的视觉词获取更多信息。
循环关闭过程以增量方式构建一个数据库，表示为invertedImageIndex对象，该对象存储基于ORB特性包的可视单词到图像映射。
加载离线创建的特性数据包bofData = load(“bagOfFeaturesDataSLAM.mat”）;初始化位置识别数据库loopDatabase = invertedImageIndex(bofData. index)转炉,SaveFeatureLocations = false);将第一个关键帧的特征添加到数据库中currKeyFrameId = 1;addmagefeatures (loopDatabase, currFeatures, currKeyFrameId);
数据管理与可视化
在使用第一对颜色和深度图像初始化映射后，可以使用imageviewset而且worldpointset存储第一个关键帧和对应的映射点:
创建一个空的imageviewset对象来存储关键帧vSetKeyFrames =图像视图;创建一个空的worldpointset对象来存储三维地图点mapPointSet = worldpointset;添加第一个关键帧vSetKeyFrames = addView(vSetKeyFrames, currKeyFrameId, initialPose, Points=currPoints，.．.特点= currFeatures.Features);添加3-D地图点[mappointment set, rgbdmappointment sidx] = addWorldPoints(mappointment set, xyzPoints);添加地图点的观测值mappointment set = addcorresponences (mappointment set, currKeyFrameId, rgbdmappointment sidx, validIndex);更新视图方向和深度mapetset = updateLimitsAndDirection(mapetset, rgbdmapetsidx, vSetKeyFrames.Views);更新代表视图mapotset = updateRepresentativeView(mapotset, rgbdmapotsidx, vSetKeyFrames.Views);在第一个关键帧中可视化匹配的特征featurePlot = helperVisualizeMatchedFeaturesRGBD(currIcolor, currIdepth, currPoints(validIndex));可视化初始地图点和相机轨迹xLim = [-4 4];yLim = [-3 1];zLim = [-1 6];mapPlot = helperVisualizeMotionAndStructure(vSetKeyFrames, mapPointSet, xLim, yLim, zLim);%显示图例showLegend (mapPlot);
跟踪
跟踪过程使用每个RGB-D图像执行，并确定何时插入新的关键帧。
最后一个关键帧的% ViewIdlastKeyFrameId = currKeyFrameId;输入图像序列中最后一个关键帧的索引lastKeyFrameIdx = currFrameIdx;%输入图像序列中所有关键帧的索引addedFramesIdx = lastKeyFrameIdx;currFrameIdx = 2;isLoopClosed = false;
每帧处理如下:
为每个新的彩色图像提取ORB特征，然后进行匹配(使用
matchFeatures)，最后一个关键帧的特征已经知道对应的三维地图点。估计相机姿势使用Perspective-n-Point算法，在给定一组3-D点和它们对应的2-D投影的情况下，使用
estworldpose．给定相机姿态，将最后一个关键帧观察到的地图点投影到当前帧，并使用搜索特征对应
matchFeaturesInRadius．在当前帧中使用3-D到2-D对应，通过执行仅运动的束调整来细化相机姿态
bundleAdjustmentMotion．将本地映射点投影到当前帧中，以搜索使用的更多特征对应
matchFeaturesInRadius并再次使用改进相机姿势bundleAdjustmentMotion．跟踪的最后一步是决定当前帧是否应该是一个新的关键帧。当满足以下两个条件时，一个帧就是关键帧:
从上一个关键帧开始，至少已经经过了20帧，或者当前帧跟踪的映射点少于100个，或者是参考关键帧跟踪的映射点的25%。
当前帧跟踪的映射点小于参考关键帧跟踪点的90%。
如果当前帧即将成为关键帧，请继续执行当地的地图的过程。否则,开始跟踪对于下一帧。
%主循环isLastFrameKeyFrame = true;而~isLoopClosed && currFrameIdx < numel(imdsColor. files) currIcolor = readimage(imdsColor, currFrameIdx);currIdepth = readimage(imdsDepth, currFrameIdx);[currFeatures, currPoints] = helperDetectAndExtractFeatures(currIcolor, scaleFactor, numLevels);跟踪最后一个关键帧% trackedMapPointsIdx:当前左帧中观察到的映射点的索引% trackedFeatureIdx:当前左帧中对应特征点的索引[currPose, trackedMapPointsIdx, trackedFeatureIdx] = helperTrackLastKeyFrame(mapPointSet, trackedmfeatureidx).．.vSetKeyFrames。Views, currFeatures, currPoints, lastKeyFrameId, intrinsic, scaleFactor);如果isempty(currPose) || number (trackedMapPointsIdx) < 30 currFrameIdx = currFrameIdx + 1;继续结束跟踪本地地图，检查当前帧是否是关键帧。如果满足以下两个条件，一个帧就是关键帧:％% 1。自最后一个关键帧或%当前帧跟踪少于100个地图点。% 2。当前帧跟踪的地图点小于90%由参考关键帧跟踪的%点。％localKeyFrameIds:当前帧中连接的关键帧的ViewIdnumSkipFrames = 20;numPointsKeyFrame = 100;[localKeyFrameIds, currPose, trackedMapPointsIdx, trackedFeatureIdx, isKeyFrame] =.．.helptracklocalmap (mappointment set, vSetKeyFrames, trackedmappointment sidx，.．.trackedFeatureIdx, currPose, currFeatures, currPoints, intrinsic, scaleFactor, numLevels，.．.isLastFrameKeyFrame, lastKeyFrameIdx, currFrameIdx, numSkipFrames, numPointsKeyFrame);%可视化匹配的特征updatePlot(featurePlot, currIcolor, currIdepth, currPoints(trackedFeatureIdx));如果~isKeyFrame currFrameIdx = currFrameIdx + 1;isLastFrameKeyFrame = false;继续其他的在立体图像之间匹配特征点，得到三维世界的位置[xyzPoints, validIndex] = helperReconstructFromRGBD(currPoints, currIdepth，.．.intrinsic, currPose, depthFactor);[untrackedFeatureIdx, ia] = setdiff(validIndex, trackedFeatureIdx);xyzPoints = xyzPoints(ia，:);isLastFrameKeyFrame = true;结束更新当前关键帧IDcurrKeyFrameId = currKeyFrameId + 1
当地的地图
对每个关键帧进行局部映射。当确定新的关键帧时，将其添加到关键帧中，并更新新关键帧所观察到的映射点的属性。为了确保mapPointSet包含尽可能少的异常值，一个有效的映射点必须在至少3个关键帧中观察到。
通过对当前关键帧及其连接的关键帧中的ORB特征点进行三角划分来创建新的映射点。对于当前关键帧中的每个不匹配的特征点，使用搜索与连接的关键帧中的其他不匹配的特征点匹配matchFeatures．局部束调整细化当前关键帧的姿态，连接关键帧的姿态，以及在这些关键帧中观察到的所有地图点。
添加新的关键帧[mapetset, vSetKeyFrames] = helperAddNewKeyFrame(mapetset, vSetKeyFrames，.．.currPose, currFeatures, currPoints, trackedMapPointsIdx, trackedFeatureIdx, localKeyFrameIds);移除在少于3个关键帧内观察到的离群地图点如果currKeyFrameId == 2 triangulatedMapPointsIdx = [];结束[mappointment set, trackedmappointment sidx] =.．.helperCullRecentMapPoints(mappointment set, trackedmappointment sidx, triangulatedmappointment sidx，.．.rgbdMapPointsIdx);从视差中添加新的地图点[mappointment set, rgbdmappointment sidx] = addWorldPoints(mappointment set, xyzPoints);mapapetset = addcorresponences (mapapetset, currKeyFrameId, rgbdmapapetsidx，.．.untrackedFeatureIdx);通过三角测量创建新的地图点minNumMatches = 10;minParallax = 0.35;[mappointment set, vSetKeyFrames, triangulatedmappointment sidx, rgbdmappointment sidx] = helpercreatenewmappointment stereo (.．.mapPointSet, vSetKeyFrames, currKeyFrameId, intrinsic, scaleFactor, minNumMatches, minParallax，.．.untrackedFeatureIdx rgbdMapPointsIdx);更新视图方向和深度mappointment set = updateLimitsAndDirection(mappointment set， [triangulatedmappointment sidx;rgbdMapPointsIdx),.．.vSetKeyFrames.Views);更新代表视图mappointment set = updateRepresentativeView(mappointment set， [triangulatedmappointment sidx;rgbdMapPointsIdx),.．.vSetKeyFrames.Views);%本地bundle调整[mappointment set, vSetKeyFrames, triangulatedmappointment sidx, rgbdMapPointsIdx] =.．.helperLocalBundleAdjustmentStereo (mapPointSet vSetKeyFrames,.．.currKeyFrameId, intrinsic, triangulatedmappointment sidx, rgbdmappointment sidx);可视化3-D世界点和相机轨迹updatePlot(mapPlot, vSetKeyFrames, mapotset);
循环关闭
环路闭合检测步骤取本地映射进程处理过的当前关键帧，尝试检测并闭合环路。通过在数据库中查询与当前关键帧在视觉上相似的图像来识别循环候选evaluateImageRetrieval．如果一个候选关键帧没有连接到最后一个关键帧，并且它的三个邻居关键帧是循环候选关键帧，那么这个候选关键帧是有效的。
当找到一个有效的循环候选时，使用estgeotform3d计算循环候选帧和当前关键帧之间的相对姿态。相对位姿表示存储在对象中的三维刚性变换rigidtform3d对象。然后添加相对姿态和更新的循环连接mapPointSet而且vSetKeyFrames．
在一些关键帧创建后检查循环是否关闭如果currKeyFrameId > 20循环边特征匹配的最小个数loopEdgeNumMatches = 120;检测可能的闭环关键帧候选[isDetected, validLoopCandidates] = helperCheckLoopClosure(vSetKeyFrames, currKeyFrameId，.．.loopDatabase, currIcolor, loopEdgeNumMatches);如果isDetected添加循环关闭连接maxDistance = 0.1;[isLoopClosed, mapPointSet, vSetKeyFrames] = helperAddLoopConnectionsStereo(.．.mapPointSet, vSetKeyFrames, validloopcandidate, currKeyFrameId，.．.currFeatures, currPoints, loopEdgeNumMatches, maxDistance);结束结束%如果未检测到循环关闭，则将当前特性添加到数据库中如果~isLoopClosed addImageFeatures(loopDatabase, currFeatures, currKeyFrameId);结束更新id和索引lastKeyFrameId = currKeyFrameId;lastKeyFrameIdx = currFrameIdx;addedFramesIdx = [addedFramesIdx;currFrameIdx];% #好< AGROW >currFrameIdx = currFrameIdx + 1;结束主循环结束
循环边缘添加在关键帧:3和87之间
最后，在基本图上应用姿态图优化vSetKeyFrames修正漂移。基本图是通过删除小于的连接在内部创建的minNumMatches在共视度图中匹配。姿态图优化后，使用优化后的姿态更新地图点的三维位置。
%优化姿势minNumMatches = 50;vSetKeyFramesOptim = optimizepose (vSetKeyFrames, minNumMatches, Tolerance=1e-16);优化姿势后更新地图点mapetset = helperUpdateGlobalMap(mapetset, vSetKeyFrames, vSetKeyFramesOptim);updatePlot(mapPlot, vSetKeyFrames, mapotset);绘制优化后的相机轨迹optimizedposed = pose (vSetKeyFramesOptim);plotOptimizedTrajectory (mapPlot optimizedPoses)%更新图例showLegend (mapPlot);
与地面真相比较
您可以将优化后的相机轨迹与地面真相进行比较，以评估精度。下载的数据包含groundtruth.txt文件，存储每一帧相机姿态的地面真相。数据以mat文件的形式保存。您还可以计算轨迹估计的均方根误差(RMSE)。
%负载地面真实值gTruthData = load(“orbslamGroundTruth.mat”）;gTruth = gTruthData.gTruth;绘制实际相机轨迹plotActualTrajectory(mapPlot, gTruth(indexPairs(addedFramesIdx, 1))， optimizedpositions);%显示图例showLegend (mapPlot);
评估跟踪精度helperEstimateTrajectoryError(gTruth(indexPairs(addedFramesIdx, 1))， optimizedpose);
关键帧轨迹的绝对RMSE (m): 0.17171
深度图像的密集重建
给定经过细化的相机姿态，可以将相关深度图像中的所有有效图像点重新投影回3-D空间，以执行密集重建。
创建一个pointCloud对象数组来存储所构造的世界点%从关键帧ptClouds = repmat(pointCloud(零(1,3))，数字(addedFramesIdx)， 1);忽略边界处的图像点偏移量= 40;[X, Y] = meshgrid(offset:2:imageSize(2)-offset, offset:2:imageSize(1)-offset);为i = 1: numel(addedFramesIdx) Icolor = readimage(imdsColor, addedFramesIdx(i));Idepth = readimage(imdsDepth, addedFramesIdx(i));[xyzPoints, validIndex] = helperReconstructFromRGBD([X(:))， Y(:)]，.．.Idepth, intrinsic, optimizedPoses.AbsolutePose(i)， depthFactor);颜色= 0(数字(X)， 1，“喜欢”, Icolor);为j = 1:元素个数(X)颜色(j, 1:3) = Icolor (Y (j) X (j):);结束ptClouds(i) = pointCloud(xyzPoints, Color=colors(validIndex，:));结束连接点云pointCloudsAll = pccat(ptClouds);图pcshow (pointCloudsAll VerticalAxis =“y”VerticalAxisDir =“向下”）;包含(“X”) ylabel (“Y”) zlabel (“Z”）
金宝app支持功能
下面列出了简短的帮助函数。较大的函数包含在单独的文件中。
helperImportTimestampFile导入时间戳文件
函数timestamp = helperImportTimestampFile(filename)%输入处理dataLines = [4, Inf];设置导入选项并导入数据opts = delimitedTextImportOptions(“NumVariables”2);指定范围和分隔符选择。DataLines = DataLines;选择。分隔符=”“；指定列名和类型选择。VariableNames = [“VarName1”，“Var2”];选择。SelectedVariableNames =“VarName1”；选择。VariableTypes = [“替身”，“字符串”];指定文件级属性选择。ExtraColumnsRule =“忽略”；选择。EmptyLineRule =“读”；选择。ConsecutiveDelimitersRule =“加入”；选择。LeadingDelimitersRule =“忽略”；指定可变属性Opts = setvaropts(选项，“Var2”，“WhitespaceRule”，“保存”）;Opts = setvaropts(选项，“Var2”，“EmptyFieldRule”，“汽车”）;导入数据数据= readtable(文件名，opts);转换为输出类型Timestamp = table2array(data);结束
helperAlignTimestamp对齐颜色和深度图像的时间戳。
函数indexPairs = helperAlignTimestamp(timeColor, timeDepth) idxDepth = 1;indexPairs = 0 (numel(timeColor)， 2);为i = 1:numel(timeColor)为j = idxDepth: numel(timeDepth)如果abs(timeColor(i) - timeDepth(j)) < 1e-4 idxDepth = j;indexPairs(i，:) = [i, j];打破elseiftimeDepth(j) - timeColor(i) > 1e-3打破结束结束结束indexPairs = indexPairs(indexPairs(:，1)>0，:);结束
helperDetectAndExtractFeatures从图像中检测并提取和ORB特征。
函数[features, validPoints] = helperDetectAndExtractFeatures(Irgb, scaleFactor, numLevels) numPoints = 1000;检测ORB特征灰度= rgb2gray(Irgb);points = detectORBFeatures(灰色，ScaleFactor= ScaleFactor, NumLevels= NumLevels);选择特征的子集，均匀分布在整个图像中points = selectUniform(points, numPoints, size(灰色，1:2));提取特征[features, validPoints] = extractFeatures(灰色，点);结束
helperReconstructFromRGBD从颜色和深度图像重建场景。
函数[xyzPoints, validIndex] = helperReconstructFromRGBD(points, validIndex).．.depthMap, intrinsic, currPose, depthFactor) ptcloud = pcfromdepth(depthMap,depthFactor,intrinsic,ImagePoints=points, DepthRange=[0.1, 5]);isPointValid = ~isnan(ptcloud. txt)位置(:1));xyzPoints = ptcloud。位置(isPointValid:);xyzPoints = transformPointsForward(currPose, xyzPoints);validIndex = find(isPointValid);结束
helperCullRecentMapPointsCull最近增加了地图点。
函数[mappointment set, mappointment sidx] =.．.helperCullRecentMapPoints(mappointment set, mappointment sidx, newPointIdx, rgbdMapPointsIndices) outlierIdx = setdiff([newPointIdx;rgbdMapPointsIndices], mapPointsIdx);如果~isempty(outlierIdx) mappointment set = removeWorldPoints(mappointment set, outlierIdx);mappointment sidx = mappointment sidx - arrayfun(@(x) nnz(x>outlierIdx)， mappointment sidx);结束结束
helperEstimateTrajectoryError计算跟踪误差。
函数rmse = helperEstimateTrajectoryError(gTruth, cameraPoses) locations = vertcat(cameraPoses. absolutepose . translation);gLocations = vertcat(gTruth.Translation);scale = median(vecnorm(gLocations, 2,2))/ median(vecnorm(locations, 2,2));scaledLocations =位置*规模;rmse = sqrt(mean(sum((scaledLocations - gLocations)。^2, 2)));disp ([关键帧轨迹的绝对RMSE (m):num2str (rmse)]);结束
helperUpdateGlobalMap姿态图优化后更新地图点的三维位置
函数mapPointSet = helperUpdateGlobalMap(mapPointSet, vSetKeyFrames, vSetKeyFramesOptim) posesOld = vSetKeyFrames. views . absolutepose;posesNew = vSetKeyFramesOptim.Views.AbsolutePose;positionsOld = mappointment . worldpoints;positionsNew = positionsOld;指数= 1:mappointment . count;根据的新绝对姿态更新每个地图点的世界位置%对应的主要视图为i = 1: mapPointSet。计数majorViewIds = mappointment . representativeviewid (i);tform = rigidtform3d(posesNew(majorViewIds).A/posesOld(majorViewIds).A);positionsNew(i，:) = transformPointsForward(tform, positionsOld(i，:));结束mappointment set = updateWorldPoints(mappointment set, indexes, posisnew);结束
参考文献
[1]穆尔·阿塔尔，劳尔·胡安·D. Tardós。ORB-SLAM2:用于单眼、立体声和RGB-D相机的开源SLAM系统。IEEE机器人汇刊33岁的没有。5(2017): 1255-1262。
您也可以从以下列表中选择一个网站: