Loop Closing 3d Point Clouds

Question

I am looking to understand how to implement a simple loop closing algorithm.

Here is my situation : I have x numbers of point clouds, and I have used a registration algorithm in 3d that has given me the pose of all these point clouds.

In the end, I more or less end up at the same point in my map, but with drift. I can use my registration algorithm to see where my actual final point cloud is located in regards to my initial one.

Knowing that, I would like to globally optimize the rest of my point clouds all the way to my initial one, based on the "drift" that I calculated.

I have managed to quickly code something in regards to the translation, which seems correct, but the rotation is problematic, as in the accuracy/superposition of features (walls etc) is reduced.

What I have been looking at : g2o, GTSAM, ISAM libraries, all looking to optimize, but I feel like there is a huge overhead to implementing those, they all require multiple constraints, setting huge number of parameters etc..

I am not even looking to detect loops automatically (I'll do that later), I would just like to do: These two point clouds represent a loop, propagate (correctly) the drift in translation and rotation(that I calculate) between them to all point clouds between the two.

Thanks in advance,

Answer 1

Loop closing as a whole is usually decomposed in these steps:

1- Loop detection 2- Loop closing 3- Loop correction

You don't need automatic loop detection right now, I'll skip it.

Loop closing is the problem of obtaining the right pose of one end to fit the other end. The pose is a SE3 transform (3D rototraslation), or if you have scale drifting (like monocular visual slam's), the pose is a Sim3 transform (3D similarity).

With this pose, Loop correction is the problem of correcting all points and keyframes to bring coherence to the whole map.

Scale Drift-Aware Large Scale Monocular SLAM is a paper from Strasdat et al describing a method to correct loops (the third step).

ORB-SLAM2 implements it in Optimizer::OptimizeEssentialGraph, and is open source.

Answer 2

I was waiting for this question. In my master degree I developed exactly that, a global optimization function to refine rotations in loopclosure cases. The product of all rotations in a circuit should be the SO(3) identity, but because of errors (drift), it's something close to that.

The trick is to compose rotations using both ways of the circuit, this will give you two independent rotations for each pose. Then, just interpolate between them using a interval proportional to the error acumulated in that pose. To do the interpolation you'll need to map all rotation matrix to quaternions (use a quaternion library), and then use SLERP (Spherical Linear Interpolation) tecnique to interpolate between them.

I'm going to explain the intervals using the image above. Supose that you have a circuit of TLS (Terrestrial Laser Scanner) stations around a object, like in (a). It can be any other type of circuit, with any geometry, and don't matter the number of clouds. Chose any cloud as the global origin, here it is the station in E0. If you register the clouds sequentialy, the registration will return relative poses:

X1(source)->X0(target) = T10 (4x4 matrix),
X2(source)->X1(target) = T21 (4x4 matrix),
X3(source)->X2(target) = T32 (4x4 matrix),
X4(source)->X3(target) = T43 (4x4 matrix), 
X0(source)->X4(target) = T04 (4x4 matrix).

Txy means the transformation which transform the point cloud on the reference frame x to y, it is a relative transformation between two adjacent clouds. Now, take the rotation R from each relative transformation T and multyply in this order: R04*R43*R32*R21*R10. What you'll obtain? If the pairwise registration has occurred with no gross errors, you wi'll find a matrix close to the identity matrix, wich represent nule rotation. If you use quaternions, you'll find something close to the quaternion identity: q_error = (1,0,0,0).

Figure (b) show relative rotations composed to the global origin in clockwise order:

R_10_a = R10
R_20_a = R21*R10
R_30_a = R30*R21*R10
R_40_a = R43*R32*R21*R10

R_00_error  = R04*R43*R32*R21*R10

Figure (c) shows relative rotations composed in anti-clockwise order:

R_40_b = (R40)^(-1)
R_30_b = (R40*R34)^(-1)
R_20_b = (R40*R34*R23)^(-1)
R_10_b = (R40*R34*R23*R12)^(-1)

The optimal rotations (R_xy_opt) will be the interpolated rotations between both absolute rotations above:

R_10_opt = interpolate(R_10_a, R_10_b, 1/5)
R_20_opt = interpolate(R_20_a, R_20_b, 2/5)
R_30_opt = interpolate(R_30_a, R_30_b, 3/5)
R_40_opt = interpolate(R_40_a, R_40_b, 4/5)

Here, R_x0 means the rotarion which transform the system i to the global origin E0. Pay attention to the interpolation intervals, they increase linearly: (1/n, 2/n, ..., n-1/n), it is because the error in last poses are bigger than the error in first ones. Doing the interpolation, you'll distribute and also decrease the total error along the circuit.

To do the interpolation you need to map rotations to quaternions:

r1 <- R_10_a 
r2 <- R_20_a 
r3 <- R_30_a 
r4 <- R_40_a 

do the same with rotations in anti-clockwise order:

p1 <- R_10_b 
p2 <- R_20_b 
p3 <- R_30_b 
p4 <- R_40_b

Now, just use the SLERP technique to do the interpolation between them:

q_opt_1 = SLERP(r1, p1, 1/5)
q_opt_2 = SLERP(r2, p2, 2/5)
q_opt_3 = SLERP(r3, p3, 3/5)
q_opt_4 = SLERP(r4, p4, 4/5)

Here q_opt_x is the optimized absolute rotation in the form of a quaternion.

Next image is a circuit with 901 clouds, accumulated error in each pairwise registration makes two ends not coincide. After the global optimization this error is backpropagated and the ends coincide. These results were obtained using two global optimizations, one for rotations, which I explained above, and another for translations, as suggested in (LU & MILIOS, 1997). Only one closed circuit is being treated. There's no use of graphs here. When dealing with this kind of optimization you naturally think in how to use multiply sobrepositions, g2o library can do this using multiples constraints within a graph, but I don't understand how it works. I only know how to optimize one closed circuit, in a graph things get complicated.

Animation of my work in the NCLT (North Campus Long Term) LiDAR dataset: https://www.youtube.com/shorts/XAiHnahhgpg

Here is my GitHub repositore with all fuctions necessary to do loop closure in a circuit and also a lot of other things related to point clouds. Everething is in python and uses only Open3D and a quaternion library. All functions are in the scrip "minhas_funcoes.py"

https://github.com/RubensBenevides/Point-Cloud-Registration-with-Global-Refinement