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

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

属性中使用camera intrinsics创建camera intrinsics对象%新筑波数据集。focalLength = [615 615];%以像素为单位指定principalPoint = [320 240];%(像素)[x, y]图象尺寸大小= (Irgb [1, 2]);%(像素)[mrows, ncols]intrinsics = cameraIntrinsics(focalLength, principalPoint, imageSize);

prevI = undistortion timage (im2gray(Irgb)， intrinsics); / /本质%检测功能。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轴垂直向下。甘氨胆酸视图(3);集(gca),“CameraUpVector”， [0， - 1,0]);甘氨胆酸camorbit (-120 0“数据”， [0,1, 0]);网格在包含(“X (cm)”）;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);初始化摄像机轨迹。轨迹估计= plot3(0,0,0，“g -”）;轨迹= plot3(0,0,0，“b -”）;传奇(“估计轨迹”，“实际轨迹”）;标题(“相机轨迹”）;

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

%转换为灰度和不失真。I = undistortion timage (im2gray(Irgb)， intrinsics);%匹配先前图像和当前图像之间的特征。[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(．..prevFeatures,我);估算当前视图相对于前一个视图的姿态。[orient, loc, inlierIdx] = helperEstimateRelativePose(．..prevPoints (indexPairs (: 1)), currPoints (indexPairs (:, 2)), intrinsic);%排除极线异常值。indexPairs = indexPairs(inlierIdx，:);将当前视图添加到视图集。vSet = addView(vSet, viewId, rigid3d(orient, loc))，“点”, currPoints);%存储前一个视图和当前视图之间的点匹配。vSet = addConnection(vSet, viewId-1, viewId，)“匹配”, indexPairs);

vSet = helperNormalizeViewSet(vSet, groundtruthpose);

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

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

为viewId = 3:15%读取并显示下一个图像Irgb = readimage(images, viewwid);步骤(球员,Irgb);%转换为灰度和不失真。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(orient, loc))，“点”, currPoints);%存储前一个视图和当前视图之间的点匹配。vSet = addConnection(vSet, viewId-1, viewId，)“匹配”, indexPairs);跟踪= findTracks (vSet);查找点轨迹跨越多个视图。坎波斯=姿势(vSet);为所有视图设置相机姿势。三角定位三维世界点的初始位置。xyzPoints = triangulateMultiview(tracks, campose, intrinsics);使用束调整优化相机姿态。[~， campose] = bundleAdjustment(xyzPoints, tracks, campose，．..intrinsic。“PointsUndistorted”,真的,“AbsoluteTolerance”1 e-12．..“RelativeTolerance”1 e-12“MaxIterations”, 200,“FixedViewID”1);vSet = updateView(vSet, camPoses);%更新视图集。% Bundle调整可以移动整个摄像机集。规范化的%视图设置，将第一个摄像机放置在原点% z轴和调整比例，以匹配的地面真相。vSet = helperNormalizeViewSet(vSet, groundtruthpose);%更新摄像机轨迹图。helperupdatecameraplot (viewId, camEstimated, camActual, pose (vSet)，．..groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated,．..trajectoryActual,姿势(vSet) groundTruthPoses);prevI =我;prevFeatures = currFeatures;prevPoints = currPoints;结束

为viewId = 16:元素个数(images.Files)%读取并显示下一个图像Irgb = readimage(images, viewwid);步骤(球员,Irgb);%转换为灰度和不失真。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(orient, loc))，“点”, currPoints);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); / /播放视频保持前两个姿势固定，保持相同的比例。fixedIds = [startFrame, startFrame+1];排除重投影误差高的点和轨道。idx = reprojErrors < 2;[~， camPoses] = bundleAdjustment(xyzPoints(idx，:)， tracks(idx)，．..坎波斯,intrinsic“FixedViewIDs”fixedIds,．..“PointsUndistorted”,真的,“AbsoluteTolerance”1 e-12．..“RelativeTolerance”1 e-12“MaxIterations”, 200);vSet = updateView(vSet, camPoses);%更新视图集。结束%更新摄像机轨迹图。helperupdatecameraplot (viewId, camEstimated, camActual, pose (vSet)，．..groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated,．..trajectoryActual,姿势(vSet) groundTruthPoses);prevI =我;prevFeatures = currFeatures;prevPoints = currPoints;结束

持有从