CVX based SLAM algorithms paper read

It is a summary of a few point cloud matching algorithms based on convex optimzation theory.

1 Banch and Bound

1.1 Practical Global Optimization for Multiview Geometry

article researchgate

Agarwal S , Chandraker M K , Kahl F , et al. Practical Global Optimization for Multiview Geometry[C]// Computer Vision - ECCV 2006, 9th European Conference on Computer Vision, Graz, Austria, May 7-13, 2006, Proceedings, Part I. Springer, Berlin, Heidelberg, 2006.

This article talks about using Banch and Bound algorithm for three types of Multiview Geometry problems:

Camera pixels triangualtion
Camera Resectioning, which is to estimate camera matrix P.
Projection from $\mathbf{P}^{n}$ to $\mathbf{P}^{m}$ , which could be homography estimation and use 3d point matches to estiamte relative transformation.

This article uses :

Fractinal Programming to reform these problems
Using Convex Envelope of these Fractinal problems as a lower bound for Banch and Bound
Use three type of nosie models :
** Ordinary Gaussian noise model :
$p(x|x') = \frac{1}{2 \pi \sigma^{2}} \exp( - \frac{ \| x - x' \|^{2}_{2}}{2\sigma^{2}})$
** A special case for Laplace nosie
$p(x|x') = \frac{1}{2 \pi \sigma} \exp( - \frac{ \| x - x' \|_{2}}{\sigma})$
** Bivariate Laplace distribution
$p(x|x') = \frac{1}{4 \sigma} \exp( - \frac{ \| x - x' \|_{1}}{\sigma})$

Here is its result on these three problems with different noise models.

it shows that the tridational method (Bundle Adjustment method) for point cloud match is very senstive to noise, and easily trapped in local minimal. While BNB method find a global optimal is apparent much better.
And it shows that L2 Guassian noise model shows better result for camera resectioning (the least reprojection error). While L1 Laplacian model shows better result for ICP type problems.
And from the Table2, we can see a real L1 norm will conerge much faster. (It may be true that the 21 centry is the centry for L1 norm)

1.2 Branch-and-Bound Methods for Euclidean Registration Problems

paper link

Olsson C , Kahl F , Oskarsson M . Branch-and-Bound Methods for Euclidean Registration Problems[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2009, 31(5):783-794.

This is a article focus on the 3D points registration problems, which needs already matched point pairs.

The lower bounds are obtained by Convex relaxations of $\mathbf{SO}(3)$ , which is the rotation part. It uses quaternion expression of the rotation. And apply a convex envelop over $q_{i}q_{j}$ .

It shows a much better result compared to linear algebra methods (DLT), and have a competable result compared to DLT + bundle adjustment.
It is much much more stable compared to the other methods.
While even though they didn’t show, I think the comptation time it needs is much more than the other two methods.

2. Lagrangian Duality

2.1 Convex Global 3D Registration with Lagrangian Duality

articel link

Briales J , Gonzalez-Jimenez J . Convex Global 3D Registration with Lagrangian Duality[C]// IEEE Conference on Computer Vision & Pattern Recognition. IEEE, 2017.

This article focus on Lagrangian Duality of ICP problems, and in practice it shows strong duality holds, and it shows a competable result compared with the article of 1.2(Branch-and-Bound Methods for Euclidean Registration Problems).

It use the matrix representation of rotation.
It has a very detail summary of the ICP methods (point-to-point, point-to-line, and point-to-plane).
It reform the problem into QCQP (nonconvex quadratically constrained quadratic programs) (much more about QCQP could be found here ), so the problem can be reformed into SDP (semidefinite program) via Lagrangian Duality (or via SDP relaxation).

I strongly recommond to read its supplemenraty material, it is a very good learning material for QCQP and ICP.

The results are mainly compared with the article of 1.2(Branch-and-Bound Methods for Euclidean Registration Problems). It shows that strong duality holds in practice (no proof offered), and this method shows a much more short CPU runtime (as we don’t need to search).

2.1.1 General form of ICP

We skip the proof here. and all the later algorithms can all be found in the additional material of the article.

2.1.2 Objective function

The obejctive function could be reform into quadratic function.

2.1.3 Marginalize translation terms

2.1.4 SO(3) Constraint

The main issue is that the origianl rotation constrain is not quadratic, So the author reform it into quadratic.

2.1.5 Primal problem

2.1.6 Duality

2.2 Solving Quadratically Constrained Geometrical Problems using Lagrangian Duality

article link

This content of this article is already contained in the upper article 1.2 and 1.1.

2.3 Duality-based Verification Techniques for 2D SLAM

article link

Carlone L , Dellaert F . Duality-based verification techniques for 2D SLAM[J]. Proceedings IEEE International Conference on Robotics & Automation, 2015, 2015:4589-4596.

It reform the problem into QCQP and use SDP relaxation to find bound. And this article mainly focus on a evulation of SLAM result, using a lower bound and a upper bound. Does not offer a BNB (or other) algorithm to be aimed at solving the problem.

The loss function is difference of poses.
It use matrix representation of rotation, and use Chordal distance(element wise squared norm) as error term.
Reform the problem into QCQP and find SDP relaxations.
Find a lower bound using exact SDP relaxation and a looser bound using a simplfied SDP.
Find upper bound by projecting the duality result into feaible set.

Better see the next article for more, which a more detailed version of this work.

2.4 Lagrangian Duality in 3D SLAM: Verification Techniques and Optimal Solutions

article link

Carlone L , Rosen D M , Calafiore G , et al. Lagrangian Duality in 3D SLAM: Verification Techniques and Optimal Solutions[C]// 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2015.

Mostly the same as the article before (2.3 Duality-based Verification Techniques for 2D SLAM) except that it in extended to 3D SLAM problems.

Here we see the analysis of results.

The bounds are good, and these two articles are very good material for a application of relaxation of QCQPs.

3. TEASER++

paper link

Yang H , Shi J , Carlone L . TEASER: Fast and Certifiable Point Cloud Registration[J]. 2020.

This is the main subject here. This method is a outlier robust, correspondence free 3D point cloud registration algorithm. It has three main contributions:

Truncated Least Squares (TLS) loss function (robust to large outliers)
Graph-theoretic framework to decouple scale, rotation and translation (it includes the etimation of scale)
A tight (empirically) SDP relaxation of the rotation estimation.

3.1 Related works

3.1.1 Correspondence based methods

3.1.1 Correspondence free methods

3.2 Pipeline

3.2.1 Objective function

3.2.2 Marginalize translation

3.2.3 Marginalize Rotation

3.2.4 Pipeline

3.3 Adaptive Voting

It is a generalization of Histogram voting algorithms.
Used to calculate translation and scale part of the problem.
Explain by focus on the scale part:

The auther has a comparsion of TLS and consensus maximization methods, it worth to read.

3.4 Reform Rotation

3.4.1 Original Expression

3.4.2 Reform into Quaternion form

3.4.3 Eliminate variable theta

3.4.4 Turn into QCQP

3.4.5 SDP relaxation

3.4.6 Global optimal

3.4.7 Optimality Certification

This article also propose an algorithm to evaluate a feasible ‘solution’ (not necessary optimal), by offering a sub-optimality bound.

3.4.8 GNC C++

see here
I was very curious about how they actually solve the SDP using C++ (as I could not find a CVX version for C++). Then I invested its reference articles and its source code.

It uses a Method called “GNC”, which is a piecewise polynomial approximation to the truncated quardatic functions (developped by Blake and Zisserman 1987).
And it using the theory from the article " On the unification of line processes, outlier rejeection, and robust statistics with applications in early vision". Whose theme is that Robust estimation and outlier rejection process are closely related, and can be converted into each other.
In summary, it uses a iterative reweighted SVD based rotation solver.

From its code (the main processes):

    // 1. Optimize for line processes weights
    Eigen::Matrix<double, 3, 1> q, p, rpq;
    for (size_t j = 0; j < match_size; ++j) {
      // p = Rq
      q = src.col(j);
      p = dst.col(j);
      rpq = p - (*rotation) * q;
      l_pq(j) = std::pow(scaled_mu / (scaled_mu + rpq.squaredNorm()), 2);
    }

    // 2. Optimize for Rotation Matrix
    *rotation = teaser::utils::svdRot(src, dst, l_pq);

(I was hopping to find something more interesting to solve SDP using C++ …)