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

imageFolder =[数据文件夹，“rgbd_dataset_freiburg3_long_office_household / rgb /”];imds = 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，“独特的”,真的,.．.“MaxRatio”, 0.9,“MatchThreshold”, 40);preMatchedPoints = prePoints(indexPairs(:，1)，:);currMatchedPoints = currPoints(indexPairs(:，2)，:);如果没有找到足够的匹配，检查下一帧minMatches = 100;如果size(indexPairs, 1) < minMatches . size(indexPairs, 1继续结束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 inliertlikox = inliersIdxH;tform = tformH;其他的inliersIdxF;tform = tformF;结束按比例计算相机位置。使用一半的%以减少计算量inlierPrePoints =预匹配点(inliertlikox);inlierCurrPoints = currMatchedPoints(inliertlikox);[relOrient, relLoc, validFraction] = relativeCameraPose(tform, intrinsic，.．.inlierPrePoints(1:2)结束,inlierCurrPoints(1:2:结束));%如果没有找到足够的内线，移动到下一帧如果validFraction < 0.9 || number (size(relOrient))==3继续结束三角化两个视图以获得3-D地图点relPose = rigid3d(relOrient, relLoc);minParallax = 1;%(度)[isValid, xyzWorldPoints, inlierTriangulationIdx] = helperTriangulateTwoFrames(.．.rigid3d, relPose, inlierPrePoints, inlierCurrPoints, intrinsic, minParallax);如果~ isValid继续结束获取两个关键帧中特征的原始索引indexPairs = indexPairs(inliertx (inlierTriangulationIdx)，:);isMapInitialized = true;disp (['用帧1和帧初始化的映射'num2str (currFrameIdx-1)])结束映射初始化循环结束

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

创建一个空的imageviewset对象来存储关键帧vSetKeyFrames =图像视图;创建一个空的worldpointset对象来存储三维地图点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地图点[mappointment set, newPointIdx] = addWorldPoints(mappointment set, xyzWorldPoints);添加地图点的观测值preLocations = prePoints.Location;currLocations = currPoints.Location;presscales = prePoints.Scale;currScales = currPoints.Scale;在第一个关键帧中添加对应地图点的图像点mappointment set = addcorresponences (mappointment set, preViewId, newPointIdx, indexPairs(:，1));在第二个关键帧中添加对应地图点的图像点mappointment set = addcorresponences (mappointment set, currViewId, newPointIdx, indexPairs(:，2));

加载离线创建的特性数据包bofData = load(“bagOfFeaturesData.mat”）;初始化位置识别数据库loopDatabase = invertedImageIndex(bofData.bof，“SaveFeatureLocations”、假);将前两个关键帧的特征添加到数据库中addmagefeatures (loopDatabase, preFeatures, preViewId);addmagefeatures (loopDatabase, currFeatures, currViewId);

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

可视化初始地图点和相机轨迹mapPlot = helperVisualizeMotionAndStructure(vSetKeyFrames, mapotset);%显示图例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, numPoints);跟踪最后一个关键帧% mappointment sidx:当前帧中观察到的映射点的索引% featureIdx:表中对应特征点的索引%电流帧[currPose, mappointment sidx, featureIdx] = helperTrackLastKeyFrame(mappointment set，.．.vSetKeyFrames。的观点,currFeatures, currPoints, lastKeyFrameId, intrinsics, scaleFactor);跟踪本地地图refKeyFrameId:包含最多的参考关键帧的ViewId%与当前帧共可见的地图点localKeyFrameIds:当前帧中连接的关键帧的ViewId[refKeyFrameId, localKeyFrameIds, currPose, mapPointsIdx, featureIdx] =.．.helptracklocalmap (mappointment set, directionAndDepth, vSetKeyFrames, mappointment sidx，.．.featureIdx, currPose, currFeatures, currPoints, intrinsic, scaleFactor, numLevels);检查当前帧是否是关键帧。如果满足以下两个条件，一个帧就是关键帧:％% 1。自最后一个关键帧或%当前帧跟踪少于100个地图点% 2。当前帧跟踪的地图点小于90%由参考关键帧跟踪的%点numSkipFrames = 20;isKeyFrame = helperIsKeyFrame(mapotset, refKeyFrameId, lastKeyFrameIdx，.．.currFrameIdx, mappointment sidx, numSkipFrames);%可视化匹配的特征updatePlot(featurePlot, currI, currPoints(featureIdx));如果~isKeyFrame currFrameIdx = currFrameIdx + 1;继续结束更新当前关键帧IDcurrKeyFrameId = currKeyFrameId + 1

添加新的关键帧[mapetset, vSetKeyFrames] = helperAddNewKeyFrame(mapetset, vSetKeyFrames，.．.currPose, currFeatures, currPoints, mapPointsIdx, featureIdx, localKeyFrameIds);移除在少于3个关键帧内观察到的离群地图点[mappointment set, directionAndDepth, mappointment sidx] = helperCullRecentMapPoints(mappointment set, directionAndDepth, mappointment sidx).．.directionAndDepth, mappointment sidx, newPointIdx);通过三角测量创建新的地图点minNumMatches = 20;minParallax = 3;[mapetset, vSetKeyFrames, newPointIdx] = helperCreateNewMapPoints(mapetset, vSetKeyFrames，.．.currKeyFrameId, intrinsic, scaleFactor, minNumMatches, minParallax);更新视图方向和深度directionAndDepth = update(directionAndDepth, mappointment set, vSetKeyFrames。的观点,.．.[mapPointsIdx;newPointIdx),真正的);%本地bundle调整[mapPointSet, directionAndDepth, vSetKeyFrames, newPointIdx] = helperLocalBundleAdjustment(.．.mappointment, directionAndDepth, vSetKeyFrames.．.currKeyFrameId, intrinsic, newPointIdx);可视化3D世界点和相机轨迹updatePlot(mapPlot, vSetKeyFrames, mapotset);

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

%优化姿势minNumMatches = 30;[vSetKeyFramesOptim, poseScales] = optimizepositions (vSetKeyFrames, minNumMatches，“宽容”1 e-16);优化姿势后更新地图点mappointment set = helperUpdateGlobalMap(mappointment set, directionAndDepth，.．.vSetKeyFrames, vSetKeyFramesOptim, poseScales;updatePlot(mapPlot, vSetKeyFrames, mapotset);绘制优化后的相机轨迹optimizedposed = pose (vSetKeyFramesOptim);plotOptimizedTrajectory (mapPlot optimizedPoses)%更新图例showLegend (mapPlot);

%负载地面真实值gTruthData = load(“orbslamGroundTruth.mat”）;gTruth = gTruthData.gTruth;绘制实际相机轨迹plotActualTrajectory(mapPlot, gTruth(addedFramesIdx)， optimizedpositions);%显示图例showLegend (mapPlot);

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

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

函数[H, score, inliersLogicalIndex] = helpcomputehomography (matchedPoints1, matchedPoints2) [H, inliersLogicalIndex] = estimateGeometricTransform2D(.．.matchedPoints1 matchedPoints2,“射影”，.．.“MaxNumTrials”1 e3,“MaxDistance”4“信心”, 90);inliersLogicalIndex = matchedPoints1;inliersLogicalIndex = matchedPoints2;inliersLogicalIndex = find(inliersLogicalIndex);location1 = 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, inliersLogicalIndex] = estimatfundamental matrix (.．.matchedPoints1 matchedPoints2,“方法”，“RANSAC”，.．.“NumTrials”1 e3,“DistanceThreshold”, 0.01);inliersLogicalIndex = matchedPoints1;inliersLogicalIndex = matchedPoints2;inliersLogicalIndex = find(inliersLogicalIndex);location1 = 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(.．.pose1, pose2, matchedPoints1, matchedPoints2, intrinsic, minParallax) [R1, t1] = cameraPoseToExtrinsics(pose1。旋转,pose1.Translation);camMatrix1 = cameraMatrix(intrinsic, R1, t1);[R2, t2] = cameraPoseToExtrinsics(pose2.)旋转,pose2.Translation);camMatrix2 = cameraMatrix(intrinsic, R2, t2);[xyzPoints, reprojectionErrors, isInFront] = triangulate(matchedPoints1，.．.matchedPoints2, camMatrix1, camMatrix2);通过视图方向和重投影误差筛选点minrerjerror = 1;inlierIdx = isInFront & reprojectionErrors < minrerjerror;xyzPoints = xyzPoints(inlierIdx，:);一个具有显著视差的良好的双视图。ray1 = xyzPoints - pose1.翻译;ray2 = xyzPoints - pose2.翻译;cosAngle = sum(ray1 .* ray2, 2) ./ (vecnorm(ray1, 2,2) .* vecnorm(ray2, 2,2));检查视差isValid = all(cosAngle < cosd(minParallax) & cosAngle>0);结束

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

函数[mappointment set, directionAndDepth, mappointment sidx] = helperCullRecentMapPoints(mappointment set, directionAndDepth, mappointment sidx, newPointIdx) outlierIdx = setdiff(newPointIdx, mappointment sidx);如果~isempty(outlierIdx) mappointment set = removeWorldPoints(mappointment set, 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 = median(vecnorm(gLocations, 2,2))/ median(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;posesNew = vSetKeyFramesOptim.Views.AbsolutePose;positionsOld = mappointment . worldpoints;positionsNew = positionsOld;指数= 1:mappointment . count;根据的新绝对姿态更新每个地图点的世界位置%对应的主要视图为i = indexes majorViewIds = directionAndDepth.MajorViewId(i);posennew = posesNew(majorViewIds).T;poseNew(1:3, 1:3) = poseNew(1:3, 1:3) * poseScales(majorViewIds);tform = posesOld(majorViewIds)。T \ posennew;positionsOld(i，:) * tform(1:3,1:3) + tform(4,1:3);结束mappointment set = updateWorldPoints(mappointment set, indexes, posisnew);结束