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 = [dataFolder,“fr3_office.tgz”];folderExists =存在(dataFolder,“dir”）;在临时目录中创建一个文件夹来保存下载的文件。如果~ folderExists mkdir (dataFolder);disp (“下载fr3_office。tgz (1.38 GB)。下载可能需要几分钟。”) websave(tgzFileName, baseDownloadURL, options);提取下载文件的内容disp (“提取fr3_office。tgz (1.38 GB)…”)解压(tgzFileName dataFolder);结束

imageFolder = [dataFolder,“rgbd_dataset_freiburg3_long_office_household / rgb /”];imd = imageDatastore (imageFolder);检查第一张图片currFrameIdx = 1;currI = readimage(imds, currFrameIdx);himage = imshow (currI);

为再现性设置随机种子rng (0);创建一个cameraIntrinsics对象来存储相机的内部参数。数据集的intrinsic可以在以下页面中找到:% https://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats注意，数据集中的图像已经是未失真的，因此%不需要指定失真系数。focalLength = [535.4, 539.2];%，以像素为单位principalPoint = [320.1, 247.6];%，以像素为单位imageSize = size(currI，[1 2]);%，以像素为单位intrinsic = cameraIntrinsics(focalLength, principalPoint, imageSize);检测和提取ORB特征scaleFactor = 1.2;numLevels = 8;numPoints = 1000;[preFeatures, prePoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels, numPoints);currFrameIdx = currFrameIdx + 1;firstI = currI;%保留第一帧isMapInitialized = false;%映射初始化循环而~isMapInitialized && currFrameIdx < numel(imds. files) currI = readimage(imds, currFrameIdx);[currFeatures, currPoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels, numPoints);currFrameIdx = currFrameIdx + 1;查找假定的特征匹配indexPairs = matchFeatures(preFeatures, currFeatures, Unique=true，.．.MaxRatio = 0.9, MatchThreshold = 40);preMatchedPoints = prePoints (indexPairs (: 1):);currMatchedPoints = currPoints (indexPairs (:, 2):);%如果没有找到足够的匹配，检查下一帧minMatches = 100;如果size(indexPairs, 1) < minMatches . size继续结束preMatchedPoints = prePoints (indexPairs (: 1):);currMatchedPoints = currPoints (indexPairs (:, 2):);计算单应性和评估重建。[tformH, score, inliersIdxH] = helperComputeHomography(preMatchedPoints, currMatchedPoints);计算基本矩阵和评估重建[tformF, scoreF, inliersIdxF] = helperComputeFundamentalMatrix(preMatchedPoints, currMatchedPoints);选择基于启发式的模型ratio = score /(score + scoreF);ratioThreshold = 0.45;如果ratio > ratioThreshold inliertmagazx = inliersIdxH;tform = tformH;其他的inlierTformIdx = inliersIdxF;tform = tformF;结束%按比例计算相机位置。使用一半的%点以减少计算inlierPrePoints = preMatchedPoints (inlierTformIdx);inlierCurrPoints = currMatchedPoints (inlierTformIdx);[relPose, validFraction] = estrelpose(tform, intrinsic，).．.inlierPrePoints(1:2)结束,inlierCurrPoints(1:2:结束));如果没有发现足够的inliers，移动到下一帧。如果validFraction < 0.9 || numel(relPose)==3继续结束对两个视图进行三角定位以获得三维地图点。minParallax = 1;%的度[isValid, xyzWorldPoints, inlierTriangulationIdx] = helperTriangulateTwoFrames(.．.rigidtform3d, relPose, inlierPrePoints, inlierCurrPoints, intrinsic, minParallax);如果~ isValid继续结束获取两个关键帧中特征的原始索引。indexPairs = indexPairs (inlierTformIdx (inlierTriangulationIdx):);isMapInitialized = true;disp (['用第1帧和第1帧初始化的地图'num2str (currFrameIdx-1)])结束映射初始化循环结束

如果isMapInitialized关闭(himage.Parent.Parent);关闭上一个图形%显示匹配的特征hfeature = showMatchedFeatures(firstI, currI, prePoints(indexPairs(:，1))，.．.currPoints (indexPairs (:, 2)),“蒙太奇”）;其他的错误(“无法初始化地图。”）结束

创建一个空的imageviewset对象来存储关键帧vSetKeyFrames = imageviewset;创建一个空的worldpointset对象来存储3-D地图点mapPointSet = worldpointset;添加第一个关键帧。将相机与第一个相关联%关键帧在原点，沿z轴方向preViewId = 1;vSetKeyFrames = addView(vSetKeyFrames, preViewId, rigidtform3d, Points=prePoints，.．.特点= preFeatures.Features);添加第二个关键帧currViewId = 2;vSetKeyFrames = addView(vSetKeyFrames, currViewId, relPose, Points=currPoints，.．.特点= currFeatures.Features);在第一个和第二个关键帧之间添加连接vSetKeyFrames = addConnection(vSetKeyFrames, preViewId, currViewId, relPose, Matches=indexPairs);添加3d地图点[mapPointSet, newPointIdx] = addWorldPoints(mapPointSet, xyzWorldPoints);添加观察地图点preLocations = prePoints.Location;currLocations = currPoints.Location;预分频= prePoints.Scale;currScales = currPoints.Scale;在第一个关键帧中添加对应的贴图点mapPointSet = addmappings (mapPointSet, preViewId, newPointIdx, indexPairs(:，1));在第二关键帧中添加对应的贴图点。mapPointSet = addmappings (mapPointSet, currViewId, newPointIdx, indexPairs(:，2));

加载离线创建的特性数据包。bofData =负载(“bagOfFeaturesDataSLAM.mat”）;初始化地点识别数据库loopDatabase = invertedImageIndex (bofData.bof SaveFeatureLocations = false);将前两个关键帧的特征添加到数据库中。addImageFeatures (loopDatabase preFeatures preViewId);addImageFeatures (loopDatabase currFeatures currViewId);

在前两个关键帧上运行完整的bundle调整跟踪= findTracks (vSetKeyFrames);cameraPoses =姿势(vSetKeyFrames);[refinedPoints, refinedAbsPoses] = bundleadjustadjustment (xyzWorldPoints, tracks，.．.cameraPoses, intrinsic FixedViewIDs = 1,.．.PointsUndistorted = true, AbsoluteTolerance = 1 e -,.．.RelativeTolerance = 1 e15汽油,MaxIteration = 20,.．.解算器=“preconditioned-conjugate-gradient”）;缩放地图和相机姿势使用地图点的中位深度medianDepth =值(vecnorm (refinedPoints。');refinedPoints = refinedPoints / medianDepth;refinedAbsPoses.AbsolutePose (currViewId)。翻译=.．.refinedAbsPoses.AbsolutePose (currViewId)。翻译/ medianDepth;relPose。翻译= relPose.Translation / medianDepth;%更新关键帧与细化的姿势vSetKeyFrames = updateView(vSetKeyFrames, refinedAbsPoses);vSetKeyFrames = updateConnection(vSetKeyFrames, preViewId, currViewId, relPose);更新地图点与细化的位置mappointment = updateWorldPoints(mappointment, newPointIdx, refinedPoints);更新视图方向和深度mapPointSet = updateLimitsAndDirection(mapPointSet, newPointIdx, vSetKeyFrames.Views);%更新代表视图mapPointSet = updateRepresentativeView(mapPointSet, newPointIdx, vSetKeyFrames.Views);在当前帧中可视化匹配的特征关闭(hfeature.Parent.Parent);featurePlot = helperVisualizeMatchedFeatures(currI, currPoints(indexPairs(:，2)));

可视化初始地图点和相机轨迹mapPlot = helperVisualizeMotionAndStructure(vSetKeyFrames, mapptset);%显示传奇showLegend (mapPlot);

当前关键帧的% ViewIdcurrKeyFrameId = currViewId;最后一个关键帧的ViewIdlastKeyFrameId = currViewId;输入图像序列中最后一个关键帧的索引lastKeyFrameIdx = currFrameIdx - 1;%输入图像序列中所有关键帧的索引addedFramesIdx = [1;lastKeyFrameIdx];isLoopClosed = false;

%主循环isLastFrameKeyFrame = true;而~isLoopClosed && currFrameIdx < numel(imds. files) currI = readimage(imds, currFrameIdx);[currFeatures, currPoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels, numPoints);跟踪最后一个关键帧% mapPointsIdx:当前帧中观察到的映射点的索引的对应特征点的索引%当前帧[currPose, mappetsidx, featureIdx] = helperTrackLastKeyFrame(mappetset，.．.vSetKeyFrames。视图，currFeatures, currPoints, lastKeyFrameId, intrinsic, scaleFactor);跟踪本地地图，检查当前帧是否是关键帧。%满足以下两个条件的帧是关键帧:％% 1。从上一个关键帧到现在至少已经过去了20帧%当前帧跟踪少于100个地图点。% 2。当前帧跟踪的地图点小于90%由参考关键帧跟踪的%点。％跟踪性能对numPointsKeyFrame的值很敏感。%如果跟踪丢失，请尝试更大的值。％% localKeyFrameIds:当前帧连接的关键帧的ViewIdnumSkipFrames = 20;numPointsKeyFrame = 80;[localKeyFrameIds, currPose, mapptsidx, featureIdx, isKeyFrame] =.．.helperTrackLocalMap (mapPointSet vSetKeyFrames mapPointsIdx,.．.featureIdx, currPose, currFeatures, currPoints, intrinsic, scaleFactor, numLevels，.．.isLastFrameKeyFrame, lastKeyFrameIdx, currFrameIdx, numSkipFrames, numPointsKeyFrame);%可视化匹配的特征updatePlot (featurePlot、currI currPoints (featureIdx));如果~isKeyFrame currFrameIdx = currFrameIdx + 1;isLastFrameKeyFrame = false;继续其他的isLastFrameKeyFrame = true;结束更新当前关键帧IDcurrKeyFrameId = currKeyFrameId + 1;

添加新的关键帧[mappset, vSetKeyFrames] = helperAddNewKeyFrame(mappset, vSetKeyFrames，.．.currPose, currFeatures, currPoints, mapPointsIdx, featureIdx, localKeyFrameIds);移除在少于3个关键帧内观察到的离群图点mapPointSet = helperCullRecentMapPoints(mapPointSet, mapPointsIdx, newPointIdx);通过三角剖分创建新的地图点minNumMatches = 10;minParallax = 3;[mapPointSet, vSetKeyFrames, newPointIdx] = helperCreateNewMapPoints(mapPointSet, vSetKeyFrames，.．.currKeyFrameId, intrinsic, scaleFactor, minNumMatches, minParallax);%本地包调整[refinedViews, dist] = connectedViews(vSetKeyFrames, currKeyFrameId, MaxDistance=2);refinedKeyFrameIds = refinedViews.ViewId;fixedViewIds = refinedKeyFrameIds (dist = = 2);fixedViewIds = fixedViewIds(1:min(10, numel(fixedViewIds)));优化本地关键帧和地图点[mapPointSet, vSetKeyFrames, mappoint] = 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, mappoint dx, vSetKeyFrames.Views);%更新代表视图mappointment = updateRepresentativeView(mappointment, mappointment, vSetKeyFrames.Views);可视化3D世界点和相机轨迹updatePlot (mapPlot vSetKeyFrames mapPointSet);

在创建一些关键帧后检查循环关闭如果currKeyFrameId > 20循环边缘特征匹配的最小个数loopEdgeNumMatches = 50;检测可能的闭环关键帧候选帧[isDetected, validloopcandidate] = helperCheckLoopClosure(vSetKeyFrames, currKeyFrameId，.．.loopDatabase、currI loopEdgeNumMatches);如果isDetected添加环路闭合连接[isLoopClosed, mapPointSet, vSetKeyFrames] = helperAddLoopConnections(.．.， validloopcandidate, currKeyFrameId，.．.currFeatures loopEdgeNumMatches);结束结束%如果未检测到循环关闭，则将当前特性添加到数据库中如果~isLoopClosed addImageFeatures(loopDatabase, currFeatures, currKeyFrameId);结束更新id和索引lastKeyFrameId = currKeyFrameId;lastKeyFrameIdx = currFrameIdx;addedFramesIdx = [addedFramesIdx;currFrameIdx];% #好< AGROW >currFrameIdx = currFrameIdx + 1;结束主循环结束

%优化姿势minNumMatches = 20;[vSetKeyFramesOptim, poseScales] = optimizepose (vSetKeyFrames, minNumMatches, Tolerance=1e-16);优化姿势后更新地图点mappset = helperUpdateGlobalMap(mappset, vSetKeyFrames, vSetKeyFramesOptim, poseScales);updatePlot (mapPlot vSetKeyFrames mapPointSet);绘制优化后的相机轨迹optimizedPoses =姿势(vSetKeyFramesOptim);plotOptimizedTrajectory (mapPlot optimizedPoses)%更新传奇showLegend (mapPlot);

%加载地面真相gTruthData =负载(“orbslamGroundTruth.mat”）;gTruth = gTruthData.gTruth;绘制实际的相机轨迹plotActualTrajectory (mapPlot gTruth (addedFramesIdx) optimizedPoses);%显示传奇showLegend (mapPlot);

%评估跟踪精度helperEstimateTrajectoryError (gTruth (addedFramesIdx) optimizedPoses);

函数[features, validPoints] = helperDetectAndExtractFeatures(Irgb，.．.scaleFactor, numLevels, numPoints, varargin)在本例中，图像已经未失真。。在一般%工作流，取消注释以下代码以恢复图像失真。％% if nargin > 4% intrinsic = varargin{1};%结束% Irgb = undistortion timage (Irgb, intrinsic);%检测ORB功能Igray = im2gray (Irgb);points = detectORBFeatures(灰度，ScaleFactor= ScaleFactor, NumLevels= NumLevels);选择特征的子集，均匀分布在整个图像。points = selectUniform(points, numPoints, size(灰度，1:2));%提取特征[features, validPoints] = extractFeatures(Igray, points);结束

函数[H, score, inliersIndex] = helperComputeHomography(matchedPoints1, matchedPoints2) [H, inliersloicalindex] = estgeotform2d(.．.matchedPoints1 matchedPoints2,“射影”，.．.MaxNumTrials = 1 e3, MaxDistance = 4,信心= 90);inlierPoints1 = matchedPoints1 (inliersLogicalIndex);inlierPoints2 = matchedPoints2 (inliersLogicalIndex);inliersIndex =找到(inliersLogicalIndex);locations1 = inlierPoints1.Location;locations2 = inlierPoints2.Location;xy1In2 = transformPointsForward(H, location1);xy2In1 = transformPointsInverse(H, locations2);error1in2 = sum((locations2 - xy1In2)。2) ^ 2;error2in1 = sum((location1 - xy2In1)。2) ^ 2; outlierThreshold = 6; score = sum(max(outlierThreshold-error1in2, 0)) +.．.sum (max (outlierThreshold-error2in1, 0));结束

函数[F, score, inliersIndex] = helperComputeFundamentalMatrix(matchedPoints1, matchedPoints2) [F, inlierlogicalindex] = estimateFundamentalMatrix(.．.matchedPoints1 matchedPoints2方法=“RANSAC”，.．.NumTrials = 1 e3, DistanceThreshold = 0.01);inlierPoints1 = matchedPoints1 (inliersLogicalIndex);inlierPoints2 = matchedPoints2 (inliersLogicalIndex);inliersIndex =找到(inliersLogicalIndex);locations1 = inlierPoints1.Location;locations2 = inlierPoints2.Location;点到极线的距离lineIn1 = epipolarLine(F'， locations2);Error2in1 = (sum([locations1, ones(size(locations1, 1)，1)]。* lineIn1, 2)) ^ 2.．.。/笔(lineIn1(:, 1:2)。2) ^ 2;lineIn2 = epipolarLine(F, location1);Error1in2 = (sum([locations2, ones(size(locations2, 1)，1)].使用实例* lineIn2, 2)) ^ 2.．.。/笔(lineIn2(:, 1:2)。2) ^ 2;outlierThreshold = 4;score = sum(max(outlierThreshold-error1in2, 0)) +.．.sum (max (outlierThreshold-error2in1, 0));结束

函数[isValid, xyzPoints, inlierIdx] = helperTriangulateTwoFrames(.．.camMatrix1 = cameraProjection(intrinsic, pose2extr(pose1));camMatrix2 = cameraProjection(intrinsic, pose2extr(pose2));[xyzPoints, reprojectionErrors, isInFront] = triangulate(matchedPoints1, isInFront)，.．.matchedPoints2、camMatrix1 camMatrix2);%根据视图方向和重投影错误筛选点minReprojError = 1;inlierIdx = isInFront & reprojectionErrors < minReprojError;xyzPoints = xyzPoints(inlierIdx，:);一个具有显著视差的良好双视图ray1 = xyzPoints - pose1.Translation;ray2 = xyzPoints - pose2.Translation;cosAngle = sum(ray1 .* ray2, 2) ./ (vecnorm(ray1, 2,2) .* vecnorm(ray2, 2,2));%检查视差isValid = all(cosAngle < cosd(minParallax) & cosAngle>0);结束

函数mapPointSet = helperCullRecentMapPoints(mapPointSet, mapPointsIdx, newPointIdx) outlierIdx = setdiff(newPointIdx, mpointsidx);如果~isempty(outlierIdx) mapPointSet = removeWorldPoints(mapPointSet, outlierIdx);结束结束

函数rmse = helperestimatetrajectory (gTruth, cameraPoses) locations = vertcat(cameraPoses. absolutepose . translation);gLocations = vertcat (gTruth.Translation);scale = median(vecnorm(gLocations, 2,2))/ median(vecnorm(locations, 2,2));scaledLocations = locations * scale;rmse = sqrt(mean(sum((scaledLocations - gLocations))。^ 2, 2)));disp ([关键帧轨迹的绝对RMSE (m):num2str (rmse)]);结束

函数mapPointSet = helperUpdateGlobalMap (.．.mappset, vSetKeyFrames, vSetKeyFramesOptim, poseScales)%helperUpdateGlobalMap在位姿图优化后更新映射点posesOld = vSetKeyFrames.Views.AbsolutePose;一直陷于= vSetKeyFramesOptim.Views.AbsolutePose;positionsOld = mapPointSet.WorldPoints;positionsNew = positionsOld;指数= 1:mapPointSet.Count;更新每个地图点的世界位置基于新的绝对姿势。%对应的主要视图为i = indexes majorViewIds = mappointment . representativeviewid (i);poseNew =一直陷于(majorViewIds)。;poseNew(1:3, 1:3) = poseNew(1:3, 1:3) * poseScales(majorViewIds);tform = affinetform3d (poseNew / posesOld (majorViewIds)。);positionsNew(i，:) = transformPointsForward(tform, positionsOld(i，:));结束mappointment = updateWorldPoints(mappointment, indexes, positionsNew);结束