imageDir = fullfile(toolboxdir(“愿景”)，“visiondata”，“upToScaleReconstructionImages”）;images = imageDatastore(imageDir);I1 = readimage(images, 1);I2 = readimage(images, 2);图imshowpair(I1, I2，“蒙太奇”）;标题(原始图像的）;

载入预先计算的相机参数负载upToScaleReconstructionCameraParameters.mat

I1 = undistortion timage (I1, cameraParams);I2 = undistortion timage (I2, cameraParams);图imshowpair(I1, I2，“蒙太奇”）;标题(“无畸变的图像”）;

检测特征点imagePoints1 = detectMinEigenFeatures(im2gray(I1)，“MinQuality”, 0.1);%可视化检测点图imshow (I1,“InitialMagnification”, 50);标题(“第一张图中最强的150个角”）;持有在情节(selectStrongest (imagePoints1, 150));

创建点跟踪器追踪者=视觉。PointTracker (“MaxBidirectionalError”, 1“NumPyramidLevels”5);初始化点跟踪器imagePoints1 = imagePoints1. location;初始化(跟踪器，imagePoints1, I1);%追踪点数[imagePoints2, validIdx] = step(跟踪器，I2);matchedPoints1 = imagePoints1(validIdx，:);matchedPoints2 = imagePoints2(validIdx，:);可视化通信图showMatchedFeatures(I1, I2, matchedPoints1, matchedPoints2);标题(“跟踪特性”）;

估计基本矩阵[E, eppolarinliers] = estimateEssentialMatrix(.．.matchedPoints1, matchedPoints2, cameraParams，“信心”, 99.99);找到极内线inlierPoints1 = matchedPoints1(epipolarInliers，:);inlierPoints2 = matchedPoints2(epipolarInliers，:);显示更深层次的匹配图showMatchedFeatures(I1, I2, inlierPoints1, inlierPoints2);标题(“纵向窗”）;

[orient, loc] = relativeCameraPose(E, cameraParams, inlierPoints1, inlierPoints2);

检测密集特征点。使用ROI来排除靠近的点%图像边缘。roi = [30,30, size(I1, 2) - 30, size(I1, 1) - 30];imagePoints1 = detectMinEigenFeatures(im2gray(I1)，“投资回报”投资回报率,.．.“MinQuality”, 0.001);创建点跟踪器追踪者=视觉。PointTracker (“MaxBidirectionalError”, 1“NumPyramidLevels”5);初始化点跟踪器imagePoints1 = imagePoints1. location;初始化(跟踪器，imagePoints1, I1);%追踪点数[imagePoints2, validIdx] = step(跟踪器，I2);matchedPoints1 = imagePoints1(validIdx，:);matchedPoints2 = imagePoints2(validIdx，:);为相机的每个位置计算相机矩阵第一个摄像机位于原点，沿z轴观察。。因此,其%变换就是单位单位。Tform1 = rigid3d;camMatrix1 = cameraMatrix(cameraParams, tform1);计算第二个摄像头的外部参数cameraPose = rigid3d(orient, loc);tform2 = cameraPoseToExtrinsics(cameraPose);camMatrix2 = cameraMatrix(cameraParams, tform2);计算三维点points3D =三角化(matchedPoints1, matchedPoints2, camMatrix1, camMatrix2);得到每个重构点的颜色numPixels = size(i1,1) * size(i1,2);allColors =重塑(I1， [numPixels, 3]);colorIdx = sub2ind([size(I1, 1)， size(I1, 2)]， round(matchedPoints1(:，2))，.．.轮(matchedPoints1 (: 1)));color = allColors(colorIdx，:);创建点云ptCloud = pointCloud(points3D，“颜色”、颜色);

可视化相机的位置和方向cameraSize = 0.3;图plotCamera (“大小”cameraSize,“颜色”，“r”，“标签”，' 1 '，“不透明度”, 0);持有在网格在plotCamera (“位置”疯狂的,“定位”东方,“大小”cameraSize,.．.“颜色”，“b”，“标签”，' 2 '，“不透明度”, 0);可视化点云pcshow (ptCloud“VerticalAxis”，“y”，“VerticalAxisDir”，“下来”，.．.“MarkerSize”, 45岁);%旋转和缩放绘图camorbit (0, -30);camzoom (1.5);标记坐标轴包含(“轴”）;ylabel (“轴”）;zlabel (z轴的)标题(“按比例重建现场”）;

%探测地球仪globe = pcfitsphere(ptCloud, 0.1);显示地球表面情节(全球);标题(“地球的估计位置和大小”）;持有从

确定比例因子scaleFactor = 10 / global . radius;缩放点云ptCloud = pointCloud(points3D * scaleFactor，“颜色”、颜色);loc = loc * scaleFactor;以厘米为单位可视化点云cameraSize = 2;图plotCamera (“大小”cameraSize,“颜色”，“r”，“标签”，' 1 '，“不透明度”, 0);持有在网格在plotCamera (“位置”疯狂的,“定位”东方,“大小”cameraSize,.．.“颜色”，“b”，“标签”，' 2 '，“不透明度”, 0);可视化点云pcshow (ptCloud“VerticalAxis”，“y”，“VerticalAxisDir”，“下来”，.．.“MarkerSize”, 45岁);camorbit (0, -30);camzoom (1.5);标记坐标轴包含(“轴(cm)”）;ylabel (“轴(cm)”）;zlabel (“z轴(cm)”)标题(“场景的度量重建”）;