单眼视觉同步定位和映射
视觉同时定位和地图(vSLAM),指的是计算摄像机的位置和姿态的过程对周围环境,同时环境的映射。这个过程只使用视觉输入的相机。申请vSLAM包括增强现实、机器人、自动驾驶。
这个例子展示了如何处理图像数据从一个单眼相机建立室内环境的地图和估计摄像机的轨迹。本例使用ORB-SLAM[1],这是一个基于vSLAM算法。
加速计算,您可以启用并行计算的计算机视觉工具箱的偏好对话框。打开计算机视觉工具箱™偏好,家选项卡,环境部分中,点击首选项。然后选择计算机视觉的工具箱。
术语表
这个例子中常用以下条款:
关键帧:视频帧的一个子集,包含信号的定位和跟踪。连续两个关键帧通常涉及足够的视觉变化。
地图上分:的3 d点列表代表了地图环境重建的关键帧。
Covisibility图:一个图形组成的关键帧作为节点。两个关键帧的边缘连接如果他们共同映射点。的重量是共享地图点的数量的优势。
基本图:covisibility图的子图只包含边缘高重量,即更多的共享地图点。
位置识别数据库:数据库用于识别是否在过去已经访问过的地方。数据库存储的视觉word-to-image映射基于输入包功能。它是用来寻找一个视觉形象与查询图像相似。
概述ORB-SLAM
ORB-SLAM管道包括:
地图初始化:ORB-SLAM首先初始化地图的三维点两个视频帧。计算三维点和相对相机的姿势使用基于二维三角ORB功能对应。
跟踪:一旦初始化地图,每一个新的框架,相机姿势估计当前帧的匹配特性在最后关键帧特征。估计相机姿势精制通过跟踪当地地图。
当地的地图:当前帧用来创建新的3 d地图点是否被确定为一个关键帧。在这个阶段,包调整用于最小化reprojection错误通过调整相机的姿势和3 d点。
循环关闭:为每个关键帧检测到循环通过比较它与以前的所有关键帧使用bag-of-features方法。一旦检测到循环闭合,构成图进行了优化完善相机带来的所有关键帧。
下载并探索输入图像序列
在这个例子中是使用的数据TUM RGB-D基准[2]。你可以下载数据到一个临时目录中使用一个web浏览器或通过运行下面的代码:
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);结束
创建一个imageDatastore
对象检查RGB图像。
imageFolder = [dataFolder,“rgbd_dataset_freiburg3_long_office_household / rgb /”];imd = imageDatastore (imageFolder);%检查第一个形象currFrameIdx = 1;currFrameIdx currI = readimage (imd);himage = imshow (currI);
地图初始化
ORB-SLAM管道从初始化开始,3 d世界地图点。这一步是至关重要的,最后一个大满贯的准确性产生重大影响的结果。最初的圆形特征点对应使用matchFeatures
一副图像之间。通讯后发现,两个几何变换模型是用于建立地图初始化:
单应性:如果现场平面单应性投影变换是描述特征点对应一个更好的选择。
基本矩阵:如果non-planar现场,必须使用一个基本矩阵。
单应性和基本矩阵可以计算使用estimateGeometricTransform2D
和estimateFundamentalMatrix
,分别。模型,结果在一个更小的reprojection选择错误估计两帧之间的相对旋转和转换使用relativeCameraPose
。由于RGB图像是由单眼相机不提供深度信息,相对的翻译只能恢复到一个特定的比例因子。
考虑到相对相机的姿势和两幅图像的特征点匹配,匹配点的三维位置确定由三角形组成的
函数。三角图点是有效时位于这两个摄像头,前面reprojection误差较低,当两个视图的点的视差是足够大的。
%制定再现性随机种子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,“独特的”,真的,…“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);[relOrient, relLoc validFraction] = relativeCameraPose (intrinsic tform,…inlierPrePoints(1:2)结束,inlierCurrPoints(1:2:结束));%如果没有足够的内围层发现,移动到下一帧如果validFraction < 0.9 | |元素个数(大小(relOrient)) = = 3继续结束%由三角形组成的两个视图获取三维地图点relPose = rigid3d (relOrient relLoc);minParallax = 1;%的度[isValid, xyzWorldPoints inlierTriangulationIdx] = helperTriangulateTwoFrames (…rigid3d, relPose、inlierPrePoints inlierCurrPoints, intrinsic minParallax);如果~ isValid继续结束%得到原来的指数在两个关键帧的功能indexPairs = indexPairs (inlierTformIdx (inlierTriangulationIdx):);isMapInitialized = true;disp ([地图初始化第一帧与帧的num2str (currFrameIdx-1)])结束%地图初始化结束循环
地图初始化第一帧与帧26
如果isMapInitialized关闭(himage.Parent.Parent);%关闭之前的图%显示匹配特性hfeature = showMatchedFeatures (firstI currI prePoints (indexPairs (: 1)),…currPoints (indexPairs (:, 2)),“蒙太奇”);其他的错误(无法初始化地图。)结束
存储初始关键帧和地图点
地图初始化后使用两个框架,您可以使用imageviewset
,worldpointset
和helperViewDirectionAndDepth
来存储两个关键帧和相应的地图点:
imageviewset
存储关键帧和它们的属性,比如ORB描述符,特征点和相机的姿势,和关键帧之间的连接,如特征点匹配和相对相机的姿势。它也构成图构建和更新。测程法的绝对相机姿态和相对相机姿势边缘被存储为rigid3d
对象。的相对相机姿势loop-closure边缘被存储为affine3d
对象。worldpointset
商店的3 d位置点和三维映射到二维投影通讯:地图点是在一个关键帧和关键帧的观察地图。helperViewDirectionAndDepth
商店其他地图的属性点,如均值视图方向,代表ORB描述符,和距离的范围的地图可以观察到。
%创建一个空imageviewset对象存储关键帧vSetKeyFrames = imageviewset;%创建一个空worldpointset对象来存储3 d地图点mapPointSet = worldpointset;%创建helperViewDirectionAndDepth对象来存储视图方向和深度directionAndDepth = helperViewDirectionAndDepth(大小(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 = addCorrespondences (mapPointSet preViewId、newPointIdx indexPairs (: 1));%添加相应像点映射点第二个关键帧mapPointSet = addCorrespondences (mapPointSet currViewId、newPointIdx indexPairs (:, 2));
初始化位置识别数据库
循环使用bags-of-words方法执行检测。视觉词汇表示为bagOfFeatures
对象创建脱机使用ORB描述符提取大量的图像数据集通过调用:
袋= bagOfFeatures (imd, CustomExtractor, @helperORBFeatureExtractorFunction, TreeProperties, 5, 10, StrongestFeatures, 1);
在哪里洛桑国际管理发展学院
是一个imageDatastore
对象存储图像和训练helperORBFeatureExtractorFunction
ORB功能器功能。看到图像检索与袋的视觉语言为更多的信息。
关闭循环过程逐步构建一个数据库,表示为一个invertedImageIndex
对象,存储视觉word-to-image映射基于ORB的包功能。
%负荷特性数据的包创建离线bofData =负载(“bagOfFeaturesDataSLAM.mat”);%初始化位置识别数据库loopDatabase = invertedImageIndex (bofData.bof,“SaveFeatureLocations”、假);%的前两个关键帧添加特性数据库addImageFeatures (loopDatabase preFeatures preViewId);addImageFeatures (loopDatabase currFeatures currViewId);
完善和可视化初始重建
完善初始重建使用bundleAdjustment
,优化相机姿态和世界指向整个reprojection错误最小化。细化后,包括3 d位置地图的属性点,更新视图方向、范围和深度。您可以使用helperVisualizeMotionAndStructure
可视化的地图分和摄像机的位置。
%运行完整的束调整前两个关键帧跟踪= findTracks (vSetKeyFrames);cameraPoses =姿势(vSetKeyFrames);[refinedPoints, refinedAbsPoses] = bundleAdjustment (xyzWorldPoints,跟踪,…cameraPoses intrinsic,“FixedViewIDs”,1…“PointsUndistorted”,真的,“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);%更新视图方向和深度directionAndDepth =更新(directionAndDepth mapPointSet vSetKeyFrames.Views,newPointIdx, true);在当前帧%可视化匹配特性关闭(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;
每一帧处理如下:
ORB功能是为每个新帧,然后提取匹配(使用
matchFeatures
),在最后关键帧特性已知相应的3 d地图点。估计摄像机构成Perspective-n-Point算法使用
estimateWorldCameraPose
。考虑到相机的姿势,项目地图点观察到最后关键帧到当前帧和通讯使用搜索功能
matchFeaturesInRadius
。与三维二维通信在目前的框架,细化相机姿势通过执行motion-only束调整使用
bundleAdjustmentMotion
。项目当地的点映射到当前帧搜索通讯使用更多的功能
matchFeaturesInRadius
和完善摄像机构成再次使用bundleAdjustmentMotion
。跟踪的最后一步是确定当前帧是一种新的关键帧。如果当前帧是一个关键帧,继续的当地的地图的过程。否则,开始跟踪为下一个框架。
如果跟踪丢失,因为没有足够数量的特征点匹配,更频繁地尝试新插入关键帧。
%主循环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 = 100;[localKeyFrameIds, currPose mapPointsIdx、featureIdx isKeyFrame] =…helperTrackLocalMap (mapPointSet directionAndDepth、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
包含尽可能少的离群值,一个有效的地图点必须在至少3关键帧。
新地图点是由呈三角形ORB在当前关键帧特征点及其连接的关键帧。对于每一个无与伦比的特征点在当前关键帧,寻找匹配与其他无与伦比的点连接关键帧的使用matchFeatures
。当地包调整改进当前的关键帧的构成,提出了连接的关键帧,所有地图点观察到这些关键帧。
%添加新的关键帧[mapPointSet, vSetKeyFrames] = helperAddNewKeyFrame (mapPointSet vSetKeyFrames,…currPose、currFeatures currPoints、mapPointsIdx featureIdx, localKeyFrameIds);%去除离群值映射点,观察不到3关键帧[mapPointSet, directionAndDepth mapPointsIdx] = helperCullRecentMapPoints (mapPointSet,…directionAndDepth、mapPointsIdx newPointIdx);%由三角创建新地图点minNumMatches = 20;minParallax = 3;[mapPointSet, vSetKeyFrames newPointIdx] = helperCreateNewMapPoints (mapPointSet vSetKeyFrames,…currKeyFrameId, intrinsic scaleFactor、minNumMatches minParallax);%更新视图方向和深度directionAndDepth =更新(directionAndDepth mapPointSet vSetKeyFrames.Views,…[mapPointsIdx;newPointIdx),真正的);%当地束调整[mapPointSet, directionAndDepth vSetKeyFrames newPointIdx] = helperLocalBundleAdjustment (…mapPointSet、directionAndDepth vSetKeyFrames,…currKeyFrameId, intrinsic newPointIdx);%显示3 d世界分和相机轨迹updatePlot (mapPlot vSetKeyFrames mapPointSet);
循环关闭
循环检测步骤需要关闭当前关键帧处理当地的映射过程,并试图发现并关闭循环。循环候选人确定通过查询数据库中的图像,视觉上类似于当前使用关键帧evaluateImageRetrieval
。候选关键帧是有效的,如果它不是连接到邻国的最后关键帧和三个关键帧循环的候选人。
当候选人找到一个有效的循环,使用estimateGeometricTransform3D
计算循环之间的相对姿态候选帧和当前的关键帧。相对姿势代表一个存储在一个三维相似变换affine3d
对象。然后添加循环与相对姿态和更新mapPointSet
和vSetKeyFrames
。
%检查循环创建了一些关键帧后关闭如果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;结束%主循环的结束
循环之间的边缘添加关键帧:9 - 166
最后,一个相似的姿势图基本图进行优化vSetKeyFrames
纠正相机带来的漂移。基本图形创建内部通过删除联系不到minNumMatches
covisibility图匹配。相似性构成图优化后,更新地图的三维位置点使用优化的姿势和相关尺度。
%优化提出了minNumMatches = 30;[vSetKeyFramesOptim, poseScales] = optimizePoses (vSetKeyFrames minNumMatches,“宽容”1 e-16);%更新地图点优化后的姿势mapPointSet = helperUpdateGlobalMap (mapPointSet directionAndDepth,…vSetKeyFrames、vSetKeyFramesOptim poseScales);updatePlot (mapPlot vSetKeyFrames mapPointSet);%画出优化的相机轨迹optimizedPoses =姿势(vSetKeyFramesOptim);plotOptimizedTrajectory (mapPlot optimizedPoses)%更新传奇showLegend (mapPlot);
比较与地面真理
你可以比较优化的相机轨迹与地面真理来评估ORB-SLAM的准确性。包含一个下载的数据groundtruth.txt
文件存储地面真理相机每一帧的姿态。数据已经保存在MAT-file的形式。你也可以计算均方根误差(RMSE)轨迹估计。
%负载地面实况gTruthData =负载(“orbslamGroundTruth.mat”);gTruth = gTruthData.gTruth;%画出实际相机轨迹plotActualTrajectory (mapPlot gTruth (addedFramesIdx) optimizedPoses);%显示传奇showLegend (mapPlot);
%评估跟踪精度helperEstimateTrajectoryError (gTruth (addedFramesIdx) optimizedPoses);
绝对RMSE关键帧轨迹(m): 0.20758
这结论概述如何构建地图的室内环境,并使用ORB-SLAM估计摄像机的轨迹。您可以测试视觉大满贯管道不同的数据集,通过优化以下参数:
numPoints
:480 x640像素集的图像分辨率numPoints
到1000岁。等更高的分辨率720×12金宝搏官方网站80,将其设置为2000。更大的值在特征提取需要更多的时间。numSkipFrames
:30帧的帧率设置numSkipFrames
是20。帧速率慢,它设置为一个较小的值。增加numSkipFrames
提高跟踪速度,但可能会导致跟踪丢失的相机运动时快。
金宝app支持功能
辅助功能包括如下。更大的功能都包含在单独的文件中。
helperAddLoopConnections
添加当前关键帧之间的连接和有效循环的候选人。
helperAddNewKeyFrame
关键帧添加关键帧集合。
helperCheckLoopClosure
检测循环候选关键帧通过从数据库中检索看起来很相似的图像。
helperCreateNewMapPoints
创建新的地图由三角点。
helperLocalBundleAdjustment
完善当前关键帧的构成和surrrounding场景的地图。
helperORBFeatureExtractorFunction
实现了ORB bagOfFeatures中使用的特征提取。
helperTrackLastKeyFrame
估计当前相机姿势通过跟踪最后关键帧。
helperTrackLocalMap
完善当前相机姿势通过跟踪当地地图。
helperViewDirectionAndDepth
存储意味着视图方向和预测映射点的深度
helperVisualizeMatchedFeatures
在一个框架显示匹配的特性。
helperVisualizeMotionAndStructure
显示地图分和相机轨迹。
helperDetectAndExtractFeatures
检测并从图像中提取和ORB功能。
函数[特性,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点);结束
helperHomographyScore
计算单应性和评估重建。
函数[H,分数,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 =总和((locations2 - xy1In2)。2)^ 2;error2in1 =总和((locations1 - xy2In1)。2)^ 2; outlierThreshold = 6; score = sum(max(outlierThreshold-error1in2, 0)) +…sum (max (outlierThreshold-error2in1, 0));结束
helperFundamentalMatrixScore
计算基本矩阵和评估重建。
函数[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));结束
helperTriangulateTwoFrames
满足两个帧初始化地图。
函数[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] =满足(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);结束
helperCullRecentMapPoints
剔除最近添加地图点。
函数[mapPointSet, directionAndDepth mapPointsIdx] = helperCullRecentMapPoints (mapPointSet、directionAndDepth mapPointsIdx, newPointIdx) outlierIdx = setdiff (newPointIdx mapPointsIdx);如果~ isempty (outlierIdx) mapPointSet = removeWorldPoints (mapPointSet outlierIdx);directionAndDepth =删除(directionAndDepth outlierIdx);mapPointsIdx = mapPointsIdx - arrayfun (@ (x) nnz (x > outlierIdx) mapPointsIdx);结束结束
helperEstimateTrajectoryError
计算跟踪误差。
函数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)]);结束
helperUpdateGlobalMap
更新三维地图的位置后点构成图优化
函数[mapPointSet, directionAndDepth] = helperUpdateGlobalMap (…mapPointSet、directionAndDepth vSetKeyFrames、vSetKeyFramesOptim poseScales)% helperUpdateGlobalMap构成图优化后更新地图点posesOld = vSetKeyFrames.Views.AbsolutePose;一直陷于= vSetKeyFramesOptim.Views.AbsolutePose;positionsOld = mapPointSet.WorldPoints;positionsNew = positionsOld;指数= 1:mapPointSet.Count;%更新世界每个映射点的位置根据新的绝对的姿势%的主要观点为我=指数majorViewIds = directionAndDepth.MajorViewId(我);poseNew =一直陷于(majorViewIds) .T;poseNew (1:3, 1:3) = poseNew (1:3, 1:3) * poseScales (majorViewIds);tform = posesOld (majorViewIds)。T \ poseNew;positionsNew(我:)= positionsOld(我,:)* tform (1:3, 1:3) + tform (1:3);结束mapPointSet = updateWorldPoints (mapPointSet,指标,positionsNew);结束
参考
[1]Mur-Artal,劳尔,前首相何塞•玛丽亚•马丁内斯打算和胡安·d·缓慢的。“ORB-SLAM:通用的和准确的单眼大满贯系统”。IEEE机器人31日。5,1147 - 116页,2015年。
[2]Sturm,尤尔根•尼古拉·恩格尔哈德,菲利克斯Endres, Wolfram Burgard和丹尼尔·克莱莫。“一个基准RGB-D大满贯评估系统”。在学报IEEE / RSJ智能机器人和系统国际会议,第580 - 573页,2012年。