MASt3R-SLAM核心流程解析

前端跟踪

MASt3R-SLAM的核心在于利用MASt3R模型进行稠密的多视图立体匹配，而不是传统SLAM中的稀疏特征点匹配。MASt3R能为图像中的每个像素提供：

• 3D点云(X)
• 置信度©
• 密集描述子(D)
• 描述子置信度(Q)

对称推理

在初始只有一帧图像的时候，通过复制输入图像，可以得到初始的点云，此时等价于使用MASt3R模型实现单目深度估计。随着新图像的加入，MASt3R-SLAM会在每个新图像上进行对称推理，生成新的点云和描述子。

两帧点云匹配

核心：match算法

idx_i2j, valid_match_j = matching.match(
    Xii, Xji, Dii, Dji, idx_1_to_2_init=idx_i2j_init
)

• Xii和Xji分别是当前帧和上一帧的点云，Xji是上一帧点云在当前帧坐标系下的结果；
• Dii和Dji分别是当前帧和上一帧的密集描述子，`Dji是当前帧视角下的表示；

在match函数中，首先调用prep_for_iter_proj函数将每个像素的3D点归一化为从相机出发的单位射线，并根据Scharr算子计算像素梯度。
归一化使用torch.nn.functional.normalize函数，确保每个像素的3D点在单位球面上，同时，初始化p作为匹配的索引，p[b, i, :]表示第b个batch中第i个目标3D点的初始投影像素坐标为[u, v]，代表了对于目标帧（上一帧）中的每个3D点，猜测在源帧中应该投影到哪个像素位置。

p的计算使用cuda实现，在iter_proj函数中，输入为：

• rays_with_grad_img，源图像的射线+梯度信息
• pts3d_norm，目标图像的3D点归一化
• p，初始投影位置
  核心是Levenberg-Marquardt迭代优化，使用CUDA并行策略，误差的计算为当前射线与目标射线的欧氏距离平方：

for (int j=0; j<3; j++) {
    err[j] = r[j] - pts_3d_norm[b][i][j];
}

通过融合Gauss-Newton和梯度下降的思想，迭代更新p，直到收敛或达到最大迭代次数。

然后，计算俩俩对应的3D点之间的欧氏距离，通过距离大小过滤匹配错误的点。

最后，使用密集描述子在小范围内遍历微调匹配位置，确保匹配的准确性。

至此，得到每个3D点在源图像中的投影位置p，然后通过idx_i2j将这些投影位置映射到源图像的像素索引。

后端优化

后端优化采用因子图建立拓扑关系，对于每一关键帧，找到当前关键帧上一帧关键帧，以及与当前关键帧最相似的历史关键帧，通过这些关键帧之间的相对位姿关系，构建因子图。

后端优化的核心是solve_GN_calib()函数，使用Gauss-Newton方法最小化重投影误差来优化关键帧位姿。该函数实现了MASt3R-SLAM论文中的BA部分以及滑动窗口优化策略，确保最早的几个关键帧保持固定，只优化最近的关键帧，避免过度约束。

同样，后端优化也使用CUDA实现，在gauss_newton_calib函数中，输入为：

• pose_data，使用Sim3表示的位姿参数
• Xs，3D点云
• Cs，置信度
• K，相机内参
• ii, jj, 边索引
• idx_ii2jj, 点匹配索引
• valid_match, 有效匹配掩码
• Q_ii2jj, 匹配质量
• height, width, 图像尺寸
• pixel_border, 像素边界
• z_eps, 深度阈值
• sigma_pixel, 像素噪声标准差
• sigma_depth, 深度噪声标准差
• C_thresh, 置信度阈值
• Q_thresh, 质量阈值
• max_iter, 最大迭代次数
• delta_thresh, 收敛阈值

采用Guass-Newton方法，首先计算每个点的重投影误差，然后构建雅可比矩阵和Hessian矩阵，最后通过迭代更新位姿参数，直到收敛或达到最大迭代次数。基于CUDA的并行计算，实现大规模优化。