Landmark SLAM使用AprilTag标记
这个例子展示了如何结合机器人里程测量数据和被称为AprilTags的观察基准标记来更好地估计机器人轨迹和环境中的地标位置。该示例使用了姿态图方法和因子图方法，并比较了这两种图。
下载数据集和加载传感器数据
下载一个. zip该文件包含从ClearPath Robotics™Jackal™上的rosbag记录的原始数据。原始数据包括:
使用Axis™M1013网络摄像机拍摄的图像，分辨率为640 * 480。
对测程数据进行预处理，并与图像数据同步。
解压缩文件，其中包含以rosbag文件和.mat文件。您可以使用exampleHelperExtractDataFromRosbagHelper函数，该函数在本例的末尾提供，用于查看如何从rosbag提取数据并对其进行预处理。要使用helper函数，必须拥有ROS Toolbox许可证。
文件名= matlab.internal.examples.downloadSuppor金宝apptFile(“spc /机器人”，“apriltag_odometry_slam_dataset.zip”）;解压缩(文件名)
在本例中，打印了一组AprilTag标记，并将其随机放置在测试环境中。姿态图和因子图将标签视为地标性建筑，这些是环境的可区分的特征，使您能够更准确地进行本地化。通过使用相机intrinsic和每个标记的大小，可以将带有AprilTag观测值的图像转换为点地标测量值readAprilTag(计算机视觉工具箱)函数。这些标志性的测量是相对于当前的机器人框架。中包含了相机的固有参数cameraParams.mat文件，但要确定您自己的相机的内在参数，请遵循使用AprilTag标记的相机校准(计算机视觉工具箱)例，或使用棋盘图案与相机校准器(计算机视觉工具箱)本例中使用的AprilTag标记来自于tag46h11家庭，没有重复的身份证。打印的吊牌尺寸为136.65 mm。
tagFamily =“tag36h11”；tagSize = 136.65;%毫米
加载传感器数据和相机参数到MATLAB®工作空间。
负载apriltag_odometry_slam_dataset / apriltag_odometry_slam_dataset.mat负载cameraParams.mat
创建一个空的字典数据结构，以维护标签id与其节点id之间的映射。
tagToNodeIDDic = dictionary([]，[]);
传感器记录的里程数据.mat文件从里程表帧到激光帧，与机器人一起移动。在激光框架和相机框架之间应用固定转换。
R1 = [0 0 1;-1 0 0;0 -1 0];Ta = blkdiag(R1,1);%相机框架z轴指向前方，y轴指向下方Tb =眼(4);T2(1,3) = -0.11;T(3,3) = 0.146;修正了相机帧原点到“激光”帧的转换
从传感器数据构建姿态图
导入同步传感器数据和相机参数后，根据传感器数据中的测量值构建节点估计的三维姿态图。姿态图包含节点估计、边缘约束和估计机器人姿态的环路闭包。
首先创建姿态图。
pg = poseGraph3D;
通过使用像AprilTags这样的基准标记，图像中的块模式为每个地标观测提供了唯一的id。当执行同步定位和映射(SLAM)时，该ID信息减少了对困难的数据关联算法的需求。
创建一个变量来存储之前的姿态和节点ID。
lastTform = [];lastPoseNodeId = 1;
创建变量来存储所有地标节点id。
lmkIDs = [];
本例基于来自里程计数据的姿态测量和来自AprilTag观测的地标测量来估计机器人的轨迹。遍历传感器数据，并遵循以下步骤:
方法将里程计数据添加到姿态图中
addRelativePose函数。在将每个里程计添加到姿态图之前，计算每个里程计之间的相对姿态。在图像中添加来自AprilTag观测的地标测量值
readAprilTag函数。因为图像可以包含多个标记，所以要遍历返回的所有id。控件添加相对于当前姿态节点的点标志addPointLandmark函数。在AprilTag观测值周围用标记显示图像。图像在遍历数据时更新。
图(可见=“上”）为i = 1:数字(imageData)将里程计数据添加到姿态图中T = odomData{i};如果isempty(lastTform) nodePair = addRelativePose(pg，[0 0 0 0 1 0 0 0 0]，[]，lastPoseNodeId);其他的relPose = examplehelpcomputerelativepose (lastTform,T);nodePair = addRelativePose(pg,relPose，[]，lastPoseNodeId);结束currPoseNodeId = nodePair(2);在图像中添加基于AprilTag观测的地标测量。I = imageData{I};[id,loc,poseRigid3d,detectedFamily] = readAprilTag(I,tagFamily, cameraparams . intrinsic,tagSize);为J = 1:数字(id)将观察标记插入图像。markerRadius = 6;numCorners = size(loc,1);markerPosition = [loc(:，:，j) repmat(markerRadius,numCorners,1)];I = insertShape(I,FilledCircle=markerPosition,Color=“红色”,透明度= 1);t = poseRigid3d(j).Translation/1000;%变化从毫米到米Po = [t(:);1);p = Tb*Ta*po;如果isKey(tagToNodeIDDic,id(j)) lmkNodeId = tagToNodeIDDic(id(j));如果~ismember(lmkNodeId,lmkIDs) lmkIDs = [lmkIDs lmkNodeId];%存储唯一的地标id结束addPointLandmark (pg, p(1:3)”,[]currPoseNodeId lmkNodeId);其他的nodePair = addPointLandmark(pg,p(1:3)'，[]，currPoseNodeId);tagToNodeIDDic(id(j)) = nodePair(2);结束结束用AprilTag观察标记显示图像。(I) drawnow lastTform = T;lastPoseNodeId = currPoseNodeId;结束
优化姿态图并检查结果
在构建姿态图之后，使用optimizePoseGraph函数。
pgUpd = optimizePoseGraph(pg);
可视化初始和优化的姿态图。优化后的姿态图显示了以下改进:
机器人相对于路标的初始位置已被修正。
每面墙上的地标都排列得更好。
垂直漂移在机器人的轨迹上z-direction已被修正。
图(可见=“上”) (pg);轴平等的Xlim ([-10.0 5.0]) ylim([-2.0 12.0]) title(“XY视图优化前姿态图结果”）
图(可见=“上”) (pgUpd);轴平等的Xlim ([-10.0 5.0]) ylim([-2.0 12.0]) title(“XY视图优化后的姿态图结果”）
%垂直漂移。图(可见=“上”) (pg);轴广场xlim ([-10.0 - 5.0]) zlim([-2.0 - 2.0])视图(0,0)标题(“优化XZ视图前姿态图结果”）
显示(pgUpd);轴广场xlim ([-10.0 - 5.0]) zlim([-2.0 - 2.0])视图(0,0)标题(“XZ视图优化后的姿态图结果”）
从传感器数据构建因子图
或者，您可以基于传感器数据中的测量值构建因子图。使用因子图可以更容易地在优化中包含额外的传感器，并且比姿态图优化得更快，尽管它可能需要更长的设置和构建时间。
创建因子图为afactorGraph对象。
G =因子图;
创建变量来存储之前的姿态和节点ID。
lastTform = [];lastPoseNodeId = 1;tagToNodeIDDic = dictionary([]，[]);
遍历传感器数据，并按照以下步骤向因子图添加因子:
将里程计数据作为因子添加到因子图中，方法将姿势与
factorTwoPoseSE3对象。在将其添加到因子图之前，计算每个里程表之间的相对姿势。在图像中添加来自AprilTag观测的地标测量值
readAprilTag函数。因为图像可以包含多个标记，所以要遍历返回的所有id。控件添加相对于当前姿态节点的点标志factorPoseSE3AndPointXYZ对象。在AprilTag观测值周围用标记显示图像。图像在遍历数据时更新。
为i = 1:数字(imageData)将里程计数据添加到因子图中T = odomData{i};如果isempty (lastTform)添加一个先验因子，松散地固定初始节点在原点fPrior = factorPoseSE3Prior(1);addFactor (G, fPrior);newPoseNodeId = lastPoseNodeId + 1;该测量值表示SE(3)中lastPoseNode和newPoseNode之间的相对位姿。fctr = factortwposenodee3 ([lastPoseNodeId newPoseNodeId]，Measurement=[0 0 0 0 1 0 0 0]);addFactor (G, fctr);其他的计算lastPoseNodeId和newPoseNodeId节点之间的相对位姿relPose = examplehelpcomputerelativepose (lastTform,T);newPoseNodeId = G.NumNodes + 1;fctr = factortwposenodee3 ([lastPoseNodeId newPoseNodeId]，测量=relPose);addFactor (G, fctr);结束currPoseNodeId = newPoseNodeId;在图像中添加基于AprilTag观测的地标测量。I = imageData{I};[id,loc,poseRigid3d,detectedFamily] = readAprilTag(I,tagFamily, cameraparams . intrinsic,tagSize);为J = 1:数字(id)将观察标记插入图像。markerRadius = 6;numCorners = size(loc,1);markerPosition = [loc(:，:，j)，repmat(markerRadius,numCorners,1)];I = insertShape(I,FilledCircle=markerPosition,Color=“红色”,透明度= 1);t = poseRigid3d(j).Translation/1000;从毫米到米的变化百分比Po = [t(:);1);p = Tb*Ta*po;如果isKey(tagToNodeIDDic,id(j)) lmkNodeId = tagToNodeIDDic(id(j));该测量值表示地标点与当前姿态节点之间的相对位置[dx,dy,dz]。fctr = factorPoseSE3AndPointXYZ([currPoseNodeId lmkNodeId]，测量=p(1:3)');addFactor (G, fctr);其他的newPointNodeId = G.NumNodes + 1;fctr = factorPoseSE3AndPointXYZ([currPoseNodeId newPointNodeId]，测量=p(1:3)');addFactor (G, fctr);tagToNodeIDDic(id(j)) = newPointNodeId;结束结束用AprilTag观察标记显示图像。imshow drawnow(我)limitratelastTform = T;lastPoseNodeId = currPoseNodeId;结束
优化因子图，检查结果
在构建因子图之后，使用优化对象函数的解算器选项factorGraphSolverOptions对象。设置VerbosityLevel财产2以显示优化过程的更多细节。
opt = factorGraphSolverOptions(VerbosityLevel=2);soln =优化(G,opt)
Iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time 0 4.984516e+02 0.00e+00 5.76e+01 8.515843e+02 -3.53e+02 5.76e+01 7.56e -01 5.96 e-02 2 8.515843e+02 -3.53e+02 5.76e+01 7.515843e +02 -3.53e+02 3.15e-04 2.15e-02 3 8.515843e+02 -3.53e+02 5.76e+01 -7.13e-01 1.25e+03 0 3.03e-04 2.28e-02 4 8.515843e+02 -3.53e+02 -7.13e- 04 2.28e-02 4 8.515843e+02 -3.53e+02 -7.13e-01 2.28e-028.515843e+02 -3.53e+02 - 5.76e+01 -7.13e-01 3.56 e+02 -3.53e+02 - 5.76e+01 -7.13e-01 7.81e+01 -3.53e+02 - 3.76 e+01 -7.13e-01 7.81e+01 - 3.01e +01 -2.13e-02 3.91e+01 - 2.98e-04 3.08e-02 9 1.365586e+02 3.62e+02 - 1.99e+01 - 7.77e-01 1.17e+02 - 3.27e+ 02 -3.22e+02 - 3.27e+01 - 2.77 e-01 1.17e+ 01 - 2.72 e+00 5.86e+01 1 4.05e-03 3.70e-02 115.329830e+01 8.33e+01 1.94e+01 3.35e+01 6.64e-01 3.94e-02 12 5.142921e+01 1.87e+00 1.91e+01 3.59 e+01 3.72e-02 2.93e+ 00 4.56e+ 01 4.56e+00 5.79 e+01 1 5.60e-03 5.59 e-02 14 1.032690e+02 -9.45e+01 4.56e+00 5.11e+01 -1.23e+ 00 4.39e+01 1 3.98e-03 5.50e-02 15 9.869378e+00 -1.63e +00 4.56e+00 2.55e+01 -1.63e-01 2.20e+01 0 2.97e-04 5.54e-02 16 3.688774e+00 5.12e+00 2.19e+00 1.28e+01 9.18e-01 6.59e+01 0 1.98e- 01 5.75e-023.409289e+01 -3.04e+01 - 1.19 e+01 - 3.30e+01 1 4.12e-03 6.17e-02 18 4.290375e+00 - 6.0e -01 2.90e-04 6.21e +00 9.61e+00 8.38e-01 4.94e+ 00 1.94e-03 6.41e-02 20 1.153071e+01 -9.19e+00 1.94e-03 6.41e-02 20 1.153071e+01 -9.19e+00 1.79e+00 2.88e+ 00 4.24e-02 3.62e+00 1.44e+ 00 4.43e-02 1.24e+ 00 9.06e-01 1.65e+00 7.21e+00 8.84e-01 3.71e+01 1 5.83e-03 7.68e-02 232.835470e+00 -1.44e+00 1.65e+00 -1.95e+00 1.85e+01 1 4.01e-03 8.09e-02 24 1.118728e+00 2.77e-01 1.76e+00 1.08e+01 5.51e-01 1.85e+01 1 5.63e-03 8.30e-02 25 7.683906e-01 1.85e+01 7.67e-01 1.10e+01 7.67e-01 5.56e+01 1 5.65e-03 9.45e-02 27 3.957137e+00 -3.45e+00 9.30e-01 3.31e+01 -8.79e+00 2.78e+01 1 3.97e-03 9.86e-02 28 4.955209e +00 1.60 e-02 1.39e+01 0 2.08e-03 1.01e-012.135810e-01 2.82e-01 7.81e-01 4.81e +00 1.99750e +00 -9.94e -01 1.99750e +00 -9.94e -01 2.91e -01 1.11e-01 31 2.592216e-01 -4.56e- 01 1.04e+01 0 2.94e- 03 1.11e-01 31 2.592216e-01 -4.56e- 01 1.13e-01 3.13e+ 00 8.358879e -01 7.77e- 01 1.13e-01 3.13e+ 00 8.358879e -01 1.13e-01 3.13e+ 00 8.368813e -01 2.36e-01 1.57e+01 -1.11e -01 1.36e -03 1.18e-01 34 1.788477e-01 1.56e+01 1 4.36e-03 1.18e-01 34 -7.68e-01 7.82e+00 0 2.89e-04 1.18e-01 351.064787e-01 2.94e-02 2.96e-01 4.40e+00 -8.44e+00 1.17e+01 1 3.97e-03 1.24e-01 3.74 e+00 -4.99e-01 5.87e+00 0 3.05e-04 3.74 e+00 -4.99e-01 1.78e-01 3.37e+00 -4.99e-01 1.78e-01 1.27e-01 39 2.110173e-01 -1.23e-01 1.78e-01 8.08e+00 -6.32e+00 8.80e+00 1 3.94e-03 1.31e-01 40 9.263937e-02 -2.96e-01 4.40e+00 -2.96e-01 4.40e+00 -2.96e-01 4.40e+00 - 2.92e-04 1.31e-01 417.771280e-02 9.84e- 01 1.32e+ 00 -3.85e+00 6.60e+00 1 4.04e-03 1.42e-01 43 7.727460e-02 4.38e- 02 3.70e+00 4.96e-02 3.30e -02 1.64e- 03 1.44e-01 44 6.863371e-02 8.64e-03 1.44e-01 1.90e+00 1 6.01e-03 1.51e-01 45 6.766035e-02 1.73e -03 1.5e -01 9.90e+00 1 5.68e-03 1.58e-01 46 6.764183e-02 1.85e-05 8.05e-05 9.00e-02 1.62e -03 1.64e-01达到功能公差。| cost_change | /成本: 1.078670e-07 <= 1.000000e-06 Solver Summary (v 2.0.0-eigen-(3.3.4)-no_lapack-eigensparse-no_openmp-no_custom_blas) Original Reduced Parameter blocks 585 585 Parameters 4039 4039 Effective parameters 3468 3468 Residual blocks 804 804 Residuals 4125 4125 Minimizer TRUST_REGION Sparse linear algebra library EIGEN_SPARSE Trust region strategy DOGLEG (TRADITIONAL) Given Used Linear solver SPARSE_NORMAL_CHOLESKY SPARSE_NORMAL_CHOLESKY Threads 1 1 Linear solver ordering AUTOMATIC 585 Cost: Initial 4.984516e+02 Final 6.764183e-02 Change 4.983839e+02 Minimizer iterations 47 Successful steps 22 Unsuccessful steps 25 Time (in seconds): Preprocessor 0.004742 Residual only evaluation 0.007090 (47) Jacobian & residual evaluation 0.035897 (22) Linear solver 0.076854 (22) Minimizer 0.165353 Postprocessor 0.000090 Total 0.170185 Termination: CONVERGENCE (Function tolerance reached. |cost_change|/cost: 1.078670e-07 <= 1.000000e-06)
溶液=带字段的结构:InitialCost: 498.4516 FinalCost: 0.0676 NumSuccessfulSteps: 22 NumUnsuccessfulSteps: 25 TotalTime: 0.1702 TerminationType: 0 IsSolutionUsable: 1
返回的解决方案信息表明优化对象函数大大降低了路径的代价，说明优化过程对路径进行了改进。的IsSolutionUsable的价值1还表明解决方案是可行的。如果优化不返回收敛结果，考虑调整求解器选项，如最大迭代次数，取决于返回cost_change而且梯度每个步骤的值。
检索优化结果，并将机器人轨迹可视化为蓝色，地标点可视化为绿色。
fgNodeStates = exampleHelperShowFactorGraphResult(G);
结论
这些图像显示了由姿态图和因子图生成的最终机器人轨迹，使用变换将其覆盖在办公区域的蓝图上，以显示估计轨迹和估计地标位置的准确性。如果你再次回放这些图像，你会发现姿态图和因子图之间有很强的相关性。所有的绿色地标都在墙壁上或附近，基于检测到的地标的两条拟合线与走廊的墙壁重叠。然而，虽然这两种解决方案在视觉上很相似，但我们可以在金宝搏官方网站数值上进行比较。
这些图像显示了AprilTags的位置，供您参考。注意，有一个AprilTag没有被摄像机捕捉到，用红色圆圈标记。
为了数值比较姿态图结果与因子图结果之间的差异，从更新后的姿态图中检索节点状态，并计算其与因子图之间机器人位置、旋转角度和地标位置的平均绝对差异。
pgUpdNodeStates = nodeEstimates(pgUpd);[robotPosDiff,robotRotDiff,lmkDiff] = exampleHelperComputeDifference(pgUpdNodeStates,fgNodeStates,lmkIDs)
robotPosDiff =1×3103× 0.0525 0.0925 0.9312
robotRotDiff =1×3103× 0.0198 0.1625 0.1355
lmkDiff =1×3103× 0.0562 0.0865 0.8646
注意，为了更好地与姿态图结果进行比较，您在原点处使用a指定了因子图初始节点姿态的初始猜测factorPoseSE3Prior对象。如果没有先验因子，因子图优化过程会得到一个具有最优相对位姿和位置但具有绝对位姿的图，并且位置不一定是正确的。平均绝对差值相当小，这意味着两种方法的优化结果相似。与姿态图相比，因子图的优化时间要短得多，但建立和构建因子图的时间要长得多。
您也可以从以下列表中选择一个网站: