images = imagedataStore（fullfile（toolboxdir（“愿景”)，“visiondata”，“NewTsukuba”)）；加载地面真实相机姿势。load（fullfile（toolboxdir（“愿景”)，“visiondata”，“visualOdometryGroundTruth.mat”)）；

%创建一个空的imageviewset对象来管理与每个视图关联的数据。vset = imageviewset;％读取并显示第一张图像。IRGB = ReadImage（图像，1）;玩家= Vision.videoplayer（“位置”， [20, 400, 650, 510]);步骤(球员,Irgb);

属性中使用camera intrinsics创建camera intrinsics对象％新的Tsukuba数据集。focalLength = [615 615];%以像素为单位指定principalPoint = [320 240];百分比像素[x，y]图象尺寸大小= (Irgb [1, 2]);%(像素)[mrows, ncols]内在=摄像石（FocalLength，Principalpoint，Imageize）;

prevI = undistortion timage (im2gray(Irgb)， intrinsics); / /本质%检测功能。prevpoints = detectsurfeatures（previ，'米兰克斯德', 500);%选择一个特征子集，均匀分布在整个图像中。numPoints = 200;prevPoints = selectUniform(prevPoints, numPoints，大小(prevI));％提取特征。使用“直立”功能可提高匹配质量相机的运动很少或没有涉及到面内旋转。prevFeatures = extractFeatures(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 (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 = undostortimage（IM2Gray（IM2Gray（IRGB），内在）;以前和当前图像之间的％匹配功能。[Furports，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))，“点”，curports）;%存储前一个视图和当前视图之间的点匹配。vSet = addConnection(vSet, viewId-1, viewId，)“匹配”, indexPairs);

vSet = helperNormalizeViewSet(vSet, groundtruthpose);

helperupdatecameraplot (viewId, camEstimated, camActual, pose (vSet)，......groundTruthPoses);Helperupdatecameratories（查看id，溯极，TrajectoryActial，......姿势(vSet) groundTruthPoses);

prevI =我;prevFeatures = currFeatures;prevpoints = curptpoints;

为viewId = 3:15%读取并显示下一个图像Irgb = readimage(images, viewwid);步骤(球员,Irgb);%转换为灰度和不失真。i = undostortimage（IM2Gray（IM2Gray（IRGB），内在）;%先前图像和当前图像之间的匹配点。[Furports，Currfeatures，IndexPairs] = HelperDetectandMatchFeatures（......prevFeatures,我);%消除特征匹配中的异常值。Inlieridx = Helperfindepipolinliers（prevpoints（IndexPairs（：，1）），......FurrPoints（IndexPairs（:,2）），内在机构）;indexPairs = IndexPairs（Inlieridx，:);从前面两个视图中三角化点，并找到%当前视图中对应的点。[worldPoints, imagePoints] = helperfind3dto2d通信(vSet，......intrinsic、indexPairs currPoints);由于RANSAC涉及一个随机过程，它有时可能不会达到期望的置信水平，并超过的最大数目％试验。自结果是以来发生这种情况时会禁用警告%仍然有效。警告State =警告（“关闭”，'愿景：Ransac：maxtrialsReached'）;估计当前视图的世界相机姿态。[orient, loc] = estimateWorldCameraPose(imagePoints, worldPoints, intrinsics);%恢复原来的警告状态警告(warningstate)将当前视图添加到视图集。vSet = addView(vSet, viewId, rigid3d(orient, loc))，“点”，curports）;%存储前一个视图和当前视图之间的点匹配。vSet = addConnection(vSet, viewId-1, viewId，)“匹配”, indexPairs);跟踪= findTracks (vSet);查找点轨迹跨越多个视图。坎波斯=姿势(vSet);为所有视图设置相机姿势。三角定位三维世界点的初始位置。xyzPoints = triangulateMultiview(tracks, campose, intrinsics);使用束调整优化相机姿态。[〜，野营] = Bundleadjustment（XYZPOINTS，轨道，野营，......内在机构，'obessundistorted',真的,“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 = curptpoints;结束

为viewId = 16:元素个数(images.Files)%读取并显示下一个图像Irgb = readimage(images, viewwid);步骤(球员,Irgb);%转换为灰度和不失真。i = undostortimage（IM2Gray（IM2Gray（IRGB），内在）;%先前图像和当前图像之间的匹配点。[Furports，Currfeatures，IndexPairs] = HelperDetectandMatchFeatures（......prevFeatures,我);从前面两个视图中三角化点，并找到%当前视图中对应的点。[worldPoints, imagePoints] = helperfind3dto2d通信(vSet，......intrinsic、indexPairs currPoints);由于RANSAC涉及一个随机过程，它有时可能不会达到期望的置信水平，并超过的最大数目％试验。自结果是以来发生这种情况时会禁用警告%仍然有效。警告State =警告（“关闭”，'愿景：Ransac：maxtrialsReached'）;估计当前视图的世界相机姿态。[orient, loc] = estimateWorldCameraPose(imagePoints, worldPoints, intrinsics);%恢复原来的警告状态警告(warningstate)将当前视图和连接添加到视图集。vSet = addView(vSet, viewId, rigid3d(orient, loc))，“点”，curports）;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”，固定性，......'obessundistorted',真的,“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 = curptpoints;结束

持有离开