单眼视觉测程法

打开生活的脚本

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

概述

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

在没有附加信息的情况下，单目相机的轨迹只能恢复到一个未知的比例因子。在移动机器人或自动车辆上使用的单目视觉里程测量系统通常从另一个传感器(例如车轮里程计或GPS)或场景中已知大小的对象获得比例因子。这个例子从地面真实值计算尺度因子。

本例分为三个部分:

估计相对于第一个视图的第二个视图的姿态。通过估计必要矩阵并将其分解成照相机的位置和方向估计所述第二视图的姿态。
自举使用全局束调整估计相机轨迹。消除使用极约束异常值。查找以前的两个视图和当前视图三角测量点之间的3D到2D对应。通过求解透视n点（PNP）的问题计算当前世界观相机姿态。估计摄影机姿态必然导致错误，随着时间累积。这种效应被称为漂移。为了减少漂移，示例提炼所有姿态估计迄今使用束调整。
估计剩余的相机轨迹使用窗口束调整。随着每一个新观点需要改进的所有姿势的时间增加。窗捆绑调整仅是通过优化最后减少计算时间的方式ñ视图，而不是整个轨迹。不为每个视图调用bundle adjustment进一步减少了计算时间。

读取输入图像序列和地面实测

实例中的图像新的筑波立体数据集筑波大学的CVLAB创建。如果您在自己的工作或出版物使用这些照片，请引用以下文章：

[1]马丁·佩里斯Martorell的，笃真希，萨拉Martull，中曾根大川，一弘福井，“建立一个仿真驱动的立体视觉系统”。ICPR，pp.1038-1042，2012年提起诉讼。

“基于地面真实视差图的真实CG立体影像数据集”，国立中山大学计算机科学与工程学院学报，2012，页40-42,2012。

图像= imageDatastore（完整文件（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对象%新的筑波数据集。focalLength = [615 615];%以像素为单位指定principal = [320240];％中的像素[X，Y]图象尺寸大小= (Irgb [1, 2]);%(单位为像素)[mrows, ncols]内在= cameraIntrinsics（长焦点，principalPoint，IMAGESIZE）;

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

prevI = un失真(rgb2gray(Irgb)， intrinsics);%检测功能。prevPoints = detectSURFFeatures (prevI,“MetricThreshold”, 500);%选择特征子集，均匀分布于整个图像。numPoints = 200;(prevPoints, numPoints, size(prevI));%提取特征。使用“直立”特征可以提高匹配质量％相机运动包括很少或没有的平面内旋转。提取特征(prevI, prevPoints，'直立'，真正的）;%添加第一个视图。放置与第一个视图关联的摄像机%在原点，沿z轴方向。viewId = 1;VSET = addView（VSET，viewId，rigid3d（眼（3），[0 0 0]），“点”，prevPoints）;

初始相机姿态

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

%设置坐标轴。图轴（[ -  220，50，-140，20，-50，300]）;%设置y轴垂直向下。甘氨胆酸视图(3);集(gca),'CameraUpVector'， [0， -1, 0]);甘氨胆酸camorbit (-120 0“数据”， [0,1,0]);网格在包含('X（厘米）');ylabel（“Y (cm)”);zlabel (“Z (cm)”);保持在绘制估计相机姿态图。cameraSize = 7;camPose =姿势（VSET）;camEstimated = plotCamera（camPose，“大小”cameraSize,...“颜色”，‘g’，“透明度”, 0);绘制实际相机姿态。camActual = plotCamera (“大小”cameraSize,“AbsolutePose”，...rigid3d (groundTruthPoses。定位{1}、groundTruthPoses.Location {1}),...“颜色”，'B'，“透明度”, 0);％初始化相机的轨迹。trajectoryEstimated = plot3（0，0，0，“g -”);plot3(0,0,0，“b -”);传奇(“估计的轨迹”，“实际轨迹”);标题(“相机轨迹”);

估计第二个视图的姿态

检测，并从第二视图中提取特征，和使用它们匹配到第一视图helperDetectAndMatchFeatures。使用估计所述第二视图相对于第一视图的姿态helperEstimateRelativePose，并将其加入imageviewset。

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

%转换为灰度和不扭曲。I = undistortImage（rgb2gray（IRGB），内联函数）;在以前的图像和当前图像之间匹配特征。[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(...prevFeatures,我);％估计当前相对于前视图的视图的姿态。[orient, loc, inlierIdx] = helperEstimateRelativePose(...prevPoints (indexPairs (: 1)), currPoints (indexPairs (:, 2)), intrinsic);%排除外极异常值。indexPairs = indexPairs（inlierIdx，:);%将当前视图添加到视图集。addView(vSet, viewId, rigid3d(orient, loc)，“点”，currPoints）;%存储前一个视图和当前视图之间的匹配点。VSET = addConnection（VSET，viewId-1，viewId，“匹配”, indexPairs);

第二视图相对于第一视图的位置只能被回收到一个未知的比例因子。计算从地面实况使用比例因子helperNormalizeViewSet，模拟一个外部传感器，将用于一个典型的单眼视觉测程系统。

vSet = helperNormalizeViewSet(vSet, groundTruthPoses);

更新相机轨迹图使用helperUpdateCameraPlots和helperUpdateCameraTrajectories。

helperupdatecameraplot (viewId, camestimate, camActual, poses(vSet)，...groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated trajectoryActual,...姿势(vSet) groundTruthPoses);

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

Bootstrap估计相机轨迹使用全局束调整

找出前两个视图中三角化的世界点与当前视图中的图像点之间的3d - 2d对应关系。使用helperFindEpipolarInliers找到满足外极约束的匹配项，然后使用helperFind3Dto2DCorrespondences将前两个视图中的三维点进行三角化，并在当前视图中找到相应的二维点。

利用视角-n-点(perspective-n-point, PnP)问题求解当前视图的世界相机姿态estimateWorldCameraPose。对于第15次，使用全局束调整完善整个轨迹。使用全局束调整为视图自举估计相机轨迹的其余部分的数量有限，并且不昂贵。

为viewId = 3:15读取并显示下一个图像Irgb = readimage(images, viewId);步骤(球员,Irgb);%转换为灰度和不扭曲。I = undistortImage（rgb2gray（IRGB），内联函数）;以前的图像和当前图像之间的%匹配点。[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(...prevFeatures,我);从特征匹配中消除异常值。inlierIdx = helperFindEpipolarInliers (prevPoints (indexPairs (: 1)),...currPoints（indexPairs（：，2）），内联函数）;indexPairs = indexPairs（inlierIdx，:);从前两个视图中三角化点，并找到%当前视图中的对应点。helperfind3dto2dcommunicdences (vSet，...intrinsic、indexPairs currPoints);%由于RANSAC涉及一个随机过程，它有时可能不%达到所需的置信水平，并且超过的最大值％试验。禁用警告这种情况发生时，因为结果是%仍然有效。warningstate =警告（“关闭”，'愿景：RANSAC：maxTrialsReached');估计当前视图的世界相机姿态。[取向，LOC] = estimateWorldCameraPose（imagePoints，worldPoints，内在）;%恢复原来的警告状态警告(warningstate)%将当前视图添加到视图集。addView(vSet, viewId, rigid3d(orient, loc)，“点”，currPoints）;%存储前一个视图和当前视图之间的匹配点。VSET = addConnection（VSET，viewId-1，viewId，“匹配”, indexPairs);跟踪= findTracks (vSet);％查找点轨道跨越多个视图。坎波斯=姿势(vSet);％获取所有视图摄影机姿态。％三角测量为3 d世界点初始位置。xyzPoints = triangulateMultiview(tracks, campose, intrinsics);使用束调整优化相机姿态。(xyzPoints, tracks, camPoses, xyzPoints, tracks, camPoses, xyzPoints, tracks, camPoses, xyzPoints)...内部函数，'PointsUndistorted',真的,'AbsoluteTolerance'1 e-12...'RelativeTolerance'1 e-12'MaxIterations', 200,“FixedViewID”1);vSet = updateView(vSet, camPoses);％更新视图集。%束调整可以移动整组相机。规范化的% view设置为将第一个摄像机放置在原点，沿着% z轴，并调整比例，以匹配地面真相。vSet = helperNormalizeViewSet(vSet, groundTruthPoses);更新相机轨迹图。helperupdatecameraplot (viewId, camestimate, camActual, poses(vSet)，...groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated,...trajectoryActual,姿势(vSet) groundTruthPoses);prevI =我;prevFeatures = currFeatures;prevPoints = currPoints;结束

估计剩余的相机轨迹使用窗口束调整

估计剩余的相机轨迹，使用窗口束调整，只细化最后15个视图，以限制计算量。此外，捆绑调整不必为每个视图调用，因为estimateWorldCameraPose计算在相同的单位3-d的点的姿态。这部分要求每7观点束调整。窗口的大小和主叫束调整的频率已被实验选择。

为viewId = 16:元素个数(images.Files)读取并显示下一个图像Irgb = readimage(images, viewId);步骤(球员,Irgb);%转换为灰度和不扭曲。I = undistortImage（rgb2gray（IRGB），内联函数）;以前的图像和当前图像之间的%匹配点。[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(...prevFeatures,我);从前两个视图中三角化点，并找到%当前视图中的对应点。helperfind3dto2dcommunicdences (vSet，...intrinsic、indexPairs currPoints);%由于RANSAC涉及一个随机过程，它有时可能不%达到所需的置信水平，并且超过的最大值％试验。禁用警告这种情况发生时，因为结果是%仍然有效。warningstate =警告（“关闭”，'愿景：RANSAC：maxTrialsReached');估计当前视图的世界相机姿态。[取向，LOC] = estimateWorldCameraPose（imagePoints，worldPoints，内在）;%恢复原来的警告状态警告(warningstate)%添加当前视图和连接到视图集。addView(vSet, viewId, rigid3d(orient, 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 = pose (vSet, startFrame:viewId);= triangulateMultiview(tracks, camPoses, intrinsics);％握住固定前两个姿势，保持同样的规模。fixedIds = [startFrame将，startFrame将+ 1];％排除点和轨道与高重投影误差。idx =错误原因< 2;[~， camPoses] = bundleAdjustment(xyzPoints(idx，:)， tracks(idx)，...camPoses，内部函数，“FixedViewIDs”，fixedIds，...'PointsUndistorted',真的,'AbsoluteTolerance'1 e-12...'RelativeTolerance'1 e-12'MaxIterations', 200);vSet = updateView(vSet, camPoses);％更新视图集。结束更新相机轨迹图。helperupdatecameraplot (viewId, camestimate, camActual, poses(vSet)，...groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated,...trajectoryActual,姿势(vSet) groundTruthPoses);prevI =我;prevFeatures = currFeatures;prevPoints = currPoints;结束