单目视觉里程计- MATLAB & Simulink万博1manbetx - 万博1manbetx,s manbetx 845,万博尤文图斯

单目视觉里程计

打开脚本

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

概述

这个例子展示了如何从一系列二维视图中估计校准相机的轨迹。这个例子使用的图像来自筑波大学CVLAB创建的新筑波立体数据集。（http://cvlab.cs.tsukuba.ac.jp)。该数据集由使用计算机图形生成的合成图像组成，并包括地面真实相机姿势。

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

本例分为三个部分:

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

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

这个例子使用的图像来自筑波大学CVLAB创建的新筑波立体数据集。（http://cvlab.cs.tsukuba.ac.jp)。如果您在自己的作品或出版物中使用这些图像，请引用以下论文:

[10] Martin Peris Martorell, Atsuto Maki, Sarah Martull, Yasuhiro Ohkawa, Kazuhiro Fukui，“走向仿真驱动的立体视觉系统”。中国生物医学工程学报，pp.1038-1042, 2012。

[10]张晓东，张晓东，“基于gis的三维立体图像数据集”，计算机工程学报，2012,pp.40-42, 2012。

images = imageDatastore(fullfile(toolboxdir))“愿景”)，“visiondata”，…“NewTsukuba”）);创建相机参数对象，使用相机的内在属性%新的筑波数据集。K = [615 0 320;0 615 240;[0 0 1];cameraParams = cameraParameters(“IntrinsicMatrix”、K);%加载地面真实的相机姿势。负载(fullfile (toolboxdir (“愿景”)，“visiondata”，…“visualOdometryGroundTruth.mat”）);

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

使用一个viewSet对象来存储和管理与每个视图相关联的图像点和相机姿态，以及视图对之间的点匹配。一旦你填充了viewSet对象，您可以使用它来查找跨多个视图的点轨迹并检索要使用的相机姿势triangulateMultiview和bundleAdjustment功能。

创建一个空的viewSet对象来管理与每个视图相关的数据。vSet = viewSet;读取并显示第一个图像。Irgb = readimage(images, 1);玩家=视野。放像机(“位置”， [20,400,650,510]);步骤(球员,Irgb);

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

prei = undistortion timage (rgb2gray(Irgb)， cameraParams);%检测功能。prevPoints = detectsurfeatures (prei，“MetricThreshold”, 500);选择特征的子集，均匀分布在整个图像中。numPoints = 150;prevPoints = selectUniform(prevPoints, numPoints, size(prevI));%提取特性。使用“直立”功能可以提高匹配质量摄像机的运动很少或根本不涉及平面内旋转。prevFeatures = extractFeatures(prevI, prevPoints，“正直”,真正的);%添加第一个视图。放置与第一个视图相关联的相机%在原点，沿z轴方向。viewId = 1;vSet = addView(vSet, viewId，“点”prevPoints,“定位”眼睛(3),…“位置”， [0 0 0]);

初始镜头姿势

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

%设置轴。图轴([-220,50，-140,20，-50,300]);设置y轴垂直向下。甘氨胆酸视图(3);集(gca),“CameraUpVector”， [0， -1, 0]);Camorbit (gca， - 1200,0;“数据”， [0,1,0]);网格在包含(“X (cm)”);ylabel (“Y (cm)”);zlabel (“Z (cm)”);持有在%情节估计相机姿势。cameraSize = 7;camEstimated = plotCamera()“大小”cameraSize,“位置”，…vSet.Views.Location {1},“定位”, vSet.Views.Orientation {1},…“颜色”，‘g’，“不透明度”, 0);%绘制实际相机姿势。camActual = plotCamera()“大小”cameraSize,“位置”，…groundTruthPoses。位置{1},“定位”，…groundTruthPoses。取向{1},“颜色”，“b”，“不透明度”, 0);初始化相机轨迹。轨迹估计= plot3(0,0,0，“g -”);轨迹= plot3(0,0,0，“b -”);传奇(“估计轨迹”，“实际轨迹”);标题(“相机轨迹”);

估计第二视图的姿态

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

%读取并显示图像。viewId = 2;Irgb = readimage(images, viewId);步骤(球员,Irgb);转换为灰度和不扭曲。I = undistortion timage (rgb2gray(Irgb)， cameraParams);%匹配上一张图像和当前图像之间的特征。[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures…prevFeatures,我);估计当前视图相对于前一个视图的姿态。[orient, loc, inlierIdx] = helperEstimateRelativePose(…prevPoints(indexPairs(:，1))， currPoints(indexPairs(:，2))， cameraParams);%排除极外异常值。indexPairs = indexPairs(inlierIdx，:);%将当前视图添加到视图集。vSet = addView(vSet, viewId，“点”currPoints,“定位”东方,…“位置”、loc);%存储前一个视图和当前视图之间的点匹配。vSet = addConnection(vSet, viewId-1, viewId，“匹配”, indexPairs);

第二个视图相对于第一个视图的位置只能恢复到未知的比例因子。从地面真值计算比例因子helperNormalizeViewSet，模拟用于典型单目视觉里程计系统的外部传感器。

vSet = helperNormalizeViewSet(vSet, groundtruthpose);

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

helperUpdateCameraPlots(viewId, camestimate, camActual, pose (vSet)，…groundTruthPoses);viewId, trajectoryEstimated, trajectoryActual，…姿势(vSet) groundTruthPoses);prevI = I;prevFeatures = currFeatures;prevPoints = currPoints;

使用全局束调整的自举估计相机轨迹

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

通过求解视角-n-点(PnP)问题，计算当前视图的世界相机姿态estimateWorldCameraPose．对于前15个视图，使用全局包调整来细化整个轨迹。对有限数量的视图使用全局包调整来估计相机轨迹的其余部分，并且它不是非常昂贵。

为viewId = 3:15%读取并显示下一个图像Irgb = readimage(images, viewId);步骤(球员,Irgb);转换为灰度和不扭曲。I = undistortion timage (rgb2gray(Irgb)， cameraParams);%前一个图像和当前图像之间的匹配点。[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures…prevFeatures,我);从特征匹配中消除异常值。inlierIdx = helperFindEpipolarInliers(prevPoints(indexPairs(:，1))，…currPoints(indexPairs(:， 2))， cameraParams);indexPairs = indexPairs(inlierIdx，:);对前两个视图中的点进行三角测量，并找到%当前视图中对应的点。[worldPoints, imagePoints] = helperfind3dto2dcorresponences (vSet，…cameraParams, indexPairs, currPoints);由于RANSAC涉及一个随机过程，因此有时可能不是%达到所需的置信水平，并且超过了的最大数目%试验。当发生这种情况时禁用警告，因为结果是%仍然有效。Warningstate =警告(“关闭”，“愿景:ransac: maxTrialsReached”);估计当前视图的世界相机姿态。[orient, loc] = estimateWorldCameraPose(imagePoints, worldPoints，…cameraParams,“信心”, 99.99,“MaxReprojectionError”, 0.8);%恢复原始警告状态警告(warningstate)%将当前视图添加到视图集。vSet = addView(vSet, viewId，“点”currPoints,“定位”东方,…“位置”、loc);%存储前一个视图和当前视图之间的点匹配。vSet = addConnection(vSet, viewId-1, viewId，“匹配”, indexPairs);轨道= findTracks(vSet);查找跨越多个视图的点轨迹。campose = pose (vSet);获取所有视图的相机姿势。对3-D世界点的初始位置进行三角测量。xyzPoints = triangulateMultiview(tracks, campose, cameraParams);使用束调整优化相机姿势。[~， campose] = bundleAdjustment(xyzPoints, tracks, campose，…cameraParams,“PointsUndistorted”,真的,“AbsoluteTolerance”1 e-9…“RelativeTolerance”1 e-9“MaxIterations”, 300);vSet = updateView(vSet, campose);%更新视图集。%束调整可以移动整套摄像机。规范化的% view设置将第一个摄像机放置在原点沿% z轴，并调整比例，以匹配地面真相。vSet = helperNormalizeViewSet(vSet, groundtruthpose);%更新相机轨迹图。helperUpdateCameraPlots(viewId, camestimate, camActual, pose (vSet)，…groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated,…轨迹实际，姿态(vSet)， groundtruthpose);prevI = I;prevFeatures = currFeatures;prevPoints = currPoints;结束

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

通过使用窗口束调整来估计剩余的摄像机轨迹，仅细化最后15个视图，以限制计算量。此外，不必为每个视图调用bundle调整，因为estimateWorldCameraPose以与三维点相同的单位计算姿态。该部分每隔第7个视图调用bundle调整。实验选择了窗口大小和调用束调整的频率。

为viewId = 16: null (images.Files)%读取并显示下一个图像Irgb = readimage(images, viewId);步骤(球员,Irgb);转换为灰度和不扭曲。I = undistortion timage (rgb2gray(Irgb)， cameraParams);%前一个图像和当前图像之间的匹配点。[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures…prevFeatures,我);对前两个视图中的点进行三角测量，并找到%当前视图中对应的点。[worldPoints, imagePoints] = helperfind3dto2dcorresponences (vSet，…cameraParams, indexPairs, currPoints);由于RANSAC涉及一个随机过程，因此有时可能不是%达到所需的置信水平，并且超过了的最大数目%试验。当发生这种情况时禁用警告，因为结果是%仍然有效。Warningstate =警告(“关闭”，“愿景:ransac: maxTrialsReached”);估计当前视图的世界相机姿态。[orient, loc] = estimateWorldCameraPose(imagePoints, worldPoints，…cameraParams,“MaxNumTrials”, 5000,“信心”, 99.99,…“MaxReprojectionError”, 0.8);%恢复原始警告状态警告(warningstate)%将当前视图和连接添加到视图集。vSet = addView(vSet, viewId，“点”currPoints,“定位”东方,…“位置”、loc);vSet = addConnection(vSet, viewId-1, viewId，“匹配”, indexPairs);使用窗口束调整来细化估计的相机姿势。运行%优化每7个视图。如果mod(viewId, 7) == 0在最近15个视图中找到点轨道并进行三角测量。windowSize = 15;startFrame = max(1, viewId - windowSize);轨道= findTracks(vSet, startFrame:viewId);campose = pose (vSet, startFrame:viewId);[xyzPoints, rejerrors] = triangulateMultiview(tracks, campose，…cameraParams);保持前两个姿势固定，以保持相同的比例。fixedIds = [startFrame, startFrame+1];排除重投影误差高的点和轨迹。idx = rejerrors < 2;[~， campose] = bundleadadjustment (xyzPoints(idx，:)， tracks(idx)，…坎波斯,cameraParams“FixedViewIDs”fixedIds,…“PointsUndistorted”,真的,“AbsoluteTolerance”1 e-9…“RelativeTolerance”1 e-9“MaxIterations”, 300);vSet = updateView(vSet, campose);%更新视图集。结束%更新相机轨迹图。helperUpdateCameraPlots(viewId, camestimate, camActual, pose (vSet)，…groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated,…轨迹实际，姿态(vSet)， groundtruthpose);prevI = I;prevFeatures = currFeatures;prevPoints = currPoints;结束持有从