我希望了解如何实现一个简单的闭环算法。
这是我的情况：我有 x 个点云，并且我在 3d 中使用了配准算法，它给了我所有这些点云的姿势。
最后，我或多或少地在我的地图上的同一点结束，但有漂移。我可以使用我的配准算法来查看我的实际最终点云相对于我的初始点云的位置。
知道了这一点，我想根据我计算的“漂移”对我的其余点云进行全局优化，一直到我的初始点云。
我已经设法快速编写了一些关于翻译的代码，这似乎是正确的，但是旋转是有问题的，因为特征（墙壁等）的准确性/叠加降低了。
我一直在看：g2o、GTSAM、ISAM 库，都在寻求优化，但我觉得实现这些有很大的开销，它们都需要多个约束，设置大量参数等。
我什至不希望自动检测循环（我稍后会这样做），我只想这样做：这两个点云代表一个循环，在它们之间传播（正确）平移和旋转的漂移（我计算）到两者之间的所有点云。
提前致谢，
闭环作为一个整体通常分解为以下步骤：
1- 环路检测 2- 环路关闭 3- 环路校正
你现在不需要自动循环检测，我会跳过它。
闭环是获得一端的正确姿势以适应另一端的问题。姿势是 SE3 变换（3D 旋转平移），或者如果您有尺度漂移（如单目视觉 slam），姿势是 Sim3 变换（3D 相似性）。
有了这个姿势，循环校正就是校正所有点和关键帧以使整个地图具有连贯性的问题。
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).
然后，R从每个相对变换中获取旋转T，并按以下顺序相乘：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) 中所建议的。只处理一个闭合回路。这里没有使用具有多个路径和循环的图形。在处理这种优化时，您自然会想到如何使用乘法定位，但要避免它，它的空间太大了。