baseDownloadURL=“https://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.tgz”；dataFolder=fullfile（tempdir，“tum_rgbd_数据集”, filesep);选择= weboptions (“超时”、正);tgzFileName = [dataFolder,“fr3_office.tgz”];folderxists =存在（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_house / rgb /']；imds=图像数据存储（imageFolder）；%检查第一个图像currFrameIdx=1；currI=readimage（imds，currFrameIdx）；himage=imshow（currI）；

%设置随机种子以获得重复性rng（0）；％创建摄像机对象以存储相机内部参数。数据集的本质可以在下面的页面找到:% https://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats%请注意，数据集中的图像已经不失真，因此%不需要指定失真系数。焦距=[535.4539.2]；以像素为单位principalPoint = [320.1, 247.6];以像素为单位imageSize = size(currI，[1 2]);以像素为单位内部=摄影机内部（焦距、主点、图像大小）；％检测和提取ORB功能scaleFactor = 1.2;numLevels = 8;[preFeatures, prePoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels);currFrameIdx = currFrameIdx + 1;firstI = currI;%保留第一帧isMapInitialized = false;%映射初始化循环而~IsMapiInitialized&&currFrameIdx％找到推定的功能匹配indexPairs = MatchFeatures（预成分，Currfeatures，“独特的”符合事实的...“最大比率”, 0.9,“匹配阈值”，40）;preMatchedPoints=prePoints（indexPairs（：，1），：）；currenmatchedpoints=currenpoints（indexPairs（：，2），：）；%如果没有找到足够的匹配，检查下一帧最小匹配数=100；如果尺寸（indexPairs，1）持续结束preMatchedPoints=prePoints（indexPairs（：，1），：）；currenmatchedpoints=currenpoints（indexPairs（：，2），：）；%计算单应性和评估重建[tformH，scoreH，inliersIdxH]=助手计算机断层摄影（预匹配点，当前匹配点）；％计算基本矩阵和评估重构[tformF，scoreF，inliersIdxF]=帮助者计算基础矩阵（preMatchedPoints，currenmatchedpoints）；%基于启发式选择模型比率=scoreH/（scoreH+scoreF）；比率reshold=0.45；如果比率>比率保留inlierTformIdx=inliersIdxH；tform=tformH；其他的inlierTformIdx=inliersIdxF；tform=tformF；结束%按比例计算相机位置。使用减少计算的％点inlierPrePoints = preMatchedPoints (inlierTformIdx);inlierCurrPoints = currMatchedPoints (inlierTformIdx);[relOrient, relLoc, validFraction] = relativeCameraPose(tform, intrinsics, validFraction)，...inlierPrePoints（1:2:end），inlierCurrPoints（1:2:end））；%如果没有找到足够的嵌线，移动到下一帧如果validFraction < 0.9 || numel(size(relOrient))==3持续结束%对两个视图进行三角剖分以获得三维地图点relPose=rigid3d（relOrient，relLoc）；最小视差=1；%度[isValid，xyzWorldPoints，inlierTriangulationIdx]=HelperTriangulatedTwoFrames(...rigid3d，ribled，inlierprepoints，InlierCurroports，Intlinsics，MinParalLAX）;如果~z~这是有效的持续结束%获取两个关键帧中特征的原始索引indexPairs=indexPairs（inlierTformIdx（inlierttriangulationidx），：）；IsMapiInitialized=true；disp(['Map被帧1和帧初始化'num2str (currFrameIdx-1)])结束％映射初始化循环

如果isMapInitialized关闭(himage.Parent.Parent);%关闭前面的数字显示匹配的特征hfeature = showMatchedFeatures(firstI, currI, prePoints(indexPairs(:，1))，...货币点数（指数（：，2）），'剪辑'）;其他的错误('无法初始化映射。'）结束

%创建空的imageviewset对象以存储关键帧vSetKeyFrames=图像视图集；%创建一个空的worldpointset对象来存储3d地图点mapPointSet = worldpointset;创建一个helperViewDirectionAndDepth对象来存储视图的方向和深度directionAndDepth=helperViewDirectionAndDepth（大小（xyzWorldPoints，1））；％添加第一关键帧。将相机与第一个相关联沿z轴定向的原点处的％键帧preViewId = 1;vSetKeyFrames = addView(vSetKeyFrames, preViewId, rigid3d，“积分”，介词，...“特征”，preFeatures.Features）；％添加第二个关键帧currViewId=2；vSetKeyFrames=addView（vSetKeyFrames，currViewId，relPose，“积分”，currPoints，...“特征”特征,特征,；%在第一个关键帧和第二个关键帧之间添加连接vSetKeyFrames=addConnection（vSetKeyFrames、preViewId、currViewId、relPose、，“匹配”，indexPairs）；%添加三维地图点[mapPointSet，newPointIdx]=addWorldPoints（mapPointSet，xyzWorldPoints）；增加地图点的观察值preLocations = prePoints.Location;currLocations = currPoints.Location;预分频= prePoints.Scale;currScales = currPoints.Scale;%添加与第一个关键帧中的贴图点对应的图像点mapPointSet=addrecordrences（mapPointSet，preViewId，newPointIdx，indexPairs（：，1））；%添加与第二关键帧中的贴图点对应的图像点mapPointSet=AddRespondences（mapPointSet、currViewId、newPointIdx、indexPairs（：，2））；

%在前两个关键帧上运行全束调整轨迹=findTracks（vSetKeyFrames）；cameraPoses=姿势（vSetKeyFrames）；[refinedPoints，refinedAbsPoses]=捆绑调整（xyzWorldPoints，tracks，...摄影机，内在的，“FixedViewIDs”1....“PointsUnderated”符合事实的'absolutetolerance'，1e-7，...'relativetolerance'，1e-15，“最大迭代”，50）;%使用地图点的中值深度缩放地图和相机姿态medianDepth=中间值（向量范数（精细点）。；精细点=精细点/medianDepth；精细点位。绝对姿势（currViewId）。转换=...definedAbsposes.AbsolutePose（currViewId）.Translation/medianDepth；relPose.Translation=relPose.Translation/medianDepth；%使用优化的姿势更新关键帧vsetKeyFrames = UpdateView（vsetKeyFrames，Refiningabsposs）;vsetKeyFrames = UpdateConnection（VSetKeyFrames，PreviewID，CurviewID，有用）;％更新使用精细位置的地图点mapPointSet=updateWorldPoints（mapPointSet、newPointIdx、refinedPoints）；％更新视图方向和深度directionAndDepth=update（directionAndDepth，mapPointSet，vSetKeyFrames.Views，newPointIdx，true）；％可视化当前帧中的匹配功能关闭（hfeature.Parent.Parent）；featurePlot=HelpServiceSualizedMatchedFeatures（currI，currPoints（indexPairs（：，2））；

%可视化初始地图点和摄影机轨迹mapPlot=helperVisualizeMotionAndStructure（vSetKeyFrames，mapPointSet）；%展示传奇显示图例（地图）；

%当前关键帧的视图IDcurrKeyFrameId = currViewId;最后一个关键框架的％视角LastKeyFrameID = CurviewID;具有最多共可见的参考关键帧的% ViewId%映射当前关键帧的点refKeyFrameId=currViewId；%输入图像序列中最后一个关键帧的索引lastKeyFrameIdx=currFrameIdx-1；%输入图像序列中所有关键帧的索引addedFramesIdx=[1；LastKeyframeId]；isLoopClosed=false；

%主循环而~isLoopClosed && currFrameIdx < numel(imds. files) currI = readimage(imds, currFrameIdx);[currFeatures, currPoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels);%跟踪最后一个关键帧%mapPointsIdx：在当前帧中观察到的贴图点的索引%featureIdx：中相应特征点的索引%当前帧[currPose, mapPointsIdx, featureIdx] = helperTrackLastKeyFrame(mapPointSet，...vsetkeyframes.views，currfeatures，curpoints，lastKeyframeID，内在，Scalefactor）;%跟踪本地地图%refKeyFrameId：具有最大%与当前帧共可见的贴图点%LocalKeyframeId：当前帧的已连接关键帧的视图ID[refKeyFrameId、LocalKeyframeId、currPose、mapPointsIdx、featureIdx]=...helperTrackLocalMap(mapPointSet, directionAndDepth, vSetKeyFrames, mapPointsIdx，...featureIdx、currPose、currFeatures、currPoints、Intrinsic、scaleFactor、NumLevel）；%检查当前帧是否是关键帧。%如果满足以下两个条件，则帧为关键帧：％%1.自最后一个关键帧或%当前帧跟踪的贴图点少于100个% 2. 当前帧跟踪的贴图点少于当前帧的90%%参照关键帧跟踪的点ISKeyFrame = HelperisKeyFrame（Mappointset，RefKeyFrameID，LastKeyFrameIdx，...currFrameIdx，mapPointsIdx）；%可视化匹配的特征updatePlot (featurePlot、currI currPoints (featureIdx));如果~isKeyFrame currFrameIdx=currFrameIdx+1；持续结束%更新当前关键帧IDcurrKeyFrameId=currKeyFrameId+1；

%添加新的关键帧[mapPointSet，vSetKeyFrames]=helperAddNewKeyFrame（mapPointSet，vSetKeyFrames，...currform, currFeatures, currPoints, mapPointsIdx, featureIdx, localKeyFrameIds);%删除在关键帧内观察到的离群点[mapPointSet，Direction and Depth，mapPointsIdx]=helperCullRecentMapPoints（mapPointSet，...directionAndDepth、mapPointsIdx newPointIdx);%通过三角剖分创建新地图点minNumMatches=20；minParallax=3；[mapPointSet，vSetKeyFrames，newPointIdx]=helperCreateNewMapPoints（mapPointSet，vSetKeyFrames，...CurrkeyFrameID，内在，Scalefactor，Minnummatches，MinParallax）;％更新视图方向和深度directionAndDepth=更新（directionAndDepth、mapPointSet、vSetKeyFrames.Views、，...[mapPointsIdx；newPointIdx]，true）；%局部束平差[mappointset，directionanddepth，vsetkeyframes，newpointidx] = helperlocalbundleadjustment（...映射点集、方向和深度、VSET关键帧、，...CurrkeyFrameID，内在函数，纽波特）;%可视化三维世界点和摄影机轨迹updatePlot（mapPlot、vSetKeyFrames、mapPointSet）；

％初始化循环关闭数据库如果currKeyFrameId = = 3%加载脱机创建的特征数据包bofData=负载('bagoffeaturesdata.mat'）;％初始化地点识别数据库loopCandidates=[1；2]；loopDatabase=indexImages（子集（imds，loopCandidates），bofData.bof）；%创建一些关键帧后检查循环闭合埃尔塞夫电流大于20%循环边的最小特征匹配数loopEdgeNumMatches=50；％检测可能的循环闭合密钥框架候选[isDetected, validLoopCandidates] = helperCheckLoopClosure(vSetKeyFrames, currKeyFrameId，...loopDatabase、currI、loop候选者、loopEdgeNumMatches）；如果检测到%添加循环闭包连接[isLoopClosed，mapPointSet，vSetKeyFrames]=helperAddLoopConnections(...mapPointSet, vSetKeyFrames, validLoopCandidates, currKeyFrameId，...currFeatures、currPoints、LoopEdgeEnumMatches）；结束结束%如果没有检测到循环闭包，将映像添加到数据库中如果~isLoopClosed addImages(loopDatabase，子集(imds, currFrameIdx))，“冗长”、假);loopCandidates = [loopCandidates;currKeyFrameId];% #好< AGROW >结束％更新ID和索引lastKeyFrameId=currKeyFrameId；lastKeyFrameIdx=currFrameIdx；addedFramesIdx=[addedFramesIdx；currFrameIdx]；% #好< AGROW >currFrameIdx=currFrameIdx+1；结束%主回路末端

%优化姿势minnummatches = 30;[vsetkeyframesoptim，posescales] =优化（vsetkeyframes，minnummatches，“宽容”，1e-16）；%在优化姿态后更新地图点mapPointSet=helperUpdateGlobalMap（mapPointSet，方向和深度，...vSetKeyFrames、vSetKeyFramesOptim、PoseCales）；更新绘图（mapPlot、vSetKeyFrames、mapPointSet）；%绘制优化的摄像机轨迹optimizedPoses=姿势（VSETKeyframeSeOptim）；绘图优化目标（mapPlot，optimizedPoses）％更新图例显示图例（地图）；

%装载地面真相gTruthData =负载(“orbslamGroundTruth.mat”)；gTruth=gTruthData.gTruth；绘制实际的摄像机轨迹plotActualTrajectory（mapPlot、gTruth（addedFramesIdx）、optimizedPoses）；%展示传奇显示图例（地图）；

%评估跟踪精度帮助估计轨迹误差（gTruth（ADDEFRAMESIDX），优化操作系统；

函数[功能，validPoints] = helperDetectAndExtractFeatures(Irgb，...Scalefactor，Numlevels，Varargin）Numpoints = 1000;%在这个例子中，图像已经是未变形的。在一般%工作流，取消注释以下代码，以消除扭曲的图像。％% if nargin > 3%内在性=varargin{1}；％ 结尾%Irgb=未失真图像（Irgb，内在）；%检测球体特征Igray=im2gray（Irgb）；点=检测器特征（Igray，“ScaleFactor”，scaleFactor，“NumLevels”, numLevels);%选择均匀分布在整个图像中的特征子集点=选择均匀（点、点数、大小（Igray，1:2））；％提取特征[特征，有效点]=提取特征（Igray，点）；结束

函数[H，得分，inliersIndex]=helperComputeHomography（匹配点1，匹配点2）[H，inliersLogicalIndex]=estimateGeometricTransform2D(...matchedPoints1 matchedPoints2,“投射的”那...“MaxNumTrials”，1e3，'maxdistance'4.“信心”, 90);inlierPoints1 = matchedPoints1 (inliersLogicalIndex);inlierPoints2 = matchedPoints2 (inliersLogicalIndex);inliersIndex =找到(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)) +...总和（最大值（异常阈值-error2in1，0））；结束

函数[F，得分，内部索引]=helperComputeFundamentalMatrix（匹配点1，匹配点2）[F，内部逻辑索引]=estimateFundamentalMatrix(...matchedPoints1 matchedPoints2,“方法”那“兰萨克”那...“NumTrials”，1e3，“距离阈值”，0.01）；inlierPoints1=matchedPoints1（inliersLogicIndex）；inlierPoints2=matchedPoints2（inliersLogicIndex）；inliersIndex=find（inliersLogicIndex）；locations1=inlierPoints1.Location；locations2=inlierPoints2.Location；点到极线的距离lineIn1=极线（F'，位置s2）；error2in1=（和（[locations1，one（大小（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..../总和（第2行（：，1:2）。^2，2）；异常阈值=4；分数=总和（最大值（异常阈值-error1in2，0））+...总和（最大值（异常阈值-error2in1，0））；结束

函数[ISVALID，XYZPOINTS，INLILIDX] = HelpertriangulateTwoframe（...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和RepoumeRingerrors %具有显著视差的良好双视点ray1 = xyzPoints - pose1.Translation;ray2 = xyzPoints - pose2.Translation;cosAngle = sum(ray1 .* ray2) ./ (vecnorm(ray1, 2,2) .* vecnorm(ray2, 2,2));%检查视差isValid = all(cosAngle < cosd(minParallax) & cosAngle>0);结束

函数isKeyFrame=helperIsKeyFrame（贴图点，...numPointsRefKeyFrame = nummel (findWorldPointsInView(mapPoints, refKeyFrameId));%从上次插入关键帧到现在已超过20帧tooManyNonKeyFrames = currFrameIndex > lastKeyFrameIndex + 20;％轨道少于100个地图点tooFewMapPoints=numel（映射点sindices）<100；%被跟踪的地图点少于90%被跟踪的点%参考关键帧tooFewTrackedPoints=numel（贴图点细分）<0.9*numPointsRefKeyFrame；isKeyFrame=（tooManyNonKeyFrames | | tooFewMapPoints）&&tooFewTrackedPoints；结束

函数[mapPointSet, directionAndDepth, mapPointsIdx] = helperCullRecentMapPoints(mapPointSet, directionAndDepth, mapPointsIdx, newPointIdx) outlierIdx = setdiff(newPointIdx, mapPointsIdx);如果〜isempty（outlieridx）mappointet = removeworldpoints（mappointset，outlieridx）;directionanddepth =删除（directionanddepth，outlieridx）;mappointsidx = mappointsidx  -  arrayfun（@（x）nnz（x> outlieridx），mappointsidx）;结束结束

函数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=locations*scale；rmse=sqrt（均值（总和（（scaledLocations，gLocations，2，2））；disp(['关键帧轨迹的绝对RMSE（m）：'num2str (rmse)]);结束

函数[mapPointSet，Direction and Depth]=helperUpdateGlobalMap(...mappointset，directionanddepth，vsetkeyframes，vsetkeyframesoptim，posescales）%helperUpdateGlobalMap在姿势图优化后更新贴图点posesOld = vSetKeyFrames.Views.AbsolutePose;一直陷于= vSetKeyFramesOptim.Views.AbsolutePose;positionsOld = mapPointSet.WorldPoints;positionsNew = positionsOld;指数= 1:mapPointSet.Count;%更新每个地图点的世界位置基于新的绝对姿态%相应的主要观点为i=1:mapPointSet.Count MajorViewId=Direction and Depth.MajorViewId（i）；poseNew=poseNew（MajorViewId）.T；poseNew（1:3,1:3）=poseNew（1:3,1:3）*PoseCales（MajorViewId）；tform=posesOld（MajorViewId）.T\poseNew；positionsNew（i，：）=PositionSeld（i，：）*tform（1:3,1:3）+tform（4,1:3）；结束mapPointSet=updateWorldPoints（mapPointSet、索引、位置新建）；结束