单眼视觉同步定位和映射

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

imageFolder = [dataFolder,“rgbd_dataset_freiburg3_long_office_household / rgb /”];imd = imageDatastore (imageFolder);%检查第一个形象currFrameIdx = 1;currFrameIdx currI = readimage (imd);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];%以像素为单位图象尺寸大小= (currI [1 - 2]);%以像素为单位intrinsic = cameraIntrinsics (focalLength principalPoint图象尺寸);%检测和提取ORB功能scaleFactor = 1.2;numLevels = 8;numPoints = 1000;[preFeatures, prePoints] = helperDetectAndExtractFeatures (currI、scaleFactor numLevels, numPoints);currFrameIdx = currFrameIdx + 1;firstI = currI;%保存第一帧isMapInitialized = false;%地图初始化循环而~ isMapInitialized & & currFrameIdx <元素个数(imds.Files) currI = readimage (imd, currFrameIdx);[currFeatures, currPoints] = helperDetectAndExtractFeatures (currI、scaleFactor numLevels, numPoints);currFrameIdx = currFrameIdx + 1;%找到公认的特征匹配indexPairs = matchFeatures (preFeatures currFeatures,独特的= true,…MaxRatio = 0.9, MatchThreshold = 40);preMatchedPoints = prePoints (indexPairs (: 1):);currMatchedPoints = currPoints (indexPairs (:, 2):);%如果找到匹配不够,检查下一个帧minMatches = 100;如果大小(indexPairs 1) < minMatches继续结束preMatchedPoints = prePoints (indexPairs (: 1):);currMatchedPoints = currPoints (indexPairs (:, 2):);%计算单应性和评估重建[tformH, scoreH inliersIdxH] = helperComputeHomography (preMatchedPoints currMatchedPoints);%计算基本矩阵和评估重建[tformF, scoreF inliersIdxF] = helperComputeFundamentalMatrix (preMatchedPoints currMatchedPoints);%基于启发式的选择模型率= scoreH / (scoreH + scoreF);ratioThreshold = 0.45;如果比> ratioThreshold inlierTformIdx = inliersIdxH;tform = tformH;其他的inlierTformIdx = inliersIdxF;tform = tformF;结束%计算摄像机位置规模。使用一半的%点,以减少计算inlierPrePoints = preMatchedPoints (inlierTformIdx);inlierCurrPoints = currMatchedPoints (inlierTformIdx);[relPose, validFraction] = estrelpose (intrinsic tform,…inlierPrePoints(1:2)结束,inlierCurrPoints(1:2:结束));%如果没有足够的内围层发现,移动到下一帧如果validFraction < 0.9 | |元素个数(relPose) = = 3继续结束%由三角形组成的两个视图获取三维地图点minParallax = 1;%的度[isValid, xyzWorldPoints inlierTriangulationIdx] = helperTriangulateTwoFrames (…rigidtform3d, relPose、inlierPrePoints inlierCurrPoints, intrinsic minParallax);如果~ isValid继续结束%得到原来的指数在两个关键帧的功能indexPairs = indexPairs (inlierTformIdx (inlierTriangulationIdx):);isMapInitialized = true;disp ([地图初始化第一帧与帧的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;%添加第一个关键帧。把相机与第一个有关%关键帧在原点,面向沿z轴preViewId = 1;vSetKeyFrames = addView (vSetKeyFrames preViewId rigidtform3d,分= 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 = addCorrespondences (mapPointSet preViewId、newPointIdx indexPairs (: 1));%添加相应像点映射点第二个关键帧mapPointSet = addCorrespondences (mapPointSet currViewId、newPointIdx indexPairs (:, 2));

%负荷特性数据的包创建离线bofData =负载(“bagOfFeaturesDataSLAM.mat”);%初始化位置识别数据库loopDatabase = invertedImageIndex (bofData.bof SaveFeatureLocations = false);%的前两个关键帧添加特性数据库addImageFeatures (loopDatabase preFeatures preViewId);addImageFeatures (loopDatabase currFeatures currViewId);

%运行完整的束调整前两个关键帧跟踪= findTracks (vSetKeyFrames);cameraPoses =姿势(vSetKeyFrames);[refinedPoints, refinedAbsPoses] = bundleAdjustment (xyzWorldPoints,跟踪,…cameraPoses, intrinsic FixedViewIDs = 1,…PointsUndistorted = true, AbsoluteTolerance = 1 e -,…RelativeTolerance = 1 e15汽油,MaxIteration = 20,…解算器=“preconditioned-conjugate-gradient”);%比例地图和相机使用的平均深度映射点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);%更新视图方向和深度mapPointSet = updateLimitsAndDirection (mapPointSet newPointIdx vSetKeyFrames.Views);%更新代表视图mapPointSet = updateRepresentativeView (mapPointSet newPointIdx vSetKeyFrames.Views);在当前帧%可视化匹配特性关闭(hfeature.Parent.Parent);featurePlot = helperVisualizeMatchedFeatures (currI currPoints (indexPairs (:, 2)));

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

% ViewId当前关键帧currKeyFrameId = currViewId;% ViewId最后的关键帧lastKeyFrameId = currViewId;%的索引输入图像序列中的最后一个关键帧lastKeyFrameIdx = currFrameIdx - 1;%指数的关键帧输入图像序列addedFramesIdx = [1;lastKeyFrameIdx];isLoopClosed = false;

%主循环isLastFrameKeyFrame = true;而~ isLoopClosed & & currFrameIdx <元素个数(imds.Files) currI = readimage (imd, currFrameIdx);[currFeatures, currPoints] = helperDetectAndExtractFeatures (currI、scaleFactor numLevels, numPoints);%跟踪最后关键帧% mapPointsIdx:指数映射的点在当前帧% featureIdx:指数对应的特征点的%当前帧[currPose, mapPointsIdx featureIdx] = helperTrackLastKeyFrame (mapPointSet,…vSetKeyFrames。视图、currFeatures currPoints, lastKeyFrameId, intrinsic scaleFactor);%跟踪当地地图和检查当前帧是一个关键帧。%一个框架是一个关键帧如果满足下面两个条件:%% 1。至少20帧已经过去了自从上次关键帧或%当前帧跟踪不到100点地图。% 2。地图上点跟踪当前帧的不到90%%点追踪的参考关键帧。%%跟踪性能敏感numPointsKeyFrame的价值。%如果跟踪丢失,尝试更大的值。%% localKeyFrameIds: ViewId连接的当前帧的关键帧numSkipFrames = 20;numPointsKeyFrame = 80;[localKeyFrameIds, currPose mapPointsIdx、featureIdx isKeyFrame] =…helperTrackLocalMap (mapPointSet vSetKeyFrames mapPointsIdx,…featureIdx, currPose、currFeatures currPoints, intrinsic scaleFactor, numLevels,…isLastFrameKeyFrame、lastKeyFrameIdx currFrameIdx、numSkipFrames numPointsKeyFrame);%可视化匹配特性updatePlot (featurePlot、currI currPoints (featureIdx));如果~ isKeyFrame currFrameIdx = currFrameIdx + 1;isLastFrameKeyFrame = false;继续其他的isLastFrameKeyFrame = true;结束%更新当前关键帧IDcurrKeyFrameId = currKeyFrameId + 1;

%添加新的关键帧[mapPointSet, vSetKeyFrames] = helperAddNewKeyFrame (mapPointSet vSetKeyFrames,…currPose、currFeatures currPoints、mapPointsIdx featureIdx, localKeyFrameIds);%去除离群值映射点,观察不到3关键帧mapPointSet = helperCullRecentMapPoints (mapPointSet mapPointsIdx newPointIdx);%由三角创建新地图点minNumMatches = 10;minParallax = 3;[mapPointSet, vSetKeyFrames newPointIdx] = helperCreateNewMapPoints (mapPointSet vSetKeyFrames,…currKeyFrameId, intrinsic scaleFactor、minNumMatches minParallax);%当地束调整[refinedViews dist] = connectedViews (vSetKeyFrames, currKeyFrameId MaxDistance = 2);refinedKeyFrameIds = refinedViews.ViewId;fixedViewIds = refinedKeyFrameIds (dist = = 2);fixedViewIds = fixedViewIds (1: min(10,元素个数(fixedViewIds)));%改进当地关键帧和地图点[mapPointSet, vSetKeyFrames mapPointIdx] = bundleAdjustment (…mapPointSet vSetKeyFrames, [refinedKeyFrameIds;intrinsic currKeyFrameId),…FixedViewIDs = FixedViewIDs PointsUndistorted = true, AbsoluteTolerance = 1 e -,…RelativeTolerance = 1 e-16,解算器=“preconditioned-conjugate-gradient”,…MaxIteration = 10);%更新视图方向和深度mapPointSet = updateLimitsAndDirection (mapPointSet mapPointIdx vSetKeyFrames.Views);%更新代表视图mapPointSet = updateRepresentativeView (mapPointSet mapPointIdx vSetKeyFrames.Views);%显示3 d世界分和相机轨迹updatePlot (mapPlot vSetKeyFrames mapPointSet);

%检查循环创建了一些关键帧后关闭如果currKeyFrameId > 20%的最小数量的特征匹配循环边缘loopEdgeNumMatches = 50;%检测可能的循环关闭关键帧的候选人[isDetected, validLoopCandidates] = helperCheckLoopClosure (vSetKeyFrames currKeyFrameId,…loopDatabase、currI loopEdgeNumMatches);如果isDetected%添加循环关闭连接[isLoopClosed, mapPointSet vSetKeyFrames] = helperAddLoopConnections (…mapPointSet、vSetKeyFrames validLoopCandidates currKeyFrameId,…currFeatures loopEdgeNumMatches);结束结束%如果没有检测到,关闭循环电流特性添加到数据库中如果~ isLoopClosed addImageFeatures (loopDatabase currFeatures currKeyFrameId);结束%更新id和指数lastKeyFrameId = currKeyFrameId;lastKeyFrameIdx = currFrameIdx;addedFramesIdx = [addedFramesIdx;currFrameIdx];% #好< AGROW >currFrameIdx = currFrameIdx + 1;结束%主循环的结束

%优化提出了minNumMatches = 20;[vSetKeyFramesOptim, poseScales] = optimizePoses (vSetKeyFrames minNumMatches,公差= 1 e-16);%更新地图点优化后的姿势mapPointSet = helperUpdateGlobalMap (mapPointSet 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 (addedFramesIdx) optimizedPoses);

函数[特性,validPoints] = helperDetectAndExtractFeatures (Irgb…scaleFactor、numLevels numPoints变长度输入宗量)%在这个例子中,图像已经不失真。在一般%的工作流程,取消以下代码undistort图像。%%如果输入参数个数> 4% intrinsic =变长度输入宗量{1};%结束% Irgb = undistortImage (Irgb intrinsic);%检测ORB功能Igray = im2gray (Irgb);点= detectORBFeatures (Igray ScaleFactor = ScaleFactor NumLevels = NumLevels);%选择特性的一个子集,均匀分布在整个图像点= selectUniform(点、numPoints大小(Igray 1:2));%提取特征[特性,validPoints] = extractFeatures (Igray点);结束

函数[H,分数,inliersIndex] = helperComputeHomography (matchedPoints1 matchedPoints2) [H, inliersLogicalIndex] = estgeotform2d (…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 =总和((locations2 - xy1In2)。2)^ 2;error2in1 =总和((locations1 - xy2In1)。2)^ 2; outlierThreshold = 6; score = sum(max(outlierThreshold-error1in2, 0)) +…sum (max (outlierThreshold-error2in1, 0));结束

函数[F,分数,inliersIndex] = helperComputeFundamentalMatrix (matchedPoints1 matchedPoints2) (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 ', locations2);error2in1 = (sum ([locations1的(大小(locations1, 1), 1)]。* lineIn1, 2)) ^ 2…。/笔(lineIn1 (:, 1:2)。2)^ 2;lineIn2 = epipolarLine (F, locations1);error1in2 = (sum ([locations2的(大小(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) camMatrix1 = cameraProjection (intrinsic, pose2extr (pose1));camMatrix2 = cameraProjection (intrinsic pose2extr (pose2));[xyzPoints, reprojectionErrors isInFront] =满足(matchedPoints1,…matchedPoints2、camMatrix1 camMatrix2);%方向滤波器的观点和reprojection错误minReprojError = 1;inlierIdx = isInFront & reprojectionErrors < minReprojError;xyzPoints = xyzPoints (inlierIdx:);%有重大视差的两个视图ray1 = xyzPoints - pose1.Translation;ray2 = xyzPoints - pose2.Translation;cosAngle =总和(ray1。* ray2, 2)。/ (vecnorm (ray1 2 2)。* vecnorm (ray2 2 2));%检查视差isValid =所有(cosAngle < cosd (minParallax) & cosAngle > 0);结束

函数mapPointSet = helperCullRecentMapPoints (mapPointSet mapPointsIdx newPointIdx) outlierIdx = setdiff (newPointIdx mapPointsIdx);如果~ isempty (outlierIdx) mapPointSet = removeWorldPoints (mapPointSet outlierIdx);结束结束

函数rmse = helperEstimateTrajectoryError (gTruth cameraPoses)位置= vertcat (cameraPoses.AbsolutePose.Translation);gLocations = vertcat (gTruth.Translation);规模=值(vecnorm (gLocations 2 2)) /中位数(vecnorm(位置2 2));scaledLocations =位置*规模;rmse =√意味着总和((scaledLocations - gLocations)。^ 2,2)));disp ([“绝对RMSE关键帧轨迹(m):“num2str (rmse)]);结束

函数mapPointSet = helperUpdateGlobalMap (…mapPointSet、vSetKeyFrames vSetKeyFramesOptim poseScales)% helperUpdateGlobalMap构成图优化后更新地图点posesOld = vSetKeyFrames.Views.AbsolutePose;一直陷于= vSetKeyFramesOptim.Views.AbsolutePose;positionsOld = mapPointSet.WorldPoints;positionsNew = positionsOld;指数= 1:mapPointSet.Count;%更新世界每个映射点的位置根据新的绝对的姿势%的主要观点为我=指数majorViewIds = mapPointSet.RepresentativeViewId(我);poseNew =一直陷于(majorViewIds)。;poseNew (1:3, 1:3) = poseNew (1:3, 1:3) * poseScales (majorViewIds);tform = affinetform3d (poseNew / posesOld (majorViewIds)。);positionsNew(我)= transformPointsForward (tform, positionsOld(我,:));结束mapPointSet = updateWorldPoints (mapPointSet,指标,positionsNew);结束