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.
- Angle axis (Ceres)
- Unit quaternion (Eigen)
- Lie algebra (Sophus)
- 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
Multiview ICP technical report, August 2015
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.
- 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.
cmake, Eigen3, Ceres, g2o
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 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 hardes 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
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
./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
...
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