关闭3d点云彩的圈

Question

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

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

最后，我或多或少会在我的地图中的同一点结束，但随着漂移。 我可以使用我的注册算法来查看我的实际最终点云在我的初始点的位置。

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

我设法快速编写了有关翻译的内容，这似乎是正确的，但旋转是有问题的，因为特征（墙壁等）的准确性/叠加性会降低。

我一直在关注：g2o，GTSAM，ISAM库，都希望优化，但我觉得实现这些都有巨大的开销，它们都需要多个约束，设置大量的参数等。< / p>

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

提前致谢，

Answer 1

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

1-环路检测 2-循环关闭 3-循环校正

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

循环关闭是获得一端的正确姿势以适合另一端的问题。姿势是SE3变换（3D旋转变换），或者如果你有缩放漂移（如单眼视觉猛击），姿势是Sim3变换（3D相似性）。

使用此姿势，循环校正是纠正所有点和关键帧以使整个地图保持一致性的问题。

规模漂移感知大规模单眼SLAM是Strasdat等人的一篇论文，描述了一种纠正循环的方法（第三步）。

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

Answer 2

我一直在等这个问题。在我的硕士学位中，我开发了一个全局优化函数，用于在闭环情况下细化旋转。 loopclouse 旋转矩阵应该是 SO(3) 身份，对吗？但由于错误，这不是那个，它有点接近。我们不称其为“闭环旋转”，而是用一个名为 q_loop

的四元数来表示它

只需使用 SLERP 技术在姿势旋转和这些旋转乘以 q_loop^(-1) 之间进行插值。诀窍是在与电路中每个姿势成比例的间隔内进行插值。因为每个姿势都会累积不同程度的误差，所以这个间隔不可能是恒定的，对吧？这个误差与什么成正比？对于用于计算该姿势的累积变换次数，因此这些间隔也是如此。

如果您有 n 朵云，那么您需要 n-1 个姿势才能将所有内容置于全球原点。第一个pose是P1 = (R1,t1)，为了优化R1，首先将其转化为四元数q1，然后乘以q_loop^-1，我们称之为p1 = q1*q_loop^-1 .第一个最佳旋转是 q1_optimal = SLERP(q1, p1, 1/n)。第二个最佳轮换是 q2_optimal = SLERP(q2, p2, 2/n)。

根据这种模式，如果您有 n=4 朵云，最佳姿势旋转将是

q1 = SLERP(q1, p1, 1/4)

q2 = SLERP(q2, p2, 2/4)

q3 = SLERP(q3, p3, 3/4)

[1/4, 2/4, 3/4] 是插值的区间，变化范围为 [0,1]

这些间隔将误差从最终姿势分配到第一姿势，但也减少了总误差。在闭环四元数应该是恒等式的限制下，它们可以在 SLERP 技术的指数中应用最小二乘法来证明。当我们做 qn*q_loop^(-1) 并得到 pn 时，pn 是在电路中通过逆向组合的旋转。但是对于第一次姿势旋转，这种方式会累积更多的误差，因此第一次旋转在 0 附近插值，这解释了 1/4。最后的姿势旋转将在 1 附近进行插值，因为以相反的方式累积更少的错误，这解释了 3/4 姿势 P3。

对于 n=4 云，其中一个旋转正好插在中间，2/4 = 0.5，因为以这种方式或另一种方式，该姿势将累积两次旋转。该方法假设每个标准杆的旋转误差分布均匀，但事实并非如此，但该模型已经过测试，并且在来自 TLS 和机器人（velodyne LIDAR）的点云中的结果非常好。作品尚未发布。

我将解释如何使用上图找到 loopclouse 旋转。假设您有一个围绕对象的 TLS 站电路，如 (a) 中所示。它可以是任何其他类型的电路，具有任何几何形状，并且与云的数量无关。您需要按顺序注册云，从全局源 E0 开始。像这样注册：

E1(source)->E0(target) = T10 (4x4 matrix),
E2(source)->E1(target) = T21 (4x4 matrix),
E3(source)->E2(target) = T32 (4x4 matrix),
E4(source)->E3(target) = T43 (4x4 matrix), 
E0(source)->E4(target) = T04 (4x4 matrix).

然后，从每个变换 R 中取出旋转 T，并按以下顺序多次叠加：R04*R43*R32*R21*R10。你会得到什么？接近单位矩阵 3x3 的东西，代表核旋转。如果你使用四元数，你会发现一些接近于 q_loop = (1,0i,0j,0k) 的东西。使用 i,j,k = -1^1/2。

Figura (b) 显示姿势旋转，找到它们：

R_pose1 = R10
R_pose2 = R21*R10
R_pose3 = R32*R21*R10
R_pose4 = R43*R32*R21*R10
R_loop  = R04*R43*R32*R21*R10

此图是一个有901个云的电路，每次配准中累积的误差（漂移）使得两端不重合，电路的起点和终点相差80 m以上。全局优化后，此误差显着减弱，两端重合。

说实话，这些结果是使用两种全局优化获得的，一种用于旋转，我在上面解释过，另一种用于平移，如 (LU & MILIOS, 1997) 中所建议的。请记住：只有一个闭合回路正在处理。这里没有使用具有多个路径和圆环的图形。在处理这种优化的时候，你自然会想到如何使用多个sobrepositions，但要避免它，那空间太大了。