单眼视觉内径术
视觉里程计是通过分析一系列图像来确定摄像机位置和方向的过程。视觉里程计用于各种应用，如移动机器人、自动驾驶汽车和无人驾驶飞行器。此示例演示如何从一系列图像估计单个校准摄影机的轨迹。
概述
此示例显示了如何从2-D视图序列估算校准相机的轨迹。此示例使用在Tsukuba University的Cvlab中创建的新Tsukuba立体声数据集的图像。（https://cvlab.cs.tukuba.ac.jp）。DataSet由合成图像组成，使用计算机图形生成，包括地面真理相机姿势。
没有附加信息，单眼相机的轨迹只能恢复到未知的比例因子。移动机器人或自主车辆上使用的单眼视觉测量系统通常从另一个传感器（例如车辆里程表或GPS）或从场景中已知尺寸的物体获得比例因子。此示例从地面真理计算规模因子。
该示例分为三个部分：
估计相对于第一个视图的第二视图的姿势。通过估计基本矩阵并将其分解成相机位置和方向来估计第二视图的姿势。
使用全局捆绑调整引导估计相机轨迹。使用eMipolar约束消除异常值。查找从前两个视图和当前视图三角化的点之间的3D到2D对应关系。通过解决透视-N点（PNP）问题来计算当前视图的世界相机姿势。估计相机的姿势不可避免地导致累积随时间累积的错误。调用这种效果漂流。为了减少漂移，示例通过束调节来改进到目前为止估计的所有姿势。
使用加窗束调整估计摄像机剩余轨迹。通过每个新视图，改进所有姿势所需的时间都会增加。窗口捆绑调整是一种通过仅优化最后一个计算时间的方式N视图，而不是整个轨迹。通过不调用每个视图的捆绑调整，进一步减少计算时间。
读取输入图像序列和地面真相
的图像新的筑波立体声数据集在筑波大学的CVLAB创建。如果您在自己的工作或出版物中使用这些图像，请引用以下论文：
[1] Martin Peris Martorell，Atsuto Maki，Sarahartull，Yasuhiro Ohkawa，Kazuhiro Fukui，“朝着模拟驱动的立体声视觉系统”。ICPR，PP.1038-1042，2012年的诉讼程序。
[2] Sarah Martull, Martin Peris Martorell, Kazuhiro Fukui，“基于地面真实视差图的真实CG立体图像数据集”，ICPR workshop TrakMark2012, pp.40- 42,2012。
images = imagedataStore（fullfile（toolboxdir（'想象'），'VisionData'那'newtsukuba'））;％负载地面真理相机姿势。加载（完整文件）（toolboxdir('想象'），'VisionData'那“visualOdometryGroundTruth.mat”））;
创建包含序列的第一个视图的视图集
用A.imageviewset.对象存储和管理与每个视图关联的图像点和相机姿势，以及对视图对之间的点匹配。一旦你填充了一个imageviewset.对象，您可以使用它来查找跨多个视图的点轨迹，并检索要使用的相机姿势triangulatemultiewiew.和Bundleadjustment.功能。
%创建一个空的imageviewset对象来管理与每个视图关联的数据。vset = imageviewset;%读取并显示第一个图像。Irgb = readimage(images, 1);球员=愿景。放像机(“位置”，[20,400,650,510]）;步骤（播放器，IRGB）;
％使用相机内在机构创建相机内在物体对象%新筑波数据集。focalLength = [615 615];%以像素为单位指定principalPoint = [320 240];百分比像素[x，y]图象尺寸大小= (Irgb [1, 2]);％像素[murows，ncls]内在=摄像石（FocalLength，Principalpoint，Imageize）;
转换为灰度和凹陷。在此示例中，未定义无效，因为图像是合成的，没有透镜失真。但是，对于真实的图像，不断变形是必要的。
prevI = undistortion timage (im2gray(Irgb)， intrinsics); / /本质%检测功能。prevPoints = detectSURFFeatures (prevI,“MetricThreshold”，500）;％选择一个功能子集，均匀分布在整个图像中。numPoints = 200;prevPoints = selectUniform(prevPoints, numPoints，大小(prevI));%提取特征。如果需要，使用“直立”特征可以提高匹配质量％相机运动涉及较少或没有面内旋转。prevfeatures =提取物（previ，prevpoints，'直立'， 真的）;%添加第一个视图。放置与第一个视图相关联的摄像机沿z轴定向的原点处的％。viewid = 1;vset = addview（vset，viewid，rigid3d（眼睛（3），[0 0 0]），“点”，prevpoints）;
绘制初始相机姿势
创建两个表示估计的图形相机对象，并且基于来自新Tsukuba数据集的地面真理数据构成的实际相机姿势。
％设置轴。图轴（[ - 220,50，-140,20,20,300]）;％设置y轴是垂直指向的。甘氨胆酸视图(3);集(gca),'cameraupvector'， [0， - 1,0]);甘氨胆酸camorbit (-120 0“数据”， [0,1, 0]);网格在包含('x（cm）'）;ylabel（'y（cm）'）;Zlabel（'z（cm）'）;抓住在绘制估计的相机姿态。摄像机= 7;campose =姿势（vset）;Camestimated = Plotcamera（Campose，'尺寸'，摄像机，......“颜色”那'G'那'不透明度'，0）;绘制实际摄像机姿态。CAMACTUAL = Plotcamera（'尺寸'，摄像机，'absolutepose'那......rigid3d (groundTruthPoses。定位{1}、groundTruthPoses.Location {1}),......“颜色”那'B'那'不透明度'，0）;％初始化相机轨迹。轨迹般的= plot3（0,0,0，'G-'）;trajectoryactual = plot3（0,0,0，“b -”）;传奇('估计轨迹'那'实际轨迹'）;标题(“相机轨迹”）;
估计第二视图的姿态
从第二视图检测和提取功能，并将它们与第一个视图匹配helperDetectAndMatchFeatures。相对于第一个视图估计第二视图的姿势HelperestimaterElativepose，并将其添加到imageviewset.。
%读取并显示图像。viewid = 2;Irgb = readimage(images, viewwid);步骤（播放器，IRGB）;
%转换为灰度和不失真。i = undostortimage（IM2Gray（IM2Gray（IRGB），内在）;%匹配先前图像和当前图像之间的特征。[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(......PrevFeatures，i）;％估计当前视图的姿势相对于前一个视图。[orient, loc, inlierIdx] = helperEstimateRelativePose(......prevpoints（IndexPairs（：，1）），CurrPoints（IndexPairs（:,2）），内在机构）;％排除末端异常值。indexPairs = IndexPairs（Inlieridx，:);将当前视图添加到视图集。vSet = addView(vSet, viewId, rigid3d(orient, loc))，“点”，curports）;％存储上一个和当前视图之间的点匹配。vset = addConnection（vset，ViewID-1，ViewID，“匹配”, indexPairs);
相对于第一视图的第二视图的位置只能恢复到未知的比例因子。使用地面真相计算比例因子Helpernormalizeviewset.，模拟外部传感器，将用于一个典型的单目视觉里程计系统。
vSet = helperNormalizeViewSet(vSet, groundtruthpose);
使用相机轨迹绘图使用Helperupdatecameraplots.和helperUpdateCameraTrajectories。
Helperupdatecameraplots（查看，山西历史，遮阳伞，姿势（vset），......groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated trajectoryActual,......姿势（vset），地下封装）;
previ = i;PrevFeatures = Currfeatures;prevpoints = curptpoints;
使用全局捆绑调整估算摄像机轨迹
找出前两个视图中三角化的世界点与当前视图中的图像点之间的3d到2d对应关系。使用Helperfindepipolariers.找到满足eMipolar约束的匹配项，然后使用Helperfind3dto2dcorresdencess.从前两个视图中三角3-D点并在当前视图中找到相应的2-D点。
通过解决透视-N点（PNP）问题来计算当前视图的世界相机姿势estismsworldcamerapose.。对于前15个视图，使用全局捆绑调整来改进整个轨迹。使用全局捆绑调整有限数量的视图引导估计相机轨迹的其余部分，并且它并不昂贵。
为了viewid = 3:15％读取并显示下一个图像Irgb = readimage(images, viewwid);步骤（播放器，IRGB）;%转换为灰度和不失真。i = undostortimage（IM2Gray（IM2Gray（IRGB），内在）;以前和当前图像之间的％匹配点。[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(......PrevFeatures，i）;%消除特征匹配中的异常值。inlierIdx = helperFindEpipolarInliers (prevPoints (indexPairs (: 1)),......FurrPoints（IndexPairs（:,2）），内在机构）;indexPairs = IndexPairs（Inlieridx，:);％三角形点从前两个视图中找到，找到了%当前视图中对应的点。[WorldPoints，imagePoints] = HelperFind3dto2dcorreskents（vset，......内在，IndexPairs，Curpopits）;由于RANSAC涉及一个随机过程，它有时可能不会达到期望的置信水平，并超过的最大数目％试验。自结果是以来发生这种情况时会禁用警告％ 还是有效。警告State =警告（“关闭”那'愿景：Ransac：maxtrialsReached'）;估计当前视图的世界相机姿态。[东方，loc] = estmityworldcamerapose（图像点，worldpoints，continsics）;％恢复原始警告状态警告（警告安全性）将当前视图添加到视图集。vSet = addView(vSet, viewId, rigid3d(orient, loc))，“点”，curports）;％存储上一个和当前视图之间的点匹配。vset = addConnection（vset，ViewID-1，ViewID，“匹配”, indexPairs);跟踪= findTracks (vSet);％查找跨越多个视图的点轨道。坎波斯=姿势(vSet);％获得相机姿势所有视图。％三角形初始位置为3-D世界点。xyzPoints = triangulateMultiview(tracks, campose, intrinsics);％精细摄像机使用捆绑调整姿势。[~，营地]=捆绑调整（XYZ点、轨道、营地、，......内在机构，'obessundistorted',真的,'absolutetolerance'，1E-12，......'relativetolerance'，1E-12，'maxtations'，200，“FixedViewID”1);vSet = updateView(vSet, camPoses);％更新视图集。％捆绑调整可以移动整套摄像机。正常化%视图设置，将第一个摄像机放置在原点％z轴并调整比例以匹配地面真理的匹配。vSet = helperNormalizeViewSet(vSet, groundtruthpose);％更新相机轨迹图。Helperupdatecameraplots（查看，山西历史，遮阳伞，姿势（vset），......groundTruthPoses);Helperupdatecameratroples（浏览，溯极，......Trajectialualual，姿势（vset），地区;previ = i;PrevFeatures = Currfeatures;prevpoints = curptpoints;结尾
使用窗口光束调整估计剩余相机轨迹
通过使用窗口束调整来估计剩余的摄像机轨迹，只优化最后15个视图，以限制计算量。此外，bundle调整不必为每个视图调用，因为estismsworldcamerapose.将与3-D点相同的单元的姿势计算。此部分调用每个第7个视图的捆绑调整。已经通过实验选择了窗口大小和呼叫捆绑调整的频率。
为了viewId = 16:元素个数(images.Files)％读取并显示下一个图像Irgb = readimage(images, viewwid);步骤（播放器，IRGB）;%转换为灰度和不失真。i = undostortimage（IM2Gray（IM2Gray（IRGB），内在）;以前和当前图像之间的％匹配点。[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(......PrevFeatures，i）;％三角形点从前两个视图中找到，找到了%当前视图中对应的点。[WorldPoints，imagePoints] = HelperFind3dto2dcorreskents（vset，......内在，IndexPairs，Curpopits）;由于RANSAC涉及一个随机过程，它有时可能不会达到期望的置信水平，并超过的最大数目％试验。自结果是以来发生这种情况时会禁用警告％ 还是有效。警告State =警告（“关闭”那'愿景：Ransac：maxtrialsReached'）;估计当前视图的世界相机姿态。[东方，loc] = estmityworldcamerapose（图像点，worldpoints，continsics）;％恢复原始警告状态警告（警告安全性）将当前视图和连接添加到视图集。vSet = addView(vSet, viewId, rigid3d(orient, loc))，“点”，curports）;vset = addConnection（vset，ViewID-1，ViewID，“匹配”, indexPairs);%使用窗口束调整优化估计的相机姿态。运行%优化每7次查看。如果mod（viewid，7）== 0％查找最后15个视图和三角形的点轨道。windowSize = 15;startFrame = max(1, viewId - windowSize);track = findTracks(vSet, startFrame:viewId);campose = pose (vSet, startFrame:viewId);[xyzPoints, reprojErrors] = triangulateMultiview(tracks, campose, intrinsics); / /播放视频％保持前两个姿势固定，保持相同的比例。cixedIds = [Startframe，Startframe + 1];％排除了具有高重注错误的点和曲目。idx = reprojErrors < 2;[~， camPoses] = bundleAdjustment(xyzPoints(idx，:)， tracks(idx)，......野营，内在，“FixedViewIDs”，固定性，......'obessundistorted',真的,'absolutetolerance'，1E-12，......'relativetolerance'，1E-12，'maxtations', 200);vSet = updateView(vSet, camPoses);％更新视图集。结尾％更新相机轨迹图。Helperupdatecameraplots（查看，山西历史，遮阳伞，姿势（vset），......groundTruthPoses);Helperupdatecameratroples（浏览，溯极，......Trajectialualual，姿势（vset），地区;previ = i;PrevFeatures = Currfeatures;prevpoints = curptpoints;结尾
抓住从
总结
此示例演示了如何从一系列视图中估计校准单目摄像机的轨迹。请注意，估计的轨迹与地面真实情况不完全匹配。尽管摄像机姿势进行了非线性优化，但摄像机姿势估计中的误差会累积，从而导致漂移。在视觉里程计系统中，此问题非常严重通常通过融合来自多个传感器的信息和执行环路闭合来解决。
工具书类
[1] Martin Peris Martorell，Atsuto Maki，Sarahartull，Yasuhiro Ohkawa，Kazuhiro Fukui，“朝着模拟驱动的立体声视觉系统”。ICPR，PP.1038-1042，2012年的诉讼程序。
[2] Sarah Martull, Martin Peris Martorell, Kazuhiro Fukui，“基于地面真实视差图的真实CG立体图像数据集”，ICPR workshop TrakMark2012, pp.40- 42,2012。
M.I.A. Lourakis和A.A. Argyros(2009)。SBA:通用稀疏束调整软件包。数学软件学报36(1):1-30。
[4] R. Hartley，A. Zisserman，“计算机愿景中的多视图几何”，2003年剑桥大学出版社。
[5] b区格;p . McLauchlan;r·哈特利;菲茨吉本(1999)。《Bundle Adjustment: A Modern Synthesis》。国际视觉算法研讨会论文集。斯普林格出版社。298 - 372页。
[6] X.-S.高，X.-r.侯，J. Tang和H.-f.程，“完整解决方案分类为透视三点问题”，IEEE Trans。图案分析与机器智能，卷。25，不。8，pp。930-943，2003。
你也可以从以下列表中选择一个网站:
