View on GitHub

mv-lm-icp

Multiview Levenberg-Marquardt ICP

mv-lm-icp - Multiview Levenberg-Marquardt ICP

The Iterative Closest Point (ICP) Algorithm extended to the multiview setting. Point-to-point and point-to-plane metrics.

A C++ implementation of Section 3.3.2 in:

Using nanoflann for the kd-tree and Ceres for the actual minimization.

Rotation parameterization options:
For evaluation, we compare against the results

Documentation

https://github.com/adrelino/mv-lm-icp/blob/master/docs/mv-lm-icp.pdf

Background: ICP Variants

point-to-point:

point-to-plane:

Previous work

Dependencies

cmake, Eigen3, Ceres, g2o

OSX El Capitan

brew tap homebrew/science
brew install cmake eigen ceres-solver g2o

Building

git checkout https://github.com/adrelino/mv-lm-icp.git
cd mv-lm-icp
git submodule init && git submodule update

mkdir build && cd build
cmake .. && make

Execution

pairwise

./pairwise 
-0.076899 -0.081785     0.421	-0.226502 -0.628639 -0.743983
-0.076716 -0.080814      0.42	-0.167658 -0.600136  -0.78213
-0.076716 -0.080037      0.42	-0.270241  -0.48616 -0.831035
-0.076533 -0.079071     0.419	-0.305349 -0.342155 -0.888646
-0.076533 -0.078296     0.419	-0.373175  -0.41905 -0.827731
-0.076533 -0.077521     0.419	-0.342708 -0.344125 -0.874145
-0.076351 -0.076562     0.418	-0.436854 -0.442649 -0.783084
-0.076351 -0.075789     0.418	-0.436506 -0.404281 -0.803753
-0.076168 -0.074836     0.417	-0.438123 -0.443576  -0.78185
-0.076168 -0.074065     0.417	-0.437831 -0.405047 -0.802647

=====  TIMING[closed] is 0.712000 s


=====  TIMING[g2o] is 0.88537000 s


=====  TIMING[ceres CeresAngleAxis] is 0.74449000 s


=====  TIMING[ceres EigenQuaternion] is 0.85321000 s

analytic diff sophusSE3 local parameterization

=====  TIMING[ceres SophusSE3] is 0.83088000 s

=====  TIMINGS ====
ceres CeresAngleAxis:	0.074
ceres EigenQuaternion:	0.085
ceres SophusSE3     :	0.083
closed              :	0.001
g2o                 :	0.089

=====  Accurracy ====
ceres CeresAngleAxis	 diff_tra:7.76957e-11	 diff_rot_degrees:1.70755e-06

ceres EigenQuaternion	 diff_tra:6.31278e-11	 diff_rot_degrees:1.70755e-06

ceres SophusSE3    	 diff_tra:6.31278e-11	 diff_rot_degrees:1.70755e-06

closed form      	 diff_tra:6.5958e-15	 diff_rot_degrees:2.41484e-06

g2o              	 diff_tra:6.43487e-17	 diff_rot_degrees:0


multiview

./multiview
...
Visualize construct
graph adjacency matrix == block structure
0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1
1 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 1 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 1 0 1 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 1 0 1 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 1 0 1 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 1 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 1 0 1 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 1 0 1 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0

multiview-init

press q to start optimization

multiview-final