单眼视觉测程法

图像= imageDatastore (fullfile (toolboxdir (“愿景”),“visiondata”,“NewTsukuba”));%负载地面实况相机姿势。负载(“visualOdometryGroundTruth.mat”)

%创建一个空imageviewset对象管理与每个视图关联的数据。vSet = imageviewset;%第一图像读取和显示。Irgb = readimage(图片1);球员=愿景。放像机(位置= [400、650、510]);步骤(球员,Irgb);

%的相机使用相机intrinsic intrinsic对象创建%筑波的新数据集。focalLength = (615 - 615);%以像素为单位指定principalPoint = (320 - 240);%的像素(x, y)图象尺寸大小= (Irgb [1, 2]);%在像素(mrows, ncols)intrinsic = cameraIntrinsics (focalLength principalPoint图象尺寸);

prevI = undistortImage (im2gray (Irgb), intrinsic);%检测功能。prevPoints = detectSURFFeatures (prevI MetricThreshold = 500);%选择特性的一个子集,均匀分布在整个图像。numPoints = 200;prevPoints = selectUniform (prevPoints numPoints、大小(prevI));%提取特征。使用“正直”特性提高匹配质量%摄像机运动涉及很少或没有平面旋转。prevFeatures = extractFeatures (prevI prevPoints,直立= true);%添加第一个视图。与第一个视图相关联的摄像机%在原点,面向沿z轴。viewId = 1;vSet = addView (vSet viewId rigidtform3d(眼(3),[0 0 0]),点= prevPoints);

%设置坐标轴。图轴([-220,-140,-50,300]);%设置轴是垂直向下。甘氨胆酸视图(3);集(gca, CameraUpVector = (0,1,0));甘氨胆酸camorbit (-120 0, data = (0,1,0));网格在包含(“X (cm)”);ylabel (“Y (cm)”);zlabel (“Z (cm)”);持有在%情节估计相机姿势。cameraSize = 7;camPose =姿势(vSet);camEstimated = plotCamera (camPose、大小= cameraSize,…颜色=“g”,透明度= 0);%情节实际相机姿势。= cameraSize camActual = plotCamera(大小,…AbsolutePose = rigidtform3d (groundTruthPoses.Orientation {1}”,…groundTruthPoses.Location {1}),…颜色=“b”,透明度= 0);%初始化相机轨迹。trajectoryEstimated = plot3 (0, 0, 0,“g -”);trajectoryActual = plot3 (0, 0, 0,“b -”);传奇(“估计轨迹”,“实际轨迹”);标题(“相机轨迹”);

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

%转换为灰度和undistort。我= undistortImage (im2gray (Irgb), intrinsic);%前面和当前图像之间的匹配特性。[currPoints, currFeatures indexPairs] = helperDetectAndMatchFeatures (…prevFeatures,我);%估计当前视图的构成相对于前面的视图。[relPose, inlierIdx] = helperEstimateRelativePose (…prevPoints (indexPairs (: 1)), currPoints (indexPairs (:, 2)), intrinsic);%排除纵向离群值。indexPairs = indexPairs (inlierIdx:);%设置当前视图添加到视图。vSet = addView (vSet viewId relPose,分= currPoints);%之前存储之间的匹配点和当前视图。vSet = addConnection (vSet viewId-1 viewId,匹配= indexPairs);

vSet = helperNormalizeViewSet (vSet groundTruthPoses);

helperUpdateCameraPlots (viewId camEstimated camActual,姿势(vSet),…groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated trajectoryActual,…姿势(vSet) groundTruthPoses);

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

为viewId = 3:15%读取和显示未来的形象Irgb = readimage(图像、viewId);步骤(球员,Irgb);%转换为灰度和undistort。我= undistortImage (im2gray (Irgb), intrinsic);%以往和当前图像之间的匹配点。[currPoints, currFeatures indexPairs] = helperDetectAndMatchFeatures (…prevFeatures,我);%消除异常值的特征匹配。[~,inlierIdx] = helperEstimateRelativePose (prevPoints (indexPairs (: 1)),…currPoints (indexPairs (:, 2)), intrinsic);indexPairs = indexPairs (inlierIdx:);%满足分前两个观点,和发现%在当前视图对应点。[worldPoints, imagePoints] = helperFind3Dto2DCorrespondences (vSet,…intrinsic、indexPairs currPoints);%因为RANSAC包括一个随机过程,它可能有时不是%达到所需的信心水平和超过最大数量的%试验。禁用警告当以来发生的结果%仍然有效。warningstate =警告(“关闭”,“愿景:ransac: maxTrialsReached”);%估计世界相机姿势当前视图。absPose = estworldpose (imagePoints、worldPoints intrinsic);%恢复原始警告状态警告(warningstate)%设置当前视图添加到视图。vSet = addView (vSet viewId absPose,分= currPoints);%之前存储之间的匹配点和当前视图。vSet = addConnection (vSet viewId-1 viewId,匹配= indexPairs);跟踪= findTracks (vSet);%找到点跟踪生成多个视图。坎波斯=姿势(vSet);%获得所有视图的相机姿势。%满足初始点位置的3 d世界。xyzPoints = triangulateMultiview(追踪,坎波斯,intrinsic);%改进相机使用束调整姿势。(~,campos) = bundleAdjustment (xyzPoints,跟踪,坎波斯,…intrinsic PointsUndistorted = true, AbsoluteTolerance e-12 = 1,…RelativeTolerance = 1 e-12 MaxIterations = 200, FixedViewID = 1);vSet = updateView (vSet campos);%更新视图组。%束调整可以移动整个相机。规范化的%视图设置为第一个相机在原点沿% z轴和调整的规模相匹配,地面真理。vSet = helperNormalizeViewSet (vSet groundTruthPoses);%更新相机轨迹图。helperUpdateCameraPlots (viewId camEstimated camActual,姿势(vSet),…groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated,…trajectoryActual,姿势(vSet) groundTruthPoses);prevI =我;prevFeatures = currFeatures;prevPoints = currPoints;结束

为viewId = 16:元素个数(images.Files)%读取和显示未来的形象Irgb = readimage(图像、viewId);步骤(球员,Irgb);%转换为灰度和undistort。我= undistortImage (im2gray (Irgb), intrinsic);%以往和当前图像之间的匹配点。[currPoints, currFeatures indexPairs] = helperDetectAndMatchFeatures (…prevFeatures,我);%满足分前两个观点,和发现%在当前视图对应点。[worldPoints, imagePoints] = helperFind3Dto2DCorrespondences (vSet,…intrinsic、indexPairs currPoints);%因为RANSAC包括一个随机过程,它可能有时不是%达到所需的信心水平和超过最大数量的%试验。禁用警告当以来发生的结果%仍然有效。warningstate =警告(“关闭”,“愿景:ransac: maxTrialsReached”);%估计世界相机姿势当前视图。absPose = estworldpose (imagePoints、worldPoints intrinsic);%恢复原始警告状态警告(warningstate)%将当前视图和连接添加到视图集。vSet = addView (vSet viewId absPose,分= currPoints);vSet = addConnection (vSet viewId-1 viewId,匹配= indexPairs);%改进估计相机使用窗口的束调整姿势。运行%优化每7日视图。如果国防部(viewId 7) = = 0%找到跟踪在过去15观点和满足。windowSize = 15;startFrame = max (1, viewId - windowSize);跟踪= findTracks (vSet startFrame: viewId);坎波斯=姿势(vSet startFrame: viewId);[xyzPoints, reprojErrors] = triangulateMultiview(追踪,坎波斯,intrinsic);%的前两个姿势固定,保持相同的规模。fixedIds = [startFrame startFrame + 1];%排除分和高reprojection跟踪错误。idx = reprojErrors < 2;(~,campos) = bundleAdjustment (xyzPoints (idx:),追踪(idx),…坎波斯,intrinsic FixedViewIDs = fixedIds,…PointsUndistorted = true, AbsoluteTolerance e-12 = 1,…RelativeTolerance = 1 e-12 MaxIterations = 200);vSet = updateView (vSet campos);%更新视图组。结束%更新相机轨迹图。helperUpdateCameraPlots (viewId camEstimated camActual,姿势(vSet),…groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated,…trajectoryActual,姿势(vSet) groundTruthPoses);prevI =我;prevFeatures = currFeatures;prevPoints = currPoints;结束

持有从