我希望了解如何实现一个简单的闭环算法。
这是我的情况：我有 x 个点云，并且我在 3d 中使用了配准算法，它为我提供了所有这些点云的姿势。
最后，我或多或少地在地图上的同一点结束，但有漂移。 我可以使用我的配准算法来查看我的实际最终点云相对于我的初始点云的位置。
知道了这一点，我想根据我计算的“漂移”对其余的点云进行全局优化，直至最初的点云。
我已经设法快速编写了一些关于平移的代码，这似乎是正确的，但旋转存在问题，因为特征（墙壁等）的准确性/叠加降低了。
我一直在看：g2o、GTSAM、ISAM 库，都在寻求优化，但我觉得实现这些有很大的开销，它们都需要多个约束，设置大量参数等。
我什至不想自动检测循环（我稍后会这样做），我只想这样做： 这两个点云代表一个循环，将它们之间的平移和旋转（我计算的）漂移（正确地）传播到两者之间的所有点云。
提前致谢，
【问题讨论】：
标签： point-clouds slam-algorithm slam g2o
闭环作为一个整体通常分解为以下步骤：
1- 循环检测 2-闭环 3-循环校正
你现在不需要自动循环检测，我会跳过它。
闭环是获取一端的正确姿势以适应另一端的问题。姿势是 SE3 变换（3D 旋转平移），或者如果您有尺度漂移（如单目视觉 slam），姿势是 Sim3 变换（3D 相似性）。
在这个姿势下，Loop Correction 就是修正所有点和关键帧的问题，以使整个地图具有连贯性。
Scale Drift-Aware Large Scale Monoocular SLAM 是 Strasdat 等人的一篇论文，描述了一种纠正循环的方法（第三步）。
ORB-SLAM2 在 Optimizer::OptimizeEssentialGraph 中实现，并且是开源的。
【讨论】：
我在等这个问题。在我的硕士学位中，我开发了一个全局优化函数来优化闭环情况下的旋转。我还将它与另一个关于翻译的全局优化集成在一起，但主要部分是旋转。电路中所有旋转的乘积应该是 SO(3) 恒等式，但是，由于误差（漂移），它接近那个。
诀窍是使用电路的两种方式组合旋转，这将为每个姿势提供两次旋转，然后，只需使用与该姿势中累积的误差成比例的间隔在它们之间进行插值。要进行插值，您需要将所有旋转矩阵映射到四元数，然后使用 SLERP 技术（球面线性插值）。
我将使用上图解释间隔。假设您在一个物体周围有一个 TLS（地面激光扫描仪）站电路，如 (a) 所示。它可以是任何其他类型的电路，具有任何几何形状，与云的数量无关。如果您按顺序注册云，注册将返回相对姿势：
X1(source)->X0(target) = T01 (4x4 matrix),
X2(source)->X1(target) = T12 (4x4 matrix),
X3(source)->X2(target) = T23 (4x4 matrix),
X4(source)->X3(target) = T34 (4x4 matrix),
X0(source)->X4(target) = T40 (4x4 matrix).
然后，从每个相对变换 T 中获取旋转 R，并按以下顺序乘以：R40*R34*R23*R12*R01。你会得到什么？接近单位矩阵的东西，代表核旋转。如果你使用四元数，你会发现接近 q_loop = (1,0i,0j,0k) 的东西。
图（b）显示了按顺时针顺序组合到全局原点的相对旋转：
R_01_a = R01
R_02_a = R12*R01
R_03_a = R23*R12*R01
R_04_a = R34*R23*R12*R01
R_near_I = R40*R34*R23*R12*R01
图（c）显示了以逆时针顺序组成的相对旋转：
R_04_b = (R40)^(-1)
R_03_b = (R40*R34)^(-1)
R_02_b = (R40*R34*R23)^(-1)
R_01_b = (R40*R34*R23*R12)^(-1)
最佳姿势旋转将是：
R_01_opt = SLERP(R_01_a, R_01_b, 1/5)
R_03_opt = SLERP(R_02_a, R_02_b, 2/5)
R_02_opt = SLERP(R_03_a, R_03_b, 3/5)
R_04_opt = SLERP(R_04_a, R_04_b, 4/5)
因此，您可以想象，间隔线性增加，因为最后一个姿势的误差大于第一个姿势。这种方法可以分散电路中的总误差，也降低了电路中的总误差。
在图中我用四元数表示旋转：
q1 = R01
q2 = R12
q3 = R23
q4 = R34
q5 = R40
绝对旋转：
r1 = R_01_a
r2 = R_02_a
r3 = R_03_a
r4 = R_04_a
p1 = R_01_b
p2 = R_02_b
p3 = R_03_b
p4 = R_04_b
该图像是一个有 901 个云的电路，每次配准中累积的误差（漂移）使两端不重合，电路的起点和终点相差 80 m 以上。在全局优化之后，这个错误被反向传播并且两端重合。
这些结果是使用两种全局优化获得的，一种用于旋转，我在上面解释过，另一种用于平移，如 (LU & MILIOS, 1997) 中所建议的。只处理一个闭合回路。这里没有使用具有多个路径和循环的图形。在处理这种优化时，您自然会想到如何使用乘法定位，但要避免它，它的空间太大了。
【讨论】：