adolfos94 / 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:

• Fantoni, Simone, Umberto Castellani, and Andrea Fusiello. "Accurate and Automatic Alignment of Range Surfaces." 3DIMPVT (2012): 73-80.

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

Rotation parameterization options:

• Angle axis (Ceres)
• Unit quaternion (Eigen)
• Lie algebra (Sophus)

For evaluation, we compare against the results

• from g2o (pairwise, multiview) using GICP edges mimicking point-to-point, point-to-plane
• and the closed-form ICP solutions (only pairwise) to point-to-point, point-to-plane

Documentation

Multiview ICP technical report, August 2015

Background: ICP Variants

point-to-point:

• Besl, Paul J., and Neil D. McKay. "Method for registration of 3-D shapes." Robotics-DL tentative (1992): 586-606.

point-to-plane:

• Chen, Yang, and Gérard Medioni. "Object modelling by registration of multiple range images." Image and vision computing 10.3 (1992): 145-155.

Previous work

• Fitzgibbon, Andrew W. "Robust registration of 2D and 3D point sets." Image and Vision Computing 21.13 (2003): 1145-1153.
• Brown, Benedict J., and Szymon Rusinkiewicz. "Global non-rigid alignment of 3-D scans." ACM Transactions on Graphics (TOG). Vol. 26. No. 3. ACM, 2007.
• Pulli, Kari. "Multiview registration for large data sets." 3-D Digital Imaging and Modeling, 1999. Proceedings. Second International Conference on. IEEE, 1999.

Installation

Dependencies

• required:
  • cmake
  • Eigen3
  • Ceres Solver
• optional:
  • g2o - General Graph Optimization

Linux (Debian-based e.g. Ubuntu 18.04)

sudo apt install xorg-dev
sudo apt install cmake libeigen3-dev libceres-dev

xorg-dev is needed for glfw on linux

optional g2o

g2o is not available in the official Ubuntu repositories, you have two options to install it:

1. : For long-term releases it can be installed via a ROS installation as libg2o. E.g. for Ubuntu 14.04 install ROS Indigo. Then

sudo apt install ros-indigo-libg2o

2. : Compile and install yourself:

git clone https://github.com/RainerKuemmerle/g2o.git
git checkout 20170730_git
cd g2o && mkdir build && cd build
cmake -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF ..
sudo make install

Mac OSX (>=El Capitan)

The easiest way is by first installing homebrew package manager:

/usr/bin/ruby -e "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/master/install)"

The science tap has all our dependencies:

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

Windows (7 / 10)

• Windows C++ Compiler and/or IDE:
  • Microsoft Visual Studio IDE or just the MSVC Compiler / Build Tools
  • Qt Creator IDE which already includes the MinGW or MinGW 64 Compiler. Also see here: https://wiki.qt.io/MinGW-64-bit
• cmake: Download and install CMake for windows, e.g. cmake-3.11.0-rc3-win64-x64.msi from https://cmake.org/download/.
• Eigen3: Download Eigen Source code (header only) and set the EIGEN3_ROOT environment variable to that directory in order to allow CMake to automatically find it. Otherwise manually specify the correct path in cmake-gui
• Ceres Solver: This is the hardest part to get compiled correctly on windows, especially with Sparse Matrix support, which is needed for good performance on our sparse toy problem since only neighbouring frames are connected in the graph. If no sparse solver is installed, use a dense (but slow) solver instead which is e.g. already included with Eigen. Here are the official installation instructions for windows: http://ceres-solver.org/installation.html#windows

Building

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

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

press q to start optimization