BrienceOdloadURL =.'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”];folderxists =存在（datafolder，“dir”）;％在临时目录中创建一个文件夹以保存下载的文件如果〜Folderexists Mkdir（DataFolder）;DISP（'下载fr3_office.tgz（1.38 GB）。这个下载可能需要几分钟。）Websave（TGZFileName，BrienceOdlognull，选项）;%提取下载文件的内容DISP（'提取fr3_office.tgz（1.38 GB）......'）Untar（TGzFilename，DataFolder）;结尾

imagefolder = [datafolder，'rgbd_dataset_freiburg3_long_office_house / rgb /'];imds = imageageataStore（imageFolder）;%检查第一个图像CurrframeIdx = 1;Curri = ReadImage（IMDS，CurrFrameIDX）;hemage = imshow（curli）;

%为重现性设置随机种子RNG（0）;％创建摄像机对象以存储相机内部参数。%可在以下页面找到数据集的内部函数：％https://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats.％介绍数据集中的图像已毫无变形，因此存在％不需要指定失真系数。focalLength = [535.4, 539.2];以像素为单位林城= [320.1,247.6];以像素为单位图像智能=尺寸（CURI，[1 2]）;以像素为单位内在=摄像石（FocalLength，Principalpoint，Imageize）;％检测和提取ORB功能[装饰，前视图] = HelperDetectAddExtractFeatures（Curli）;CurrFrameIDX = CurrFrameIDX + 1;Firsti = Curli;%保留第一帧ismapinitialized = false;％映射初始化循环虽然〜isMapInitialized && hasdata（IMDS）Curri = ReadImage（IMDS，CurrFrameIDX）;[Currfeatures，Currpoints] = HelperDetectAndExtractFeatures（Curli）;CurrFrameIDX = CurrFrameIDX + 1;％找到推定的功能匹配indexPairs = MatchFeatures（预成分，Currfeatures，'独特',真的,......'maxratio'，0.7，“MatchThreshold”, 70);％如果没有找到足够的匹配，请检查下一帧minmatches = 100;如果尺寸（indexpaire，1）继续结尾preMatchedPoints = prePoints (indexPairs (: 1):);currMatchedPoints = currPoints (indexPairs (:, 2):);％计算同众法和评估重建[tformh，scounth，Inliersidxh] = HelperComputeHomography（PrematchedPoints，CurrmatchedPoints）;％计算基本矩阵和评估重构[tformf，scoundf，Inliersidxf] =升迁普通邮奖amatrix（prematchedpoints，currmatchedpoints）;%基于启发式选择模型比率=记分/（记分+得分）;ratiothreshold = 0.45;如果比率> ratiothreshold InliertFormidx = InliersidXH;tform = tformh;别的InliertFormIdx = InliersidXF;tform = tformf;结尾%按比例计算相机位置。用一半的时间减少计算的％点InlierPrepoints = prematchedPoints（InliertFormIdx）;InlierCurropits = CurrMatchedPoints（InliertFormIdx）;[Relorient，Relloc，ValudFraction] = RelativeCamerapose（Tform，Intrinsics，......InlierPropoints（1：2：结束），InlierCurroports（1：2：结束））;％如果没有找到足够的inliers，则移动到下一个帧如果有效期<0.7 ||numel（尺寸（Relorient））== 3继续结尾％三角化两个视图以获得3-D映射点相关= rigid3d（Relorient，Relloc）;[IsValid，XYZWorldPoints，InliertriangulationIdX] = HelpertriangulateTwoframe（......rigid3d，有关，inlierprepoints，Inliercurrports，内在机构）;如果〜Isvalid.继续结尾％获得两个关键帧中的原始功能索引indexPairs = IndexPairs（InliertFormIdx（InliertriangulationIDX），:);ismapinitialized = true;DISP（['用框架1和框架初始化映射，num2str（currFrameIdx-1）]）结尾％映射初始化循环

如果ismapinitialized关闭（Hemage.Parent.Parent）;％关闭前一个数字%显示匹配的特征hfeature = ShowMatchedFeatures（第一，Curli，Prepoints（IndexPairs（：，1）），......FurrPoints（IndexPairs（:,2）），'剪辑'）;别的错误（“无法初始化映射。”）结尾

％创建一个空的ImageViewset对象以存储密钥帧vsetkeyframs = imageviewset;%创建一个空的helperMapPointSet对象来存储3D地图点mappointset = helpermappointset;％添加第一关键帧。将相机与第一个相关联沿z轴定向的原点处的％键帧priviewid = 1;vsetkeyframes = addview（vsetKeyFrames，PreviewID，Rigid3D，“点”，前景，......'特征'，装饰.Features）;％添加第二个关键帧currviewid = 2;vsetkeyframes = addview（vSetKeyFrames，curviewId，ripled，“点”，curports，......'特征'，currfeatures.features）;％在第一和第二关键帧之间添加连接vSetKeyFrames = addConnection(vSetKeyFrames, preViewId, currViewId, relPose，“火柴”，indexpairs）;％添加3-D映射点[mappointet，newpointidx] = addmappoint（mappointset，xyzworldpoints）;％添加地图点的观察前置位置=前置位置；currLocations=currPoints.Location；预标定=预标定刻度；currScales=currPoints.Scale；％添加与第一关键帧中的地图点对应的图像点mappointet = addobservation（mappointset，newpointidx，previewid，indexpaess（：1），......．prelations（IndexPairs（：，1），:)，预分析（IndexPairs（：，1）））;％添加与第二关键帧中的地图点对应的图像点mappointet = addobservation（mappointset，newpointidx，currviewid，indexPairs（:,2），......currLocations (indexPairs (: 2):), currScales (indexPairs (:, 2)));

%在前两个关键帧上运行完整的bundle调整曲目= findtracks（vsetkeyframs）;cameraposes =姿势（vsetkeyframes）;[RefiningPoints，Refiningabsposs] = BundleAdjustment（XYZWorldPoints，轨道，......Cameraposes，内在机构，'filedviewids'，1，......'obessundistorted',真的,'absolutetolerance'，1e-7，......'relativetolerance'，1e-15，'maxIteration'，50）;％缩放地图和相机姿势使用中值的地图点中位数=中位数（vecnorm（RefiningPoints。））;RefiningPoints = RefiningPoints /中年;Refiningabsposs.absolutepose（currviewid）.translation =......Refiningabsposs.absolutepose（currviewid）。翻译/中美型;resime.ranslation = resimed.ranslation /中学部;％更新带有精细姿势的关键帧vsetKeyFrames = UpdateView（vsetKeyFrames，Refiningabsposs）;vsetKeyFrames = UpdateConnection（VSetKeyFrames，PreviewID，CurviewID，有用）;％更新使用精细位置的地图点mappointset = UpdatElocation（MappointSet，RefiningPoints）;％更新视图方向和深度mappointet = updateViewandRange（Mappointset，vsetKeyFrames.views，newpointidx）;％可视化当前帧中的匹配功能关闭（hfeature.parent.parent）;Featureplot = HelpervisualizeMatchedFeatures（Curri，CurrPoints（IndexPairs（:,2）））;

％可视化初始映射点和相机轨迹mapplot = helpervisualizemotionandandanduredulture（vsetkeyframes，mappointset）;%显示传奇ShowLegend（Mapplot）;

当前关键帧的％视角currKeyFrameId=currViewId；最后一个关键框架的％视角LastKeyFrameID = CurviewId;%具有最多共可见的参考关键帧的ViewId%使用当前关键帧映射点RefKeyFrameID = CurviewID;输入图像序列中最后一个键帧的％索引lastKeyFrameIdx = currFrameIdx - 1;输入图像序列中所有关键帧的％索引AddatedFramesIdx = [1;lastkeyframeidx];isloopclosed = false;

%主回路虽然~isLoopClosed&&hasdata（imds）currI=readimage（imds，currFrameIdx）[currFeatures，currPoints]=helperDetectAndExtractFeatures（currI）；%跟踪最后一个关键帧％mappointsidx：在当前帧中观察到的地图点的索引％featuredx：相应特征点的指标％当前帧[currPose，mapPointsIdx，featureIdx]=helperTrackLastKeyFrame（mapPointSet，......vsetkeyframs.views，currfeatures，curpoints，lastKeyFrameID，内在机构）;%跟踪本地地图％refKeyFrameID：查看最多的参考密钥框架具有当前帧的％共同可见映射点％localKeyFrameID：查看当前帧的连接密钥帧的视角[refKeyFrameID，LocalKeyFrameID，施加，mappointsidx，featuredIdx] =......HelperTrackLocalMap（MappointSet，VSetKeyFrames，Mappointsidx，......Featureidx，施法，Curfeatures，Currpoints，内在函数）;%检查当前帧是否为关键帧。%满足以下两个条件的帧为关键帧:％％1.自上次关键框架或者以来已经通过了至少20帧％当前帧轨道轨道少于80个地图点％2.当前帧跟踪的地图点少于90％参考密钥框架跟踪的％点ISKeyFrame = HelperisKeyFrame（Mappointset，RefKeyFrameID，LastKeyFrameIdx，......currFrameIdx mapPointsIdx);％可视化匹配的功能updateplot（featureplot，curli，currpoints（featuredidx））;如果〜ISKeyFrame CurrFrameIDX = CurrFrameIDX + 1;继续结尾％更新当前密钥帧IDCurrkeyFrameID = CurrkeyFrameID + 1;

％添加新密钥框架[mappointset，vsetkeyframes] = helperaddnewkeyframe（mappointset，vsetkeyframes，......施归，Currfeatores，Curpopitts，Mappointsidx，FeaturedIDX，LocalKeyFrameID）;％更新视图方向和深度mappointet = updateViewandRange（Mappointset，vsetKeyFrames.views，mappointsidx）;％删除在少于3个关键框架中观察到的异常值地图点mapPointSet = helperCullRecentMapPoints(mapPointSet, vSetKeyFrames, newPointIdx);％通过三角测量创建新的地图点[mappointet，vsetkeyframes，newpointIdx] = helpercreateNewmappoints（mappointset，vsetkeyframes，......currKeyFrameId intrinsic);％本地捆绑调整[mapPointSet，vSetKeyFrames]=helperLocalBundleAdjustment（mapPointSet，vSetKeyFrames，......currKeyFrameId intrinsic);可视化3D世界点和摄像机轨迹updateplot（mapplot，vsetkeyframes，mappointset）;

％初始化循环关闭数据库如果CurrEid==3％加载脱机创建的功能数据bofdata = load（'bagoffeaturesdata.mat'）;LoopDatabase = InvertedImageIndex（Bofdata.bof）;LoopCandIdates = [1;2];在创建某些关键帧后％检查循环关闭eleesifcurrKeyFrameId > 20％检测可能的循环闭合密钥框架候选[Isdetected，ValidLoopCandIdates] = HelperCheckLoopClosure（vsetKeyFrames，CurrkeyFrameID，......LoopDatabase，Curli，LoopcandIdates）;如果isdetected.％添加循环闭合连接[isLoopClosed，mapPointSet，vSetKeyFrames]=helperAddLoopConnections(......mapPointSet、vSetKeyFrames、ValidLoop、，......CurrkeyFrameID，Currfeatures，Curpopits，内在机构）;结尾结尾％如果未检测到循环关闭，则将图像添加到数据库中如果~isLoopClosed currds=imageDatastore（imds.Files{currFrameIdx}）；addImages（循环数据库、currds、，'verbose'， 错误的）;LoopCandIdates = [LoopCandIdates;CurrkeyFrameID];％＃OK 结尾％更新ID和索引lastKeyFrameID = CurkeyFrameID;lastkeyframebidx = currframeidx;AddatedFramesIDX = [已添加FramesIdx;CurrframeIDX];％＃OK CurrFrameIDX = CurrFrameIDX + 1;结尾％循环的末端

优化姿势minnummatches = 40;vsetkeyframesoptim =优化邮件（vsetKeyFrames，minnummatches，“宽容”，1e-16，'verbose'， 真的）;

％绘制优化的相机轨迹优化=姿势（vsetkeyFramesoptim）;plotOptimizedTrajection（Mapplot，优化）％更新图例ShowLegend（Mapplot）;

％负荷地面真相GTR数据=负载('orbslamgroundtruth.mat'）;gtruth = gtruthdata.gtruth;%绘制实际的相机轨迹plotactualtrajectory（mapplot，gtruth（附加帧），优化）;%显示传奇ShowLegend（Mapplot）;

％评估跟踪准确性HelperestimateTrajectoryError（GTRUTH（已添加FRAMESIDX），优化）;

功能[功能，有效点] = HelperDetectAndExtractFeatures（IRGB，Varargin）Scalefactor = 1.2;numlevels = 8;Numpoints = 1000;%在本例中，图像已经不失真。总的来说%工作流中，取消注释以下代码以取消对图像的干扰。％%如果nargin>1％内在= varargin {1};％ 结尾％Irgb = untostortimage（Irgb，内在）;％检测ORB功能IGRAY = RGB2GRAY（IRGB）;点= detectorbfeatures（Igay，'比例因子'， 比例因子，'numlevels'，numlevels）;％选择一个功能子集，均匀分布在整个图像中点= SelectUniform（点，Numpoints，大小（Igray，1：2））;％提取特征[features, validPoints] = extractFeatures(Igray, points);结尾

功能[H，Score，InliersIndex] = HelperComputeHomography（MatchedPoints1，MatchedPoints2）[H，InLierpoints1，InLierpoints2] = eStimateGeometricTransform（......matchedpoints1，matchedpoints2，'投影'那......'maxnumtrial'1 e3,'maxdistance'，4，'置信度'，90）;[〜，InliersIndex] =相交（matchedpoints1.location，......Inlierpoints1.Location，'排'那'稳定的'）;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）;OuttiAlthreshold = 6;得分= sum（max（OutlierThreshold-error1in2,0））+......sum (max (outlierThreshold-error2in1, 0));结尾

功能[f，score，InliersIndex] = HelperComputeFundamentalMatrix（matchedPoints1，matchedpoints2）[f，InlierslogicalIndex] = estImationFundamentalMatrix（......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'，位置2）;ERROR2IN1 =（SUM（[LOCATIONS1，SUNATIONS1（LOCATIONS1,1），1）]。* LINEIN1,2））。^ 2......./总和（LineIn1（：，1：2）。^ 2,2）;linein2 = epipolarline（f，locations1）;ERROR1IN2 =（SUM（[LOCATIONS2，SUNATIONS2）]。* LINEIN2,2））。^ 2....../和（第2行（：，1:2）。^2，2）；异常阈值=4；分数=总和（最大值（离群值阈值-error1in2，0））+......sum (max (outlierThreshold-error2in1, 0));结尾

功能[ISVALID，XYZPOINTS，INLILIDX] = HelpertriangulateTwoframe（......pose1，pose2，matchedpoints1，matchedpoints2，内在函数）[r1，t1] = cameraposetoextrinsics（pose1.rotation，pose1.translation）;Cammatrix1 = Cameramatrix（内在机构，R1，T1）;[R2，T2] = CameraPoSetoExtrinsics（POSE2.ROTATION，POSE2.Translation）;Cammatrix2 = Cameramatrix（内在，R2，T2）;[XYZPOINTS，REPOURNEEREERRORS] =三角形（匹配点1，......MatchedPoints2，Cammatrix1，Cammatrix2）;通过查看方向和重注错误％过滤点minReprojError=1；inlierIdx=xyzPoints（：，3）>0&reprojectionErrors％一个很好的双视图，具有重要的视差ray1=xyzPoints-位置1.平移；ray2=xyzPoints-位置2.平移；余角=和（ray1.*ray2，2）。/（vecnorm（ray1，2，2）。*vecnorm（ray2，2））；％检查视差minparallax = 3;％以度为单位isValid=all（cosAngle0）；结尾

功能iskeyframe = helperiskeyframe（mappoints，......RefKeyFrameID，LastKeyFrameIndex，CurrframeIndex，MappointSindices）NumpointSrefKeyFrame = Numel（GetMappointIndex（MapPoints，RefKeyFrameID））;从上次键帧插入传递％超过20帧toomanynonkeyframes = currframeIndex> = lastKeyFrameIndex + 20;%跟踪少于80个地图点toofewmappinoints = numel（mappointsindices）<80;%跟踪的地图点少于用户跟踪的点的90%％参考密钥框架toofewtrackedpoints = numel（mappointsindices）<0.9 * numpointsrefkeyframe;ISKEYFRAME =（TOOMANYNONKEYFRAMES || TOOFEWMAPPOINTS）&& toofewtrackedpoints;结尾

功能mapPoints = helperCullRecentMapPoints(mapPoints, keyFrames, newPointIdx)为了i = 1：numel（newpointidx）idx = newpointidx（i）;％如果在少于3个关键框架中观察到地图点，请将其丢弃如果numel（mapPoints.Observations{idx，1}）<3&&......max（mappoints.observations {idx，1}）结尾结尾结尾

功能RMSE = HelperestimateTrajectoryError（GTRUTH，Camerapse）位置= VertCAT（Camerapose.absolutePose.Ranslation）;glocations = Vertcat（gtruth.translation）;Scale =中位数（Vecnorm（壁垒，2,2））/中位数（Vecnorm（位置，2,2））;ScaleDlocations =位置*比例;RMSE = SQRT（平均值（SUMEDLOCATIONS  - 绑持）。^ 2,2）））;DISP（['关键帧轨迹的绝对RMSE（m）：'，num2str（rmse）]；结尾