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，BrienceOdowLobleL，选项）;%提取下载文件的内容disp (“提取fr3_office。tgz (1.38 GB)…)解压(tgzFileName dataFolder);结束

imageFolder = [dataFolder,“rgbd_dataset_freiburg3_long_office_household / rgb /”];imds = imageageataStore（imagefolder）;%检查第一个图像currFrameIdx = 1;currI = readimage(imds, currFrameIdx);himage = imshow (currI);

%为重现性设置随机种子rng (0);%创建一个cameraIntrinsics对象来存储相机的内部参数。数据集的本质可以在下面的页面找到:% 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]);%，单位为像素intrinsics = cameraIntrinsics(focalLength, principalPoint, imageSize);%检测和提取ORB特征scaleFactor = 1.2;numLevels = 8;[preFeatures, prePoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels);currFrameIdx = currFrameIdx + 1;firstI = currI;%保留第一帧isMapInitialized = false;％映射初始化循环而〜IsmapInitialized && curframeidx 找到假定的特征匹配indexPairs = matchFeatures(preFeatures, currFeatures，“独特的”,真的,．..“MaxRatio”, 0.9,“MatchThreshold”, 40);preMatchedPoints = prePoints (indexPairs (: 1):);currMatchedPoints = currPoints (indexPairs (:, 2):);%如果没有找到足够的匹配，检查下一帧minMatches = 100;如果< minMatches . size(indexPairs, 1) < minMatches . size(indexPairs, 1继续结束preMatchedPoints = prePoints (indexPairs (: 1):);currMatchedPoints = currPoints (indexPairs (:, 2):);％计算同众法和评估重构[tformH, scoreH, inliersIdxH] = helperComputeHomography(preMatchedPoints, currMatchedPoints);计算基本矩阵和评估重建[tformf，scoundf，inliersidxf] = empercomputefundamentalmatrix（prematchedpoints，currmatchedpoints）;%基于启发式选择模型ratio = scoreH/(scoreH + scoreF);ratioThreshold = 0.45;如果比率> ratiothreshold inliertformidx = 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:结束));%如果没有找到足够的嵌线，移动到下一帧如果validFraction < 0.9 || numel(size(relOrient))==3继续结束对两个视图进行三角测量以获得三维地图点有关= 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对象来存储3d地图点mapPointSet = worldpointset;创建一个helperViewDirectionAndDepth对象来存储视图的方向和深度directionAndDepth = helperViewDirectionAndDepth(size(xyzWorldPoints, 1));添加第一个关键帧。放置与第一个相关联的相机%关键帧在原点，沿z轴方向preViewId = 1;vSetKeyFrames = addView(vSetKeyFrames, preViewId, rigid3d，“点”，prepoints，．..“特性”, preFeatures.Features);添加第二个关键帧currViewId = 2;vSetKeyFrames = addView(vSetKeyFrames, currViewId, relPose，“点”currPoints,．..“特性”, currFeatures.Features);％在第一和第二键帧之间添加连接vSetKeyFrames = addConnection(vSetKeyFrames, preViewId, currViewId, relPose，“匹配”，indexpairs）;％添加3-D映射点[mapPointSet, newPointIdx] = addWorldPoints(mapPointSet, xyzWorldPoints);增加地图点的观察值preLocations = prePoints.Location;currLocations = currPoints.Location;预分频= prePoints.Scale;currScales = currPoints.Scale;在第一个关键帧中添加与地图点对应的图像点mapPointSet = addements (mapPointSet, preViewId, newPointIdx, indexPairs(:，1));%在第二个关键帧中添加与地图点对应的图像点mappointset = AddCorreskents（Mappointset，CurrViewID，NewpointIdx，IndexPairs（:,2））;

%在前两个关键帧上运行完整的bundle调整曲目= findtracks（vsetkeyframs）;cameraposes = pose（vsetkeyframes）;[RefiningPoints，Refiningabsposs] = BundleAdjustment（XYZWorldPoints，轨道，．..Cameraposes，内在，“FixedViewIDs”, 1．..“PointsUndistorted”,真的,“AbsoluteTolerance”，1e-7，．..“RelativeTolerance”1 e15汽油,“MaxIteration”, 50);%使用地图点的中值深度缩放地图和相机姿态medianDepth =值(vecnorm (refinedPoints。');refinedPoints = refinedPoints / medianDepth;refinedAbsPoses.AbsolutePose (currViewId)。翻译=．..Refiningabsposs.absolutepose（currviewid）。翻译/中美长期;resime.Translation = resimed.ranslation /中介;％更新带有精细姿势的关键帧vSetKeyFrames = updateView(vSetKeyFrames, refinedAbsPoses);vSetKeyFrames = updateConnection(vSetKeyFrames, preViewId, currViewId, relPose);%更新地图点与精炼的位置mapPointSet = updateWorldPoints(mapPointSet, newPointIdx, refinedPoints);%更新视图方向和深度directionAndDepth = update(directionAndDepth, mapPointSet, vSetKeyFrames.)视图、newPointIdx真实);%在当前帧中可视化匹配的特征关闭(hfeature.Parent.Parent);featurePlot = helperVisualizeMatchedFeatures(currI, currPoints(indexPairs(:，2)));

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

当前关键帧的% ViewIdcurrKeyFrameId = currViewId;上一个关键帧的% ViewIdlastKeyFrameId = currViewId;具有最多共可见的参考关键帧的% ViewId%映射当前关键帧的点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:文件中相应特征点的索引%当前帧[currPose, 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;

%添加新的关键帧[mappointet，vsetkeyframes] = helperaddnewkeyframe（mappointset，vsetkeyframes，．..currform, currFeatures, currPoints, mapPointsIdx, featureIdx, localKeyFrameIds);%删除在关键帧内观察到的离群点[mapPointSet, directionAndDepth, mapPointsIdx] = helperCullRecentMapPoints(mapPointSet，．..directionAndDepth、mapPointsIdx newPointIdx);通过三角测量创建新的地图点minNumMatches = 20;minParallax = 3;[mapPointSet, vSetKeyFrames, newPointIdx] = helperCreateNewMapPoints(mapPointSet, vSetKeyFrames, newPointIdx)．..currKeyFrameId, intrinsics, scaleFactor, minNumMatches, minParallax);%更新视图方向和深度directionAndDepth = update(directionAndDepth, mapPointSet, vSetKeyFrames.)的观点,．..[mapPointsIdx;newPointIdx),真正的);局部束平差[mapPointSet, directionAndDepth, vSetKeyFrames, newPointIdx] = helperLocalBundleAdjustment(．..mapPointSet、directionAndDepth vSetKeyFrames,．..currKeyFrameId, intrinsic newPointIdx);可视化3D世界点和摄像机轨迹updateplot（mapplot，vsetkeyframes，mappointset）;

初始化循环闭包数据库如果currKeyFrameId = = 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，CurPoints，LoopedGenummatches）;结束结束%如果没有检测到循环闭包，将映像添加到数据库中如果~isLoopClosed addImages(loopDatabase，子集(imds, currFrameIdx))，'verbose'、假);loopCandidates = [loopCandidates;currKeyFrameId];% #好< AGROW >结束%更新id和索引LastKeyFrameID = CurkeyFrameID;LastKeyFrameIdX = CurrFrameIdx;AddatedFramesIDX = [已添加FramesIdx;CurrframeIdx];% #好< AGROW >CurrframeIDX = CurrFrameIDX + 1;结束%主循环结束

优化姿势minNumMatches = 30;[vSetKeyFramesOptim, poseScales] = optimizePoses(vSetKeyFrames, minNumMatches，“宽容”，1E-16）;%在优化姿态后更新地图点mappointset = helperupdateglobalmap（mappointset，directionanddepth，．..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（AddanedFramesIDX），优化）;

函数[功能，validPoints] = helperDetectAndExtractFeatures(Irgb，．..scaleFactor, numLevels, varargin)%在这个例子中，图像已经是未变形的。在一般%工作流，取消注释以下代码，以消除扭曲的图像。%% if nargin > 3% intrinsics = varargin{1};%结束％Irgb = untostortimage（IRGB，内在）;%检测ORB功能Igray = im2gray (Irgb);点= detectORBFeatures (Igray,“ScaleFactor”scaleFactor,'numlevels', numLevels);%选择一个特征子集，均匀分布在整个图像中点数= selectUniform(点数，numPoints，大小(Igray, 1:2));%提取特征[features, validPoints] = extractFeatures(Igray, points);结束

函数[H，Score，InliersIndex] = HelperComputeHomography（MatchedPoints1，MatchedPoints2）[H，InlierslogicalIndex] = eStimateGeometricTransform2D（．..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, 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”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，SUNITY（LOCIONS1,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;score = 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;cosAngle = sum(ray1 .* ray2) ./ (vecnorm(ray1, 2,2) .* vecnorm(ray2, 2,2));%检查视差isValid = all(cosAngle < cosd(minParallax) & cosAngle>0);结束

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

函数[mapPointSet, directionAndDepth, mapPointsIdx] = helperCullRecentMapPoints(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更新姿势图优化后的地图点posesOld = vSetKeyFrames.Views.AbsolutePose;一直陷于= vSetKeyFramesOptim.Views.AbsolutePose;positionsOld = mapPointSet.WorldPoints;positionsNew = positionsOld;指数= 1:mapPointSet.Count;%更新每个地图点的世界位置基于新的绝对姿态％相应的主要视图为i = 1: mapPointSet。Count majorviewid = directionAndDepth.MajorViewId(i); / /显示主视图poseNew =一直陷于(majorViewIds) .T;posennew (1:3, 1:3) = posennew (1:3, 1:3) * poseScales(majorViewIds);tform = posesOld (majorViewIds)。T \ poseNew;position new (i，:) = positionsOld(i，:) * tform(1:3,1:3) + tform(4, 1:3);结束mapPointSet = updateWorldPoints(mapPointSet, indices, positionsNew);结束