baseDownloadURL ='https://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.tgz'；dataFolder = fullfile (tempdir,'tum_rgbd_dataset', filesep);选择= weboptions (“超时”，INF）;tgzfilename = [datafolder，'fr3_office.tgz'];folderExists =存在(dataFolder,'dir'）;％在临时目录中创建一个文件夹以保存下载的文件如果~ folderExists mkdir (dataFolder);disp (“下载fr3_office。tgz (1.38 GB)。这次下载可能需要几分钟。”) websave(tgzFileName, baseDownloadURL，选项);％提取下载文件的内容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对象来存储相机的内部参数。％数据集的内在功能可以在下页找到：% https://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats%注意，数据集中的图像已经是未变形的，因此存在％不需要指定失真系数。焦点= [535.4,539.2];%，单位为像素principalPoint = [320.1, 247.6];%，单位为像素图像智能=尺寸（CURI，[1 2]）;%，单位为像素intrinsics = cameraIntrinsics(focalLength, principalPoint, imageSize);%检测和提取ORB特征scaleFactor = 1.2;numLevels = 8;[preFeatures, prePoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels);currFrameIdx = currFrameIdx + 1;firstI = currI;％保留第一帧isMapInitialized = false;% Map初始化循环尽管~isMapInitialized && currFrameIdx < numel(imds. files) currI = readimage(imds, currFrameIdx);[currFeatures, currPoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels);currFrameIdx = currFrameIdx + 1;找到假定的特征匹配indexPairs = matchFeatures(preFeatures, currFeatures，'独特'， 真的，．..“MaxRatio”, 0.9,'matchthreshold', 40);preMatchedPoints = prePoints (indexPairs (: 1):);currMatchedPoints = currPoints (indexPairs (:, 2):);%如果没有找到足够的匹配，检查下一帧minmatches = 100;如果尺寸（indexpaire，1）继续结束preMatchedPoints = prePoints (indexPairs (: 1):);currMatchedPoints = currPoints (indexPairs (:, 2):);计算单应性并评估重建[tformH, scoreH, inliersIdxH] = helperComputeHomography(preMatchedPoints, currMatchedPoints);计算基本矩阵和评估重建[tformF, scoreF, inliersIdxF] = helperComputeFundamentalMatrix(preMatchedPoints, currMatchedPoints);%基于启发式选择模型ratio = scoreH/(scoreH + scoreF);ratioThreshold = 0.45;如果ratio > ratioThreshold inliertorigx = inliersIdxH;tform = tformH;其他的inlierTformIdx = inliersIdxF;tform = tformF;结束%按比例计算摄像机位置。用一半%点数减少计算inlierPrePoints = preMatchedPoints (inlierTformIdx);inlierCurrPoints = currMatchedPoints (inlierTformIdx);[relOrient, relLoc, validFraction] = relativeCameraPose(tform, intrinsics, validFraction)，．..inlierPrePoints(1:2)结束,inlierCurrPoints(1:2:结束));%如果没有找到足够的嵌线，移动到下一帧如果有效期<0.9 ||numel（尺寸（Relorient））== 3继续结束对两个视图进行三角测量以获得三维地图点relPose = rigid3d(relOrient, relLoc);minParallax = 1;%的度[isValid, xyzWorldPoints, inlierTriangulationIdx] = helperTriangulateTwoFrames(．..rigid3d, relPose, inlierPrePoints, inlierCurrPoints, intrinsics, minParallax);如果~ isValid继续结束%获取两个关键帧中特征的原始索引indexPairs = indexPairs (inlierTformIdx (inlierTriangulationIdx):);isMapInitialized = true;disp (['Map被帧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;％创建一个升迁virectionanddepth对象以存储视图方向和深度directionAndDepth = helperViewDirectionAndDepth(size(xyzWorldPoints, 1));添加第一个关键帧。放置与第一个相关联的相机%关键帧在原点，沿z轴方向preViewId = 1;vSetKeyFrames = addView(vSetKeyFrames, preViewId, rigid3d，“点”prePoints,．..“特性”，装饰.Features）;添加第二个关键帧currViewId = 2;vSetKeyFrames = addView(vSetKeyFrames, currViewId, relPose，“点”currPoints,．..“特性”, currFeatures.Features);在第一和第二关键帧之间增加连接vsetkeyframs = addConnection（vsetKeyFrames，PreviewID，CurviewID，有关，'火柴', indexPairs);%添加3d地图点[mappointet，newpointidx] = addworldpoints（mappointset，xyzworldpoints）;增加地图点的观察值prelocations = prepoints.location;CRUCLOCations = CurrPoints.Location;prescales = prepoints.scale;currcales = currpoints.cale;在第一个关键帧中添加与地图点对应的图像点mapPointSet = addements (mapPointSet, preViewId, newPointIdx, indexPairs(:，1));%在第二个关键帧中添加与地图点对应的图像点mapPointSet = addements (mapPointSet, currViewId, newPointIdx, indexPairs(:，2));

％在前两个关键帧上运行完整捆绑调整跟踪= findTracks (vSetKeyFrames);cameraPoses =姿势(vSetKeyFrames);[refinedPoints, refinedAbsPoses] = bundleAdjustment(xyzWorldPoints, tracks，．..cameraPoses intrinsic,'filedviewids', 1．..“PointsUndistorted”， 真的，“AbsoluteTolerance”1 e -．..“RelativeTolerance”1 e15汽油,“MaxIteration”, 50);%使用地图点的中值深度缩放地图和相机姿态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);%更新地图点与精炼的位置mapPointSet = updateWorldPoints(mapPointSet, newPointIdx, refinedPoints);%更新视图方向和深度directionanddepth =更新（directionanddepth，mappointset，vsetkeyframes.views，newPointIdx, true);%在当前帧中可视化匹配的特征关闭(hfeature.Parent.Parent);featurePlot = helperVisualizeMatchedFeatures(currI, currPoints(indexPairs(:，2)));

％可视化初始映射点和相机轨迹mapPlot = helperVisualizeMotionAndStructure(vSetKeyFrames, mapPointSet);％显示传奇showLegend (mapPlot);

当前关键帧的% ViewIdCurrkeyFrameID = CurviewId;上一个关键帧的% ViewIdlastKeyFrameId = currViewId;具有最可观的参考密钥帧的％视角％映射点与当前关键框架refKeyFrameId = currViewId;%输入图像序列中最后一个关键帧的索引LastKeyFrameIdx = CurrFrameIDX  -  1;%输入图像序列中所有关键帧的索引addedFramesIdx = [1;lastKeyFrameIdx];isLoopClosed = false;

％主循环尽管~isLoopClosed && currFrameIdx < numel(imds. files) currI = readimage(imds, currFrameIdx);[currFeatures, currPoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels);％跟踪最后一个关键帧% mapPointsIdx:当前帧中观察到的映射点的索引% featureIdx:文件中相应特征点的索引%当前帧[省略mappointsidx，featureidx] = helpertracklastkeyframe（mappointset，．..vSetKeyFrames。视图，currFeatures, currPoints, lastKeyFrameId, intrinsics, scaleFactor);％跟踪本地地图% refKeyFrameId:引用关键帧的ViewId与当前帧共可见的地图点% localKeyFrameIds:当前帧连接的关键帧的ViewId[refKeyFrameId, localkeyframeid, currPose, mapPointsIdx, featureIdx] = .使用实例．..HelperTrackLocalMap（Mappointset，DirectionAndDepth，VsetKeyFrames，Mappointsidx，．..featureIdx, currPose, currFeatures, currPoints, intrinsics, scaleFactor, numLevels);％检查当前帧是否是关键帧。如果满足以下两个条件，％帧是关键帧：％% 1。从上一个关键帧到现在已经过去了至少20帧%当前帧跟踪少于100个地图点% 2。当前帧跟踪的地图点不到90%由参考关键帧跟踪的%点isKeyFrame = helperIsKeyFrame(mapPointSet, refKeyFrameId, lastKeyFrameIdx，．..currframeidx，mappointsidx）;可视化匹配特征updatePlot (featurePlot、currI currPoints (featureIdx));如果~isKeyFrame currFrameIdx = currFrameIdx + 1;继续结束%更新当前关键帧IDcurrKeyFrameId = currKeyFrameId + 1;

%添加新的关键帧[mappointment set, vSetKeyFrames] = helperAddNewKeyFrame(mappointment set, vSetKeyFrames，．..currform, currFeatures, currPoints, mapPointsIdx, featureIdx, localKeyFrameIds);%删除在关键帧内观察到的离群点[mapPointSet, directionAndDepth, mapPointsIdx] = helperCullRecentMapPoints(mapPointSet，．..directionanddepth，mappointsidx，newpointidx）;通过三角测量创建新的地图点minnummatches = 20;minparallax = 3;[mappointet，vsetkeyframes，newpointIdx] = helpercreateNewmappoints（mappointset，vsetkeyframes，．..currKeyFrameId, intrinsics, scaleFactor, minNumMatches, minParallax);%更新视图方向和深度directionanddepth =更新（directionanddepth，mappointset，vsetkeyframes.views，．..[mapPointsIdx;newPointIdx),真正的);局部束平差[mapPointSet, directionAndDepth, vSetKeyFrames, newPointIdx] = helperLocalBundleAdjustment(．..mapPointSet、directionAndDepth vSetKeyFrames,．..currKeyFrameId, intrinsic newPointIdx);％可视化3D世界点和相机轨迹updatePlot (mapPlot vSetKeyFrames mapPointSet);

初始化循环闭包数据库如果CurryFrameID == 3.%加载离线创建的特性数据包bofData =负载(“bagOfFeaturesData.mat”）;初始化位置识别数据库LoopCandIdates = [1;2];LoopDatabase = IndexImages（子集（IMDS，LoopCandIdates），BOFData.BOF）;%在一些关键帧被创建后检查循环关闭elseifCurrkeyFrameID> 20.%循环边的最小特征匹配数loopEdgeNumMatches = 50;%检测可能的循环关闭关键帧候选[Isdetected，ValidLoopCandIdates] = HelperCheckLoopClosure（vsetKeyFrames，CurrkeyFrameID，．..loopDatabase, currI, loopCandidates, loopEdgeNumMatches);如果isDetected%添加循环闭包连接[isLoopClosed, mapPointSet, vSetKeyFrames] = helperAddLoopConnections(．..mappointset，vsetkeyframes，validloopcandIdates，currkeyFrameID，．..currFeatures、currPoints loopEdgeNumMatches);结束结束％如果未检测到循环关闭，则将图像添加到数据库中如果~isLoopClosed addImages(loopDatabase，子集(imds, currFrameIdx))，“详细”、假);loopCandidates = [loopCandidates;currKeyFrameId];％＃OK 结束%更新id和索引lastKeyFrameId = currKeyFrameId;lastKeyFrameIdx = currFrameIdx;addedFramesIdx = [addedFramesIdx;currFrameIdx];％＃OK currFrameIdx = currFrameIdx + 1;结束%主循环结束

％优化姿势minNumMatches = 30;[vSetKeyFramesOptim, poseScales] = optimizePoses(vSetKeyFrames, minNumMatches，'宽容'1 e-16);%在优化姿态后更新地图点mappoint = helperUpdateGlobalMap(mappoint, directionAndDepth，．..vsetkeyframes，vsetkeyframesoptim，posescales）;updatePlot (mapPlot vSetKeyFrames mapPointSet);%绘制优化的摄像机轨迹optimizedPoses =姿势(vSetKeyFramesOptim);plotOptimizedTrajectory (mapPlot optimizedPoses)%更新传奇showLegend (mapPlot);

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

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

功能[特征，有效点] = HelperDetectAndExtractFeatures（IRGB，．..scaleFactor, numLevels, varargin)％在此示例中，图像已不变置。一般％工作流程，取消注释以下代码以贴变图像。％％如果nargin> 3% intrinsics = varargin{1};%结束% Irgb = undistortion timage (Irgb, intrinsics);%检测ORB功能Igray = im2gray (Irgb);点= detectORBFeatures (Igray,“ScaleFactor”scaleFactor,“NumLevels”, numLevels);%选择一个特征子集，均匀分布在整个图像中点= SelectUniform（点，Numpoints，大小（Igray，1：2））;%提取特征[功能，有效点] =提取物（IGRAI，点）;结束

功能[H, score, inlierindex] = helperComputeHomography(matchedPoints1, matchedPoints2) [H, inliersLogicalIndex] = estimateGeometricTransform2D(．..matchedPoints1 matchedPoints2,“射影”，．..“MaxNumTrials”，1e3，“MaxDistance”4'置信度'，90）;Inlierpoints1 = matchedpoints1（InlierslogicalIndex）;Inlierpoints2 = matchedpoints2（InlierslogicalIndex）;InliersIndex = find（InlierslogicalIndex）;Locations1 = Inlierpoints1.Location;locations2 = Inlierpoints2.Location;XY1in2 = TransformPointSforward（H，Locations1）;XY2IN1 = TransformPointsInverse（H，Locations2）;error1in2 = sum（（locations2  -  xy1in2）。^ 2,2）;ERROR2IN1 = SUM（（LOCATIONS1  -  XY2IN1）。^ 2,2）; outlierThreshold = 6; score = sum(max(outlierThreshold-error1in2, 0)) +．..SUM（MAX（OutlierThreshold-error2in1,0））;结束

功能[F, score, inlierindex] = helperComputeFundamentalMatrix(matchedpoint1, matchedpoint2) [F, inliersLogicalIndex] = estimateFundamentalMatrix(．..matchedPoints1 matchedPoints2,“方法”，“RANSAC”，．..'numtrials'，1e3，“DistanceThreshold”，0.01）;Inlierpoints1 = matchedpoints1（InlierslogicalIndex）;Inlierpoints2 = matchedpoints2（InlierslogicalIndex）;InliersIndex = find（InlierslogicalIndex）;Locations1 = Inlierpoints1.Location;locations2 = Inlierpoints2.Location;从点到ePipolar线的距离距离lineIn1 = epipolarLine(F'， locations2);Error2in1 = (sum([locations1, ones(size(locations1, 1)，1)])。* lineIn1, 2)) ^ 2．..。/笔(lineIn1(:, 1:2)。2) ^ 2;lineIn2 = epipolarLine(F, locations1);Error1in2 = (sum([locations2, ones(size(locations2, 1)，1)])。* lineIn2, 2)) ^ 2．..。/笔(lineIn2(:, 1:2)。2) ^ 2;outlierThreshold = 4;得分= sum（max（OutlierThreshold-error1in2,0））+．..SUM（MAX（OutlierThreshold-error2in1,0））;结束

功能[isValid, xyzPoints, inlierIdx] = helperTriangulateTwoFrames(．..pose1、pose2 matchedPoints1、matchedPoints2 intrinsic minParallax) (R1, t1) = cameraPoseToExtrinsics (pose1。旋转,pose1.Translation);camMatrix1 = cameraMatrix(intrinsics, R1, t1);[R2, t2] = cameraPoseToExtrinsics(pose2. p);旋转,pose2.Translation);camMatrix2 = cameraMatrix(intrinsics, R2, t2);[xyzPoints, reprojectionErrors, isInFront] = triangulate(matchedPoints1，．..matchedPoints2、camMatrix1 camMatrix2);%通过视图方向和重投影错误过滤点minReprojError = 1;inlierIdx = isInFront & reprojectionErrors < minReprojError;xyzPoints = xyzPoints(inlierIdx，:);%具有显著视差的良好双视点RAY1 = XYZPOINTS  -  POSE1.TRANSLATION;RAY2 = XYZPOINTS  -  POSE2.TRANSLATION;casangle = sum（ray1。* ray2,2）./（vecnorm（ray1,2,2）。* vecnorm（Ray2,2,2））;％检查视差IsValid =全部（Casangle  0）;结束

功能isKeyFrame = helperIsKeyFrame (mappoint,．..numPointsRefKeyFrame = nummel (findWorldPointsInView(mapPoints, refKeyFrameId));%从上次关键帧插入已经传递了超过20帧tooManyNonKeyFrames = currFrameIndex > lastKeyFrameIndex + 20;%追踪少于100个地图点tooFewMapPoints = nummel (mapPointsIndices) < 100;％追踪的地图点数不到90％的点%参考关键帧toofewtrackkedpoints = nummel (mapPointsIndices) < 0.9 * numPointsRefKeyFrame;isKeyFrame = (tooManyNonKeyFrames || tooFewMapPoints) && toofewtrackkedpoints;结束

功能[mappointset，directionanddepth，mappointsidx] = helpercullrecentmoppoints（mappointset，directionanddepth，mappointsidx，newpointidx）outlieridx = setdiff（newpointidx，mappointsidx）;如果~isempty(outlierIdx) mapPointSet = removeWorldPoints(mapPointSet, outlierIdx);directionAndDepth = remove(directionAndDepth, outlierIdx);mappointment sidx = mappointment sidx - arrayfun(@(x) nnz(x>outlierIdx)， mappointment sidx);结束结束

功能rmse = helperEstimateTrajectoryError(gTruth, cameraPoses) locations = vertcat(cameraPoses. absolutepose . translation);gLocations = vertcat (gTruth.Translation);scale =中位数(vecnorm(gLocations, 2,2))/中位数(vecnorm(locations, 2,2));scaledLocations =位置*比例;rmse = sqrt(mean(sum((scaledLocations - gLocations))。^ 2, 2)));disp ([关键帧轨迹的绝对RMSE (m):，num2str（RMSE）]）;结束

功能[mapPointSet, directionAndDepth] = helperUpdateGlobalMap(．..mapPointSet, directionAndDepth, vSetKeyFrames, vSetKeyFramesOptim, poseScales)%helperUpdateGlobalMap在姿态图优化后更新映射点poseold = vsetkeyframes.views.absolutepose;posenew = vsetkeyframesoptim.views.absolutepose;positysold = mappointet.worldpoints;positynew = positysold;indices = 1：mappointet.count;%更新每个地图点的世界位置基于新的绝对姿态%对应的主要观点为i = 1：mappointet.count majorviewIds = diventionanddepth.majorviewId（i）;posenew = posenew（majorviewids）.t;POSENEW（1：3,1：3）= POSENED（1：3,1：3）* PoseScales（MajorViewID）;tform = poseold（majorviewids）.t \ posenew;Positednew（I，：）= Positysold（I，:) * Tform（1：3,1：3）+ Tform（4,1：3）;结束mapPointSet = updateWorldPoints(mapPointSet, indices, positionsNew);结束