单眼视觉测程法

打开直播脚本

视觉里程计是通过分析一系列图像来确定相机的位置和方向的过程。视觉测程仪被用于各种应用，如移动机器人、自动驾驶汽车和无人驾驶飞行器。这个例子向你展示了如何从一个图像序列估计单个校准相机的轨迹。

概述

这个例子展示了如何从一个2-D视图序列估计校准相机的轨迹。本例使用筑波大学的CVLAB创建的新筑波立体数据集的图像。(https://cvlab.cs.tsukuba.ac.jp)。数据集由计算机图形生成的合成图像组成，包括地面真实相机的姿态。

如果没有额外的信息，单目摄像机的轨迹只能恢复到一个未知的比例因子。用于移动机器人或自动驾驶车辆的单目视觉里程计系统通常从另一个传感器(如车轮里程表或GPS)或场景中已知大小的物体获得比例因子。这个例子是从地面真实值计算比例因子。

该示例分为三个部分:

估计相对于第一视图的第二视图的姿态。通过估计基本矩阵并将其分解为摄像机的位置和方向来估计第二视图的姿态。
使用全局束平差的自举估计摄像机轨迹。使用极线约束消除异常值。找到从前两个视图和当前视图三角化的点之间的3d到2d对应关系。通过求解perspective-n-point (PnP)问题来计算当前视图的世界相机位姿。估计相机的姿态不可避免地会导致误差，而且误差会随着时间累积。这种效应被称为的漂移．为了减少漂移，本例使用束调整对目前估计的所有姿态进行了改进。
估计使用加窗束调整剩余的相机轨迹。随着每一个新的视图，改进所有姿势的时间都在增加。窗口束调整是一种通过优化最后一个来减少计算时间的方法n而不是整个轨迹。通过不为每个视图调用bundle调整，计算时间进一步减少。

读取输入图像序列和地面真理

此示例使用从图像中新筑波立体声数据集由筑波大学的CVLAB创造。如果您在自己的作品或出版物中使用这些图片，请引用以下论文:

[1] Martin Peris Martorell, Atsuto Maki, Sarah Martull, Yasuhiro Ohkawa, Kazuhiro Fukui，“走向模拟驱动的立体视觉系统”。acta ICPR sinica, pp.1038-1042, 2012。

[2]萨拉Martull，马丁潜望马尔托雷尔，福井一弘，“现实的CG立体图像数据集与地面实况视差图”，ICPR车间TrakMark2012，pp.40-42，2012年论文集。

图像= imageDatastore (fullfile (toolboxdir (“愿景”)，“visiondata”，“NewTsukuba”))；加载地面真实相机姿势。负载(fullfile (toolboxdir (“愿景”)，“visiondata”，'visualOdometryGroundTruth.mat'))；

创建视图组包含该序列的第一个视图

使用一个imageviewset对象来存储和管理与每个视图相关联的图像点和摄像机姿态，以及视图对之间的点匹配。一旦你填充了imageviewset对象，你可以使用它在多个视图中找到点轨迹和检索相机的姿态要使用triangulateMultiview和bundleAdjustment职能。

％创建一个空imageviewset对象管理与每个视图相关联的数据。vSet = imageviewset;％读取并显示所述第一图像。IRGB = readimage（图像，1）;玩家= vision.VideoPlayer（'位置'， [20, 400, 650, 510]);步骤(球员,Irgb);

属性中使用camera intrinsics创建camera intrinsics对象%新筑波数据集。长焦点= [615 615]。％以像素为单位指定的principalPoint = [320 240]。%(像素)[x, y]IMAGESIZE =尺寸（IRGB，[1,2]）;%(像素)[mrows, ncols]intrinsics = cameraIntrinsics(focalLength, principalPoint, imageSize);

转换为灰度和不失真。在这个例子中，不失真没有效果，因为图像是合成的，没有镜头失真。然而，对于真实的图像，不失真是必要的。

PREVI = undistortImage（im2gray（IRGB），内联函数）;％检测功能。prevPoints = detectSURFFeatures（PREVI，'MetricThreshold', 500);％选择的特征的子集，整个图像均匀地分布。numPoints = 200;prevPoints = selectUniform(prevPoints, numPoints，大小(prevI));%提取特征。如果使用“直立”功能可以提高匹配质量相机的运动很少或没有涉及到面内旋转。prevFeatures = extractFeatures(prevI, prevPoints，“正直”,真正的);％添加的第一个观点。放置与所述第一视图相关联的相机%在原点，沿z轴方向。viewId = 1;vSet = addView(vSet, viewId, rigid3d(eye(3)， [0 0 0]))，“点”, prevPoints);

绘制初始摄像机姿态

根据新筑波数据集的地面真实数据，创建两个图形相机对象，代表估计和实际相机姿态。

%设置坐标轴。图轴([-220,50，-140,20，-50,300]);%设置y轴垂直向下。视图（GCA，3）;集（GCA，“CameraUpVector”，[0，-1,0]）;camorbit（GCA，-120，0，'数据'，[0，1，0]）;网格在xlabel（“X (cm)”）;ylabel (“Y (cm)”）;zlabel (“Z (cm)”）;持有在％绘制估计相机姿势。cameraSize = 7;camPose =姿势(vSet);camEstimated = plotCamera (camPose,“大小”cameraSize,．..'颜色'，‘g’，“不透明度”, 0);％绘制实际相机的姿势。camActual = plotCamera (“大小”cameraSize,“AbsolutePose”，．..rigid3d（groundTruthPoses.Orientation {1}，groundTruthPoses.Location {1}），．..'颜色'，“b”，“不透明度”, 0);初始化摄像机轨迹。轨迹估计= plot3(0,0,0，“g -”）;轨迹= plot3(0,0,0，'B-'）;传奇（“估计轨迹”，“实际轨迹”）;标题(“相机轨迹”）;

估计第二视图的姿态

从第二个视图中检测和提取特征，并将其与第一个视图进行匹配helperDetectAndMatchFeatures．使用，估计第二个视图相对于第一个视图的姿态helperEstimateRelativePose，并将其添加到imageviewset．

％读取并显示该图像。viewId = 2;Irgb = readimage(images, viewwid);步骤(球员,Irgb);

％转换为灰度级和undistort。I = undistortion timage (im2gray(Irgb)， intrinsics);以前和当前图像之间％比赛的特点。[currPoints，currFeatures，indexPairs] = helperDetectAndMatchFeatures（．..prevFeatures,我);估算当前视图相对于前一个视图的姿态。[取向，LOC，inlierIdx] = helperEstimateRelativePose（．..prevPoints (indexPairs (: 1)), currPoints (indexPairs (:, 2)), intrinsic);%排除极线异常值。indexPairs = indexPairs(inlierIdx，:);％添加当前视图到视图集。VSET = addView（VSET，viewId，rigid3d（东方，LOC），“点”, currPoints);%存储前一个视图和当前视图之间的点匹配。vSet = addConnection(vSet, viewId-1, viewId，)'火柴'，indexPairs）;

第二视图相对于第一视图的位置只能恢复到一个未知的比例因子。利用地面真实值计算比例因子helperNormalizeViewSet，模拟外部传感器，这将在一个典型的单目视觉测距系统中使用。

VSET = helperNormalizeViewSet（VSET，groundTruthPoses）;

更新摄像机轨迹图使用helperUpdateCameraPlots和helperUpdateCameraTrajectories．

helperupdatecameraplot (viewId, camEstimated, camActual, pose (vSet)，．..groundTruthPoses);helperUpdateCameraTrajectories（viewId，trajectoryEstimated，trajectoryActual，．..姿势(vSet) groundTruthPoses);

prevI =我;prevFeatures = currFeatures;prevPoints = currPoints;

使用全局束平差的Bootstrap相机轨迹估计

查找从目前前两个观点和图像点世界三角点之间的3D到2D对应。用helperFindEpipolarInliers查找满足极线约束的匹配项，然后使用helperFind3Dto2DCorrespondences将前两个视图中的三维点进行三角剖分，并在当前视图中找到相应的二维点。

通过求解透视-n点(PnP)问题，计算当前视图的世界相机位姿estimateWorldCameraPose．对于前15个视图，使用全局bundle调整来完善整个轨迹。对有限数量的视图使用全局bundle调整将引导估计相机轨迹的其余部分，而且这并不昂贵。

为viewId = 3:15%读取并显示下一个图像Irgb = readimage(images, viewwid);步骤(球员,Irgb);％转换为灰度级和undistort。I = undistortion timage (im2gray(Irgb)， intrinsics);%先前图像和当前图像之间的匹配点。[currPoints，currFeatures，indexPairs] = helperDetectAndMatchFeatures（．..prevFeatures,我);％从消除特征匹配的异常值。inlierIdx = helperFindEpipolarInliers（prevPoints（indexPairs（：，1）），．..currPoints (indexPairs (:, 2)), intrinsic);indexPairs = indexPairs(inlierIdx，:);从前面两个视图中三角化点，并找到％在当前视图中对应的点。[worldPoints, imagePoints] = helperfind3dto2d通信(vSet，．..intrinsic、indexPairs currPoints);％与RANSAC涉及一个随机过程，有时不％达到预期的置信水平和超越的最大数量%试验。当它发生时禁用警告，因为结果是%仍然有效。warningstate =警告('离开'，“愿景:ransac: maxTrialsReached”）;％估算当前世界观相机姿态。[orient, loc] = estimateWorldCameraPose(imagePoints, worldPoints, intrinsics);%恢复原来的警告状态警告(warningstate)％添加当前视图到视图集。VSET = addView（VSET，viewId，rigid3d（东方，LOC），“点”, currPoints);%存储前一个视图和当前视图之间的点匹配。vSet = addConnection(vSet, viewId-1, viewId，)'火柴'，indexPairs）;曲目= findTracks（VSET）;查找点轨迹跨越多个视图。camPoses =姿势（VSET）;为所有视图设置相机姿势。三角定位三维世界点的初始位置。xyzPoints = triangulateMultiview（轨道，camPoses，内在）;使用束调整优化相机姿态。[~， campose] = bundleAdjustment(xyzPoints, tracks, campose，．..intrinsic。“PointsUndistorted”， 真的，“AbsoluteTolerance”1 e-12．..“RelativeTolerance”1 e-12“MaxIterations”, 200,'FixedViewID'，1）;VSET =更新视图（VSET，camPoses）;%更新视图集。% Bundle调整可以移动整个摄像机集。规范化的％视野设定放置在原点第一摄像机沿寻找% z轴和调整比例，以匹配的地面真相。VSET = helperNormalizeViewSet（VSET，groundTruthPoses）;%更新摄像机轨迹图。helperupdatecameraplot (viewId, camEstimated, camActual, pose (vSet)，．..groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated,．..trajectoryActual,姿势(vSet) groundTruthPoses);prevI =我;prevFeatures = currFeatures;prevPoints = currPoints;结束

使用加窗束调整估计剩余的摄像机轨迹

通过使用加窗束调节到只细化最后15次，以限制计算量估计剩余相机轨迹。此外，束调整不必调用每一个观点，因为estimateWorldCameraPose以与三维点相同的单位计算姿态。本节为每7个视图调用bundle调整。实验选择了调用束调整的窗口大小和频率。

为viewId = 16:元素个数(images.Files)%读取并显示下一个图像Irgb = readimage(images, viewwid);步骤(球员,Irgb);％转换为灰度级和undistort。I = undistortion timage (im2gray(Irgb)， intrinsics);%先前图像和当前图像之间的匹配点。[currPoints，currFeatures，indexPairs] = helperDetectAndMatchFeatures（．..prevFeatures,我);从前面两个视图中三角化点，并找到％在当前视图中对应的点。[worldPoints, imagePoints] = helperfind3dto2d通信(vSet，．..intrinsic、indexPairs currPoints);％与RANSAC涉及一个随机过程，有时不％达到预期的置信水平和超越的最大数量%试验。当它发生时禁用警告，因为结果是%仍然有效。warningstate =警告('离开'，“愿景:ransac: maxTrialsReached”）;％估算当前世界观相机姿态。[orient, loc] = estimateWorldCameraPose(imagePoints, worldPoints, intrinsics);%恢复原来的警告状态警告(warningstate)％添加当前视图和连接到视图集。VSET = addView（VSET，viewId，rigid3d（东方，LOC），“点”, currPoints);vSet = addConnection(vSet, viewId-1, viewId，)'火柴'，indexPairs）;％精确估计使用加窗束调整相机的姿势。跑％优化每7视图。如果mod(viewId, 7) == 0找到过去15个视图中的点轨迹并进行三角定位。windowSize = 15;startFrame将= MAX（1，viewId  -  windowSize）;曲目= findTracks（VSET，startFrame将：viewId）;camPoses =姿势（VSET，startFrame将：viewId）;[xyzPoints，reprojErrors] = triangulateMultiview（轨道，camPoses，内在）;保持前两个姿势固定，保持相同的比例。fixedIds = [startFrame, startFrame+1];排除重投影误差高的点和轨道。IDX = reprojErrors <2;[〜，camPoses] = bundleAdjustment（xyzPoints（IDX，:)，轨道（IDX），．..坎波斯,intrinsic'FixedViewIDs'fixedIds,．..“PointsUndistorted”， 真的，“AbsoluteTolerance”1 e-12．..“RelativeTolerance”1 e-12“MaxIterations”，200）;VSET =更新视图（VSET，camPoses）;%更新视图集。结束%更新摄像机轨迹图。helperupdatecameraplot (viewId, camEstimated, camActual, pose (vSet)，．..groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated,．..trajectoryActual,姿势(vSet) groundTruthPoses);prevI =我;prevFeatures = currFeatures;prevPoints = currPoints;结束

持有离开

总结

这个例子展示了如何从一个序列的视图估计一个校准的单目相机的轨迹。注意，估计的轨迹并不完全符合地面的真实情况。尽管相机位姿的非线性细化，但相机位姿估计的误差累积，导致漂移。在视觉测程系统中，这个问题通常是通过融合来自多个传感器的信息和执行环路闭合来解决的。

参考文献

[1] Martin Peris Martorell, Atsuto Maki, Sarah Martull, Yasuhiro Ohkawa, Kazuhiro Fukui，“走向模拟驱动的立体视觉系统”。acta ICPR sinica, pp.1038-1042, 2012。

[2]萨拉Martull，马丁潜望马尔托雷尔，福井一弘，“现实的CG立体图像数据集与地面实况视差图”，ICPR车间TrakMark2012，pp.40-42，2012年论文集。

[3] M.I.A.Lourakis和A.A.Argyros（2009年）。“SBA：一个软件包用于通用稀疏捆绑调整”。ACM交易在数学软件（ACM），36（1）：1-30。

[4] R. Hartley, A. Zisserman，“计算机视觉中的多视图几何”，剑桥大学出版社，2003。

[5] B. Triggs;P. McLauchlan;R.哈特利;A.菲茨吉本（1999年）。“光束法平差：现代合成”。国际研讨会视觉算法的程序。施普林格出版社。第298-372。

[6] X.-S。高,X.-R。侯俊华，唐俊华，侯俊华。程，“透视-三点问题的完整解决方案分类”，IEEE Trans。模式分析与机器智能，vol. 25, no. 28，页930-943,2003。