dataFolder = [fullfile(pwd)， filesep，“stereoImageData”];zipFileName = [dataFolder, filesep，“run_000005.zip”];folderExists =存在(dataFolder,“dir”）;在当前目录下创建一个文件夹来保存下载的文件。如果~ folderExists mkdir (dataFolder);disp (正在下载run_000005.zip (818 MB)。下载可能需要几分钟。”）get ftp://asrl3.utias.utoronto.ca/2020-vtr-dataset/UTIAS-In-The-Dark/run_000005.zip -P ./stereoImageData提取下载文件的内容disp (正在提取run_000005.zip (818 MB)…)解压缩(zipFileName dataFolder);结束

imgFolderLeft = [dataFolder,“/图片/左/”];imgFolderRight = [dataFolder,“/图片/正确/”];imdsLeft = imageDatastore (imgFolderLeft);imdsRight = imageDatastore (imgFolderRight);检查第一对图像currFrameIdx = 1;currILeft = readimage(imdsLeft, currFrameIdx);currIRight = readimage(imdsRight, currFrameIdx);imshowpair (currILeft currIRight,“蒙太奇”）;

为再现性设置随机种子rng (0);加载初始相机姿势。在此基础上推导出初始相机姿态%关于相机和车辆之间的转换:% http://asrl.utias.utoronto.ca/datasets/2020-vtr-dataset/text_files/transform_camera_vehicle.txinitialPoseData =负载(“initialPose.mat”）;initialPose = initialPoseData.initialPose;%构造用于三维重建的重投影矩阵。数据集的intrinsic可以在以下页面中找到:% http://asrl.utias.utoronto.ca/datasets/2020-vtr-dataset/text_files/camera_parameters.txtfocalLength = [387.777 387.777];%(像素)principalPoint = [257.446 197.718];以像素为单位的百分比[x, y]基线= 0.239965;%以米为单位图象尺寸大小= (currILeft [1, 2]);%(像素)[mrows, ncols]intrinsic = cameraIntrinsics(focalLength, principalPoint, imageSize);reprojectionMatrix = [1,0,0， -principalPoint(1);0, 1,0， -principalPoint(2);0, 0, 0, focalLength(1);0,0,1 /baseline, 0];在本例中，图像已经未失真和校正。。在一般的工作流程中，% un注释以下代码，以恢复和校正图像。% currILeft = undistortion timage (currILeft, intrinsic);% currIRight = undistortion timage (currIRight, intrinsic);% stereooparams = stereoparameter (intrinsic, intrinsic, eye(3)， [-baseline, 0 0]);% [currILeft, currIRight] = rectifyStereoImages(currILeft, currIRight, stereooparams, OutputView="full");从校正后的立体图像中检测并提取ORB特征scaleFactor = 1.2;numLevels = 8;[currFeaturesLeft, currPointsLeft] = helperDetectAndExtractFeatures(im2gray(currILeft)， scaleFactor, numLevels);[currFeaturesRight, currPointsRight] = helperDetectAndExtractFeatures(im2gray(currright)， scaleFactor, numLevels);匹配立体图像之间的特征点，得到三维世界的位置。disparityRange = [0 48];%(像素)[xyzPoints, matchedPairs] = helperReconstructFromStereo(im2gray(currILeft)， im2gray(currIRight)，.．.currFeaturesLeft, currFeaturesRight, currPointsLeft, currPointsRight, reprojectionMatrix, initialPose, disparityRange);

创建一个空的imageviewset对象来存储关键帧vSetKeyFrames = imageviewset;创建一个空的worldpointset对象来存储3-D地图点mapPointSet = worldpointset;添加第一个关键帧currKeyFrameId = 1;vSetKeyFrames = addView(vSetKeyFrames, currKeyFrameId, initialPose, Points=currPointsLeft，.．.特点= currFeaturesLeft.Features);添加3d地图点[mapPointSet, stereoMapPointsIdx] = addWorldPoints(mapPointSet, xyzPoints);添加观察地图点mapPointSet = addanswences (mapPointSet, currKeyFrameId, stereomappointment sidx, matchedPairs(:， 1));更新视图方向和深度mappointment = updateLimitsAndDirection(mappointment, stereomappointment sidx, vSetKeyFrames.Views);%更新代表视图mappointment = updateRepresentativeView(mappointment, stereomappointment sidx, vSetKeyFrames.Views);在第一个关键帧中可视化匹配的特征featurePlot = helperVisualizeMatchedFeaturesStereo(currILeft, currIRight, currPointsLeft，.．.currPointsRight matchedPairs);可视化初始地图点和相机轨迹mapPlot = helperVisualizeMotionAndStructureStereo(vSetKeyFrames, mapptset);%显示传奇showLegend (mapPlot);

加载离线创建的特性数据包。bofData =负载(“bagOfFeaturesDataSLAM.mat”）;初始化地点识别数据库loopDatabase = invertedImageIndex (bofData.bof SaveFeatureLocations = false);将第一个关键帧的特征添加到数据库中addImageFeatures (loopDatabase currFeaturesLeft currKeyFrameId);

最后一个关键帧的ViewIdlastKeyFrameId = currKeyFrameId;输入图像序列中最后一个关键帧的索引lastKeyFrameIdx = currFrameIdx;%输入图像序列中所有关键帧的索引addedFramesIdx = lastKeyFrameIdx;currFrameIdx = 2;isLoopClosed = false;

%主循环isLastFrameKeyFrame = true;而~isLoopClosed && currFrameIdx <= numel(imdsLeft. files) currILeft = readimage(imdsLeft, currFrameIdx);currIRight = readimage(imdsRight, currFrameIdx);currILeftGray = im2gray (currILeft);currIRightGray = im2gray (currIRight);[currFeaturesLeft, currPointsLeft] = helperDetectAndExtractFeatures(currILeftGray, scaleFactor, numLevels);[currFeaturesRight, currPointsRight] = helperDetectAndExtractFeatures(currIRightGray, scaleFactor, numLevels);跟踪最后一个关键帧% trackedmappointment sidx:当前左帧中观察到的映射点的索引% trackedFeatureIdx:当前左帧对应特征点的索引[currPose, trackedMapPointsIdx, trackedFeatureIdx] = helperTrackLastKeyFrame(mapPointSet，.．.vSetKeyFrames。视图，currFeaturesLeft, currPointsLeft, lastKeyFrameId, intrinsic, scaleFactor);如果isempty(currPose) || numl (trackedMapPointsIdx) < 30 currFrameIdx = currFrameIdx + 1;继续结束跟踪本地地图，检查当前帧是否是关键帧。% localKeyFrameIds:当前帧连接的关键帧的ViewIdnumSkipFrames = 5;numPointsKeyFrame = 80;[localKeyFrameIds, currPose, trackedMapPointsIdx, trackedFeatureIdx, isKeyFrame] =.．.helperTrackLocalMap (mapPointSet vSetKeyFrames trackedMapPointsIdx,.．.trackedFeatureIdx, currPose, currFeaturesLeft, currPointsLeft, intrinsic, scaleFactor, numLevels，.．.isLastFrameKeyFrame, lastKeyFrameIdx, currFrameIdx, numSkipFrames, numPointsKeyFrame);匹配立体图像之间的特征点，得到三维世界的位置。[xyzPoints, matchedPairs] = helperReconstructFromStereo(currILeftGray, currIRightGray, currFeaturesLeft，.．.currFeaturesRight, currPointsLeft, currPointsRight, reprojectionMatrix, currPose, disparityRange);在立体图像中可视化匹配的特征updatePlot(featurePlot, currILeft, currIRight, currPointsLeft, currPointsRight, trackedFeatureIdx, matchedPairs);如果~isKeyFrame && currFrameIdx < numel(imdsLeft.Files) currFrameIdx = currFrameIdx + 1;isLastFrameKeyFrame = false;继续其他的[untrackedFeatureIdx, ia] = setdiff(matchedPairs(:， 1)， trackedFeatureIdx);xyzPoints = xyzPoints(ia，:);isLastFrameKeyFrame = true;结束更新当前关键帧IDcurrKeyFrameId = currKeyFrameId + 1;

添加新的关键帧[mappset, vSetKeyFrames] = helperAddNewKeyFrame(mappset, vSetKeyFrames，.．.currPose, currFeaturesLeft, currPointsLeft, trackedMapPointsIdx, trackedFeatureIdx, localKeyFrameIds);移除在少于3个关键帧内观察到的离群图点如果currKeyFrameId == 2 triangulatedMapPointsIdx = [];结束mapPointSet = helperCullRecentMapPoints (mapPointSet,.．.trackedMapPointsIdx、triangulatedMapPointsIdx stereoMapPointsIdx);添加从视差计算的新地图点[mapPointSet, stereoMapPointsIdx] = addWorldPoints(mapPointSet, xyzPoints);mapPointSet = addanswences (mapPointSet, currKeyFrameId, stereomappointment sidx，.．.untrackedFeatureIdx);通过三角剖分创建新的地图点minNumMatches = 20;minParallax = 0.35;[mappointment set, vSetKeyFrames, triangulatedmappointment sidx, stereomappointment sidx] = helpercreatenewmappointment stereo (.．.mappset, vSetKeyFrames, currKeyFrameId, intrinsic, scaleFactor, minNumMatches, minParallax，.．.untrackedFeatureIdx stereoMapPointsIdx);%本地包调整[refinedViews, dist] = connectedViews(vSetKeyFrames, currKeyFrameId, MaxDistance=2);refinedKeyFrameIds = refinedViews.ViewId;总是修复前两帧关键帧。fixedViewIds = refinedKeyFrameIds (dist = = 2);fixedViewIds = fixedViewIds(1:min(10, numel(fixedViewIds)));优化本地关键帧和地图点[mapPointSet, vSetKeyFrames, mappoint] = bundleAdjustment(.．.mapPointSet vSetKeyFrames, [refinedKeyFrameIds;intrinsic currKeyFrameId),.．.FixedViewIDs = FixedViewIDs PointsUndistorted = true, AbsoluteTolerance = 1 e -,.．.RelativeTolerance = 1 e-16,解算器=“preconditioned-conjugate-gradient”, MaxIteration = 10);更新视图方向和深度mapPointSet = updateLimitsAndDirection(mapPointSet, mappoint dx，.．.vSetKeyFrames.Views);%更新代表视图mapPointSet = updateRepresentativeView(mapPointSet, mappoint dx，.．.vSetKeyFrames.Views);可视化3-D世界点和相机轨迹updatePlot (mapPlot vSetKeyFrames mapPointSet);

在创建一些关键帧后检查循环关闭如果currKeyFrameId > 50循环边缘特征匹配的最小个数loopEdgeNumMatches = 50;检测可能的闭环关键帧候选帧[isDetected, validloopcandidate] = helperCheckLoopClosure(vSetKeyFrames, currKeyFrameId，.．.loopDatabase、currILeft loopEdgeNumMatches);isTooCloseView = currKeyFrameId - max(validloopcandidate) < 100;如果isDetected & & ~ isTooCloseView添加环路闭合连接[isLoopClosed, mappset, vSetKeyFrames] = helperAddLoopConnectionsStereo(.．.， validloopcandidate, currKeyFrameId，.．.currFeaturesLeft、currPointsLeft loopEdgeNumMatches);结束结束%如果未检测到循环关闭，则将当前特性添加到数据库中如果~isLoopClosed addImageFeatures(loopDatabase, currFeaturesLeft, currKeyFrameId);结束更新id和索引lastKeyFrameId = currKeyFrameId;lastKeyFrameIdx = currFrameIdx;addedFramesIdx = [addedFramesIdx;currFrameIdx];currFrameIdx = currFrameIdx + 1;结束主循环结束

%优化姿势vSetKeyFramesOptim = optimizeposed (vSetKeyFrames, minNumMatches, Tolerance=1e-16);优化姿势后更新地图点mappset = helperUpdateGlobalMap(mappset, vSetKeyFrames, vSetKeyFramesOptim);updatePlot (mapPlot vSetKeyFrames mapPointSet);绘制优化后的相机轨迹optimizedPoses =姿势(vSetKeyFramesOptim);plotOptimizedTrajectory (mapPlot optimizedPoses)%更新传奇showLegend (mapPlot);

载入GPS数据gpsData =负载(“gpsLocation.mat”）;gpsLocation = gpsData.gpsLocation;将GPS位置转换为参考坐标系统gTruth = helperTransformGPSLocations(gpsLocation, optimizedposed);绘制GPS定位图plotActualTrajectory (mapPlot gTruth (addedFramesIdx:));%显示传奇showLegend (mapPlot);

创建一个pointCloud对象数组来存储3d世界点。%与关键帧相关联ptClouds = repmat(pointCloud(0 (1,3))， numel(addedFramesIdx)， 1);为i = 1: numel(addedFramesIdx) ILeft = readimage(imdsLeft, addedFramesIdx(i));IRight = readimage(imdsRight, addedFramesIdx(i));从视差中重建场景distitymap = distitysgm (im2gray(ILeft)， im2gray(IRight)， distityrange = distityrange);xyzPoints = reconstructScene(distitymap, reprojectionMatrix);忽略图像的上半部分，它主要显示的是天空xyzPoints(1:floor(imageSize(1)/2)，:，:) = NaN;忽略图片下方显示车辆的部分。xyzPoints(imageSize(1)-50:end，::) = NaN;xyzPoints =重整(xyzPoints， [imageSize(1)*imageSize(2)， 3]);从彩色图像中获取颜色colors =重整(ILeft， [imageSize(1)*imageSize(2)， 3]);移除离镜头太远的世界点。validIndex = xyzPoints(:， 3) > 0 & xyzPoints(:， 3) < 100/reprojectionMatrix(4,3);xyzPoints = xyzPoints(validIndex，:);colors = colors(validIndex，:);将世界点转换为世界坐标currPose = optimizedPoses.AbsolutePose(我);xyzPoints = transformPointsForward(currPose, xyzPoints);ptCloud = pointCloud(xyzPoints, Color=colors);%对点云进行下采样ptClouds(i) = pcdownsample(ptCloud, random=0.5);结束连接点云pointCloudsAll = pccat (ptClouds);形象化点云图ax = pcshow(pointCloudsAll,VerticalAxis=“y”VerticalAxisDir =“向下”）;包含(“X”) ylabel (“Y”) zlabel (“Z”)标题(“密集的重建”）显示鸟瞰图的场景视图(ax， [0 0 1]);camroll (ax, 90);

函数[features, validPoints] = helperDetectAndExtractFeatures(Igray, scaleFactor, numLevels) numPoints = 600;%检测ORB功能points = detectORBFeatures(灰度，ScaleFactor= ScaleFactor, NumLevels= NumLevels);选择特征的子集，均匀分布在整个图像。points = selectUniform(points, numPoints, size(灰度，1:2));%提取特征[features, validPoints] = extractFeatures(Igray, points);结束

函数[xyzPoints, indexPairs] = helperReconstructFromStereo(I1, I2，.．.features1, features2, points1, points2, reprojectionMatrix, currPose, disparityRange) indexPairs = helperfindvalidfeatufixes (features1, features2, points1, points2, disparityRange);计算左边图像中所有像素的视差。实际上，它更多%常用来计算匹配特征点的视差。disitymap = disityysgm (I1, I2, disityrange = disityrange);xyzPointsAll = reconstructScene(distitymap, reprojectionMatrix);查找匹配特征点对应的世界点。位置=地板(里。Location(indexPairs(:， 1)， [2 1]));xyzPoints = [];isPointFound = false(大小(里));为i = 1:size(locations, 1) point3d = squeeze(xyzPointsAll(locations(i,1)， locations(i, 2)，:))';isPointValid = point3d(3) > 0 & point3d(3) < 200/reprojectionMatrix(4,3);如果isPointValid xyzPoints = [xyzPoints;point3d];% #好< * AGROW >isPointFound (i) = true;结束结束indexPairs = indexPairs(isPointFound，:);xyzPoints = xyzPoints * currPose。旋转+ currPose.Translation;结束

函数indexPairs = helperfindvalidfeatufixes (features1, features2, points1, points2, disparityRange) indexPairs = matchFeatures(features1, features2, features2，.．.独特的= true, MaxRatio = 1, MatchThreshold = 40);matchedPoints1 = points1.Location(indexPairs(:，1)，:);matchedPoints2 = points2.Location(indexPairs(:，2)，:);scales1 = points1.Scale(indexPairs(:，1)，:);scales2 = points2.Scale(indexPairs(:，2)，:);dist2EpipolarLine = abs(matchedPoints2(:， 2) - matchedPoints1(:， 2));shiftDist = matchedPoints1(:， 1) - matchedPoints2(:， 1);isCloseToEpipolarline = dist2EpipolarLine < 2*scales2;isDisparityValid = shiftDist > 0 & shiftDist < disparityRange(2);isscalesame = scales1 == scales2; indexPairs = indexPairs(isCloseToEpipolarline & isDisparityValid & isScaleIdentical, :);结束

函数mappointment = helperCullRecentMapPoints(mappointment set, mappointment sidx, newPointIdx, stereomappointment sindices) outlierIdx = setdiff([newPointIdx;stereoMapPointsIndices], mapPointsIdx);如果~isempty(outlierIdx) mapPointSet = removeWorldPoints(mapPointSet, outlierIdx);结束结束

函数mappset = helperUpdateGlobalMap(mappset, vSetKeyFrames, vSetKeyFramesOptim) posesOld = vSetKeyFrames. views . absolutepose;一直陷于= vSetKeyFramesOptim.Views.AbsolutePose;positionsOld = mapPointSet.WorldPoints;positionsNew = positionsOld;指数= 1:mapPointSet.Count;更新每个地图点的世界位置基于新的绝对姿势。%对应的主要视图为i = 1: mappointment。Count majorViewIds = mappointment . representativeviewid (i);tform = rigidtform3d(一直陷于(majorViewIds)。/ posesOld (majorViewIds)。);positionsNew(i，:) = transformPointsForward(tform, positionsOld(i，:));结束mappointment = updateWorldPoints(mappointment, indexes, positionsNew);结束

函数gTruth = helpertransformgppslocations (gppslocations, optimizedposed) initialYawGPS = atan((gppslocations (100,2) - gppslocations (1,2)) /.．.(gpsLocations(100, 1) - gpsLocations(1,1)));initialYawSLAM = atan((optimizedposesabsolutepose (50).Translation(2) -.．.optimizedPoses.AbsolutePose (1) .Translation) / (2).．.(optimizedPoses.AbsolutePose (59) .Translation (1) -.．.optimizedPoses.AbsolutePose (1) .Translation (1)));relYaw = initialYawGPS - initialYawSLAM;relTranslation = optimizedPoses.AbsolutePose (1) .Translation;initialTform = rotationVectorToMatrix([0 0 relYaw]);为i = 1:size(gpsLocations, 1) gTruth(i，:) = initialTform * gpsLocations(i，:)' + relTranslation';结束结束