闭合三维点云的循环

Question

我希望了解如何实现一个简单的循环关闭算法。

这是我的情况:我有x个点云，我在3d中使用了一个配准算法，它给了我所有这些点云的姿势。

最后，我或多或少地在我的地图上的同一个点结束了，但是漂移了。我可以使用我的配准算法来查看我的实际最终点云相对于我的初始点云的位置。

知道了这一点，我想基于我计算的“漂移”，全局地优化我的其余点云，一直到我的初始点云。

我已经设法快速地编写了一些关于翻译的代码，这看起来是正确的，但旋转是有问题的，因为特征(墙等)的准确性/叠加性降低了。

我一直在看的: g2o，GTSAM，ISAM库，都在寻求优化，但我觉得实现这些都有很大的开销，它们都需要多个约束，设置大量的参数等。

我甚至不想自动检测环路(我稍后会这样做)，我只是想这样做:这两个点云代表一个环路，将它们之间的平移和旋转(我计算的)漂移传播(正确)到这两个点云之间的所有点云。

提前谢谢你，

Answer 1

循环关闭作为一个整体通常在以下步骤中分解：

1-回路检测2-回路闭合3-回路校正

你现在不需要自动循环检测，我跳过它。

闭合环是指获得一端适合另一端的正确姿势的问题。姿势是SE3变换(3D旋转变换)，或者如果你有比例漂移(像单目视觉slam)，姿势是Sim3变换(3D相似性)。

使用此姿势，循环校正是校正所有点和关键帧以使整个地图连贯的问题。

Scale Drift-Aware大规模单目SLAM是Strasdat等人的一篇论文，描述了一种纠正循环的方法(第三步)。

ORB-SLAM2在Optimizer::OptimizeEssentialGraph中实现了它，并且是开源的。

Answer 2

我一直在等这个问题。在我的硕士学位中，我开发了一个全局优化函数来改进循环闭包情况下的旋转。我还将它与另一个关于翻译的全局优化集成在一起，但主要部分是旋转。电路中所有旋转的乘积应该是SO(3)恒等式，但由于误差(漂移)，它接近于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。你会得到什么？接近单位矩阵的东西，表示nule旋转。如果你使用四元数，你会发现一些接近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个云的电路，在每个配准中累积的误差(漂移)使得两端不重合，电路的开始和结束相差超过80m。在全局优化之后，该错误被反向传播，并且结束重合。

​

​

这些结果是通过两种全局优化获得的，一种是针对旋转的，我在上面解释过，另一种是针对平移的，如(LU & MILIOS，1997)所建议的。只有一个闭合回路正在接受治疗。这里不使用具有多个路径和环路的图。在处理这种优化时，你自然会想到如何使用multiply sobrepositions，但要避免它，它的空间太大了。