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

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

类中的相机intrinsic对象创建相机intrinsic对象%新的Tsukuba数据集。focalLength = [615 615];%以像素为单位指定principalPoint = [320 240];%(像素[x, y])imageSize = size(Irgb，[1,2]);%(像素)[mrows, ncols]intrinsic = cameraIntrinsics(focalLength, principalPoint, imageSize);

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

%设置轴。图轴([-220,50，-140,20，-50,300]);设置y轴为垂直向下。甘氨胆酸视图(3);set(gca, CameraUpVector=[0， - 1,0]);Camorbit (gca， -120, 0, data=[0,1,0]);网格在包含(“X (cm)”）;ylabel (“Y (cm)”）;zlabel (“Z (cm)”）;持有在绘制估计的相机姿势。cameraSize = 7;camPose =姿势(vSet);camestimate = plotCamera(camPose, Size=cameraSize，.．.颜色=“g”,透明度= 0);绘制实际相机姿势。camActual = plotCamera(大小=cameraSize，.．.AbsolutePose = rigidtform3d (groundTruthPoses。取向{1}”,.．.groundTruthPoses.Location {1}),.．.颜色=“b”,透明度= 0);初始化相机轨迹。轨迹估计= plot3(0,0,0，“g -”）;轨迹实际= plot3(0,0,0，“b -”）;传奇(“估计轨迹”，“实际轨迹”）;标题(“相机轨迹”）;

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

%转换为灰度和不扭曲。I = undistortion timage (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, Points=currPoints);存储前一个视图和当前视图之间的匹配点。vSet = addConnection(vSet, viewId-1, viewId, Matches=indexPairs);

vSet = helperNormalizeViewSet(vSet, groundtruthpositions);

helperUpdateCameraPlots(viewId, camEstimated, camActual, pose (vSet)，.．.groundTruthPoses);helperUpdateCameraTrajectories(viewId，轨迹估计，轨迹实际，.．.姿势(vSet) groundTruthPoses);

prevI = I;prevFeatures = currFeatures;prevPoints = currPoints;

为viewId = 3:15读取并显示下一个图像Irgb = readimage(图像，viewId);步骤(球员,Irgb);%转换为灰度和不扭曲。I = undistortion timage (im2gray(Irgb)， intrinsic);%前一个图像和当前图像之间的匹配点。[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(.．.prevFeatures,我);从特征匹配中消除异常值。[~， inlierIdx] = helperEstimateRelativePose(prevPoints(indexPairs(:，1))，.．.currPoints(indexPairs(:， 2))， intrinsic);indexPairs = indexPairs(inlierIdx，:);%对前两个视图中的点进行三角剖分，并找到%当前视图中的对应点。[worldPoints, imagePoints] = helperfind3dto2dresponses (vSet，.．.intrinsic, indexPairs, currPoints);由于RANSAC涉及随机过程，有时可能不是达到所期望的置信水平，并超过的最大数目%试验。当发生这种情况时禁用警告，因为结果是%仍然有效。Warningstate =警告(“关闭”，“愿景:ransac: maxTrialsReached”）;估计当前视图的世界相机姿态。absPose = estworldpose(imagePoints, worldPoints, intrinsic);恢复原始警告状态警告(warningstate)将当前视图添加到视图集中。vSet = addView(vSet, viewId, absPose, Points=currPoints);存储前一个视图和当前视图之间的匹配点。vSet = addConnection(vSet, viewId-1, viewId, Matches=indexPairs);tracks = findTracks(vSet);查找跨越多个视图的点轨迹。camposed = pose (vSet);获取所有视图的相机姿势。三角化三维世界点的初始位置。。xyzPoints = triangulateMultiview(轨迹，campose, intrinsic);使用束调整优化相机姿势。[~， campose] = bundleadadjustment (xyzPoints, tracks, campose，.．.intrinsic, pointsundistortion =true, AbsoluteTolerance=1e-12，.．.RelativeTolerance=1e-12, MaxIterations=200, FixedViewID=1);vSet = updateView(vSet, camposed);%更新视图集。捆绑调整可以移动整套相机。规范化的%视图设置，将第一个摄像机放在原点沿% z轴，并调整比例以匹配地面真相。vSet = helperNormalizeViewSet(vSet, groundtruthpositions);更新相机轨迹图。helperUpdateCameraPlots(viewId, camEstimated, camActual, pose (vSet)，.．.groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated,.．.轨迹实际，pose (vSet)， groundtruthpose);prevI = I;prevFeatures = currFeatures;prevPoints = currPoints;结束

为viewId = 16:numel(images.Files)读取并显示下一个图像Irgb = readimage(图像，viewId);步骤(球员,Irgb);%转换为灰度和不扭曲。I = undistortion timage (im2gray(Irgb)， intrinsic);%前一个图像和当前图像之间的匹配点。[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(.．.prevFeatures,我);%对前两个视图中的点进行三角剖分，并找到%当前视图中的对应点。[worldPoints, imagePoints] = helperfind3dto2dresponses (vSet，.．.intrinsic, indexPairs, currPoints);由于RANSAC涉及随机过程，有时可能不是达到所期望的置信水平，并超过的最大数目%试验。当发生这种情况时禁用警告，因为结果是%仍然有效。Warningstate =警告(“关闭”，“愿景:ransac: maxTrialsReached”）;估计当前视图的世界相机姿态。absPose = estworldpose(imagePoints, worldPoints, intrinsic);恢复原始警告状态警告(warningstate)将当前视图和连接添加到视图集中。vSet = addView(vSet, viewId, absPose, Points=currPoints);vSet = addConnection(vSet, viewId-1, viewId, Matches=indexPairs);使用窗口束调整优化估计的相机姿势。运行%优化每7个视图。如果mod(viewId, 7) == 0在最近15个视图中找到点轨迹并进行三角测量。windowSize = 15;startFrame = max(1, viewId - windowSize);tracks = findTracks(vSet, startFrame:viewId);campositions = pose (vSet, startFrame:viewId);[xyzPoints, retrjerrors] = triangulateMultiview(轨迹，campose, intrinsic);保持前两个姿势不变，保持相同的比例。。fixedIds = [startFrame, startFrame+1];排除重投影误差大的点和轨迹。idx = retrjerrors < 2;[~， camposed] = bundleadadjustment (xyzPoints(idx，:)， tracks(idx)，.．.campose, intrinsic, FixedViewIDs=fixedIds，.．.PointsUndistorted = true, AbsoluteTolerance e-12 = 1,.．.RelativeTolerance = 1 e-12 MaxIterations = 200);vSet = updateView(vSet, camposed);%更新视图集。结束更新相机轨迹图。helperUpdateCameraPlots(viewId, camEstimated, camActual, pose (vSet)，.．.groundTruthPoses);helperUpdateCameraTrajectories (viewId trajectoryEstimated,.．.轨迹实际，pose (vSet)， groundtruthpose);prevI = I;prevFeatures = currFeatures;prevPoints = currPoints;结束

持有从