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

I was waiting for this question. In my master degree I developed a function to refine all rotations and translations in a closed loop of transformations. The product of all rotations in a circuit should be the SO(3) identity, but because of errors in each pairwise registration, it's something close to that.

Loop-closure of ROTATIONS

Let's start with the simplest example of loop closure: 3 relative poses between stations. 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 position of that pose in the circuit. To do the interpolation you will need to map all rotation matrices to quaternions and then use the SLERP (Spherical Linear Interpolation) between them. Just install pyquaternion lib to do that. After optimizing the rotations by SLERP, we will optimize the translations with a simple Least Squares model.

I am going to explain the intervals using the image bellow. Supose that you have a circuit of TLS stations around a object, like in (a). It can be any other type of circuit, with any geometry, and don't matter the sensor used to obtain the data. Chose any cloud as the global origin, here we chose the cloud in station E0. Then, register the clouds sequentialy to obtain relative poses:

T10 = registration_fuction(E1, E0) # E1_source -> E0_target 
T21 = registration_fuction(E2, E1) # E2_source -> E1_target
T02 = registration_fuction(E0, E2) # E0_source -> E2_target

Where E0, E1 and E2 are the point clouds stations; T10 means the homogeneus transformation T10 = (R10,t10) which transform the point cloud on the reference system 1 to 0 (a relative transformation, but also global, because E0 is our global origin). Now, take the rotations of each relative transformation in the matrix form and multyply in this order: R_closure = R02*R21*R10.

If the pairwise registration has occurred with no gross errors, you will find a 3x3 matrix close to the identity, the nule rotation. If you use quaternions, you will find a quaternion close to the quaternion identity:

q_closure = (1,0,0,0)    

In any closed loop of relative poses, once a global origin is defined, the first and last relative poses are also global poses. They reference the first and last cloud at the origin. The difference is that one of them will be reversed, in our case, the last one, T02, can be reverted to the global pose T20_reverse (transform the system E2 to E0). For rotations, the inverse is equal to the transpose:

R20_reverse = transpose(R02)

Now that we have R20_reverse, we can find R20_direct by rotation composition. R20_direct = R21*R10. * = matrix multiplication. R20_direct will have more errors than R20_reverse, because it acumulate two rotations. Therefore, an interpolation between the rotations R20_reverse and R20_direct should be closer to R20_reverse than to R20_direct. Specifically, it can be shown that the desired optimum rotations should be interpolated with the SLERP technique in 1/3 and 2/3, thus:

R20_optimal = SLERP(R20_direct, R20_reverse, t = 2/3)
R10_optimal = SLERP(R10_direct, R10_reverse, t = 1/3)

Where t is the interpolation interval [0,1].

And R10_reverse = transpose(R21)*transpose(R02)

For n poses:

Ri0_optimal/n = SLERP(Ri0_direct, Ri0_reverse, t = i/n)

where n is the number of clouds/stations of the circuit. For example, in a circuit with n=4, one of the rotations will be interpolated at t=1/2, because going in the reverse direction of the circuit, the most distant pose will be two stations away from the global origin.

For n=4, he optimum rotations will be:

R10_opt = SLERP(R10_direct, R10_reverse, 1/4)
R20_opt = SLERP(R20_direct, R20_reverse, 2/4)
R30_opt = SLERP(R30_direct, R30_reverse, 3/4)

Example of interpolation between too trivial rotations in python using pyquaternion:

import numpy as np
import pyquaternion as pyq

# Define two 3D rotation matrices using numpy
R10 = np.array([[1,0,0], [0,1,0], [0,0,1]])
R20 = np.array([[0,-1,0], [1,0,0], [0,0,1]])

# Map matrices to quaternions using pyquaternion
q1 = pyq.Quaternion(matrix=R10)
q2 = pyq.Quaternion(matrix=R20)

# Interpolate between the two quaternions at t=0.5
q_slerped = pyq.Quaternion.slerp(q1, q2, 1/2)
q_slerped
Quaternion(0.9238795325112867, 0.0, 0.0, 0.3826834323650898)

# Return the interpolated quaternion to matrix:
R_slerped = q_slerped.rotation_matrix
R_slerped
array([[ 0.70710678, -0.70710678,  0.        ],
       [ 0.70710678,  0.70710678,  0.        ],
       [ 0.        ,  0.        ,  1.        ]])

Loop-closure of Translations

If we write out all the matrix multiplications of a closed circuit with three stations, we get the following:

Where: This defines three closure equations: one for X, one for Y and one for Z. In our example we want to optimize the translations, so we have to derive this equation system in relation to t_10, t_21, and t_02, in each of their X, Y and Z. So, our jacobian matrix B becomes:

Note that the jacobian pattern is a simple concatenation of the absolute rotations B = [R20,R10,R_I], where R_I is the identity matrix, and R20 = R21*R10. The number of stations/clouds in the circuit define the size of this matrix. For this minimal example involving 3 transformations in a closed circuit, the matrix B has 3 rows by 9 columns. Calling w the closure vector, the solution for obtaining the corrections dy of the vector of observations y is:

Where W is the weight square matrix of the observations (if you want to use it, otherwise just use the identity),k is used just to compute 'dy', the vector of corrections in the order of the Jacobian. And 'y' is the the corrected vector of observations of our Least Squares model:

These two models are indepent. You should use the refined rotations in the Least Squares model of the translations to obtain the best results. Next image is a circuit with 901 point clouds. Accumulated error in each pairwise registration makes the two ends of the circuit to not coincide. After the global optimizations described here the drift is mitigated. Only one closed circuit is being treated. No use of graphs.

When dealing with this kind of optimization one naturally think in how to use multiply sobrepositions. g2o model can do this using multiples constraints within a graph, but I do not understand how it works.

Here is my article with these two models, which I call SLERP+LUM: https://www.mdpi.com/1424-8220/24/16/5112 SLERP refers to the optimization of rotations by SLERP and LUM refers to the optimization of translation by Lu and Milios (1992), which I explained above.

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

GitHub repositore with all fuctions necessary to do loop closure in a circuit and also a lot of other things related to point clouds. See the script "all_fuctions.py" https://github.com/RubensBenevides/Point-Cloud-Registration-with-Global-Refinement

Everything here can be done fast, as there are no iterations at any step. The SLERP operation is extremely simple and so is the inversion of a 3x3 matrix (no need of iterations on Least Squares). Therefore, both optimizations take place in O(n) time in the number of poses.

Answer 2

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.