python 点云配准_TEASER++：快速鲁棒的C++点云配准库

TEASER++: fast & certifiable 3D registration

TEASER++ is a fast and certifiably-robust point cloud registration library written in C++, with Python and MATLAB bindings.

About

Left: correspondences generated by 3DSmoothNet (green and red lines represent the inlier and outlier correspondences according to the ground truth respectively). Right: alignment estimated by TEASER++ (green dots represent inliers found by TEASER++).

TEASER++ can solve the rigid body transformation problem between two point clouds in 3D. It performs well even if the input correspondences have an extremely large number of outliers. For a short conceptual introduction, check out our video. For more information, please refer to our papers:

H. Yang, J. Shi, and L. Carlone, "TEASER: Fast and Certifiable Point Cloud Registration,". arXiv:2001.07715 [cs, math], Jan. 2020. (pdf)

H. Yang and L. Carlone, “A Polynomial-time Solution for Robust Registration with Extreme Outlier Rates,” in Robotics: Science and Systems (RSS), 2019. (pdf)

If you find this library helpful or use it in your projects, please cite:

@article{Yang20arXiv-TEASER,

title={TEASER: Fast and Certifiable Point Cloud Registration},

author={Yang, Heng and Shi, Jingnan and Carlone, Luca},

year={2020},

eprint={2001.07715},

archivePrefix={arXiv},

primaryClass={cs.RO},

url = {https://github.com/MIT-SPARK/TEASER-plusplus},

pdf = {https://arxiv.org/abs/2001.07715}

}

If you are interested in more works from us, please visit our lab page here.

TL;DR

Minimal C++ example

Run the following script to show a minimal C++ example:

sudo apt install cmake libeigen3-dev libboost-all-dev

git clone https://github.com/MIT-SPARK/TEASER-plusplus.git

cd TEASER-plusplus && mkdir build && cd build

cmake .. && make

sudo make install

cd .. && cd examples/teaser_cpp_ply && mkdir build && cd build

cmake .. && make

./teaser_cpp_ply

You should see terminal output like this:

Read 1889 total vertices

*** [pmc heuristic: thread 1] current max clique = 577, time = 0.00163579 sec

...

*** [pmc: thread 2] current max clique = 602, time = 0.44515 sec

-----------------------------------------------------------------------

=====================================

TEASER++ Results

=====================================

Expected rotation:

0.996927 0.0668736 -0.0406664

-0.066129 0.997618 0.0194009

0.0418676 -0.0166518 0.998978

Estimated rotation:

0.996658 0.0729647 0.0367288

-0.0740469 0.996832 0.0290182

-0.0344951 -0.0316408 0.998904

Error (deg): 0.0783556

Expected translation:

-0.115577

-0.0387705

0.114875

Estimated translation:

-0.116132

-0.0390858

0.11729

Error (m): 0.00249818

Number of correspondences: 1889

Number of outliers: 1700

Time taken (s): 0.786677

Minimal Python 3 example

Run the following script to show a minimal Python 3 example (needs Anaconda installed):

sudo apt install cmake libeigen3-dev libboost-all-dev

conda create -n teaser_test python=3.6 numpy

conda activate teaser_test

conda install -c open3d-admin open3d=0.9.0.0

git clone https://github.com/MIT-SPARK/TEASER-plusplus.git

cd TEASER-plusplus && mkdir build && cd build

cmake -DTEASERPP_PYTHON_VERSION=3.6 .. && make teaserpp_python

cd python && pip install .

cd ../.. && cd examples/teaser_python_ply

python teaser_python_ply.py

You should see output similar to this:

==================================================

TEASER++ Python registration example

==================================================

*** [pmc heuristic: thread 1] current max clique = 563, time = 0.00185895 sec

...

*** [pmc: thread 2] current max clique = 605, time = 0.618481 sec

-----------------------------------------------------------------------

=====================================

TEASER++ Results

=====================================

Expected rotation:

[[ 0.99692656 0.06687358 -0.04066644]

[-0.06612899 0.99761788 0.01940087]

[ 0.04186755 -0.01665178 0.99897777]]

Estimated rotation:

[[ 9.96883589e-01 7.88648224e-02 -1.85738207e-03]

[-7.88858464e-02 9.96487579e-01 -2.80985536e-02]

[-3.65129272e-04 2.81575081e-02 9.99603432e-01]]

Error (deg):

0.06284342361637997

Expected translation:

[-0.11557694 -0.03877054 0.11487489]

Estimated translation:

[-0.11652176 -0.0373522 0.111885 ]

Error (m):

0.0034414811018018978

Number of correspondences: 1889

Number of outliers: 1700

Time taken (s): 0.9492652416229248

Reproduce the GIF Above

Run the following script:

sudo apt install cmake libeigen3-dev libboost-all-dev

conda create -n teaser_3dsmooth python=3.6 numpy

conda activate teaser_3dsmooth

conda install -c open3d-admin open3d=0.9.0.0

conda install scikit-learn

git clone https://github.com/MIT-SPARK/TEASER-plusplus.git

cd TEASER-plusplus && mkdir build && cd build

cmake -DTEASERPP_PYTHON_VERSION=3.6 .. && make teaserpp_python

cd python && pip install .

cd ../.. && cd examples/teaser_python_3dsmooth

python teaser_python_3dsmooth.py

You should be able to see Open3D windows showing registration results:

Getting Started

Supported Platforms

TEASER++ has been tested on Ubuntu 18.04 with g++-7/9 and clang++-7/8/9.

Installing Dependencies

Building TEASER++ requires the following libraries installed:

A compiler that supports OpenMP. See here for a list.

CMake >= 3.10

Eigen3 >= 3.3

PCL >= 1.9 (optional)

Boost >= 1.58 (optional)

Run the following script to install all required dependencies:

sudo apt install cmake libeigen3-dev libboost-all-dev

Run the following script to install PCL from source:

# Compile and install PCL 1.91 from source

PCL_PACKAGE_DIR="$HOME/pcl"

mkdir "$PCL_PACKAGE_DIR"

cd "$PCL_PACKAGE_DIR"

wget "https://github.com/PointCloudLibrary/pcl/archive/pcl-1.9.1.zip"

unzip pcl-*.zip

rm pcl-*.zip

cd pcl-* && mkdir build && cd build

cmake ..

make -j $(python3 -c 'import multiprocessing as mp; print(int(mp.cpu_count() * 1.5))')

sudo make install

Notice that PCL is not required for the TEASER++ registration library. Installing it merely allows you to build example tests that uses PCL's FPFH features for registration.

If you want to build Python bindings, you also need:

Python 2 or 3 (make sure to include the desired interpreter in your PATH variable)

If you want to build MATLAB bindings, you also need:

MATLAB

CMake >= 3.13

TEASER++ uses the Parallel Maximum Clique (paper, code) for maximum clique calculation. It will be downloaded automatically during CMake configuration. In addition, CMake will also download Google Test and pybind11 if necessary.

Compilation and Installation

Clone the repo to your local directory. Open a terminal in the repo root directory. Run the following commands:

# Clone the repo

git clone https://github.com/MIT-SPARK/TEASER-plusplus.git

# Configure and compile

cd TEASER-plusplus && mkdir build

cd build

cmake ..

make

# Generate doxygen documentation in doc/

make doc

# Run tests

ctest

# Install shared libraries and headers

sudo make install

Available CMake Options

Here are the available CMake options you can turn on/off during configuration:

Option Name

Description

Default Value

BUILD_TESTS

Build tests

ON

BUILD_TEASER_FPFH

Build TEASER++ wrappers for PCL FPFH estimation

OFF

BUILD_MATLAB_BINDINGS

Build MATLAB bindings

OFF

BUILD_PYTHON_BINDINGS

Build Python bindings

ON

BUILD_DOC

Build documentation

ON

BUILD_WITH_MARCH_NATIVE

Build with flag march=native

OFF

ENABLE_DIAGNOSTIC_PRINT

Enable printing of diagnostic messages

OFF

For example, if you want to build with the march=native flag (potentially faster at a loss of binary portability), run the following script for compilation:

cmake -DBUILD_WITH_MARCH_NATIVE=ON ..

make

Notice that by default the library is built in release mode. To build with debug symbols enabled, use the following commands:

cmake -DCMAKE_BUILD_TYPE=Debug ..

make

Run Tests

By default, the library is built in release mode. If you instead choose to build it in debug mode, some tests are likely to time out.

To run tests and benchmarks (for speed & accuracy tests), you can execute the following command:

# Run all tests

ctest

# Run benchmarks

ctest --verbose -R RegistrationBenchmark.*

The --verbose option allows you to see the output, as well as the summary tables generated by each benchmark.

How to use TEASER++

In CMake-based C++ Projects

When installing TEASER++, CMake will export the following targets that can be included in other CMake projects using find_package():

teaserpp::teaser_registration: the core registration library

teaserpp::teaser_io: library for importing .ply files

teaserpp::teaser_features: convenience wrappers around the PCL FPFH library, and simple feature matching functions

A minimally-working CMakeList.txt looks something like this:

cmake_minimum_required(VERSION 3.10)

project(teaserpp_example)

set (CMAKE_CXX_STANDARD 14)

find_package(Eigen3 REQUIRED)

find_package(teaserpp REQUIRED)

# Change this line to include your own executable file

add_executable(cpp_example cpp_example.cpp)

# Link to teaserpp & Eigen3

target_link_libraries(cpp_example Eigen3::Eigen teaserpp::teaser_registration teaserpp::teaser_io)

In the examples/ folder, you can find two C++ examples that can be compiled with CMake:

teaser_cpp_ply: showing how to import .ply files and perform registration with TEASER++

teaser_cpp_fpfh: showing how to use TEASER++ with FPFH features

In Python

In the examples/ folder, you can find two C++ examples that can be compiled with CMake:

teaser_python_ply: showing how to import .ply files and perform registration with TEASER++ and Open3D

teaser_python_3dsmooth: showing how to use TEASER++ on descriptors generated by 3DSmoothNet on the 3DMatch dataset, with Open3D visualization

For a short documentation on how to use the Python bindings for TEASER++, please refer to this document.

In MATLAB

For a short documentation on how to use the MATLAB bindings for TEASER++, please refer to this document.

In ROS

To use TEASER++ in a ROS environment, simple clone the repo to your catkin workspace.

Known Issues

If you are encountering segmentation faults from PMC, try add the environmental variable OMP_NUM_THREADS=${MAX_THREADS} (replace ${MAX_THREADS} with the maximum number of threads available on your machine) in your current shell. You can also just prepend OMP_NUM_THREADS=${MAX_THREADS} when running your executable.

When using the MATLAB wrapper with MATLAB on terminal (-nojvm option enabled), you might encounter errors similar to this: /usr/local/MATLAB/R2019a/bin/glnxa64/MATLAB: symbol lookup error: /opt/intel/compilers_and_libraries_2019.4.243/linux/mkl/lib/intel64_lin/libmkl_vml_avx2.so: undefined symbol: mkl_serv_getenv. One way to get around this is to run the following command in the environment where you start MATLAB: export LD_PRELOAD=/opt/intel/mkl/lib/intel64/libmkl_intel_lp64.so:/opt/intel/mkl/lib/intel64/libmkl_gnu_thread.so:/opt/intel/mkl/lib/intel64/libmkl_core.so. You may need to change the paths according to your MKL installation.

Other Publications

Other publications related to TEASER include:

H. Yang and L. Carlone, “A quaternion-based certifiably optimal solution to the Wahba problem with outliers,” in Proceedings of the IEEE International Conference on Computer Vision (ICCV), 2019, pp. 1665–1674. (pdf)

H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection,” IEEE Robotics and Automation Letters (RA-L), 2020. (pdf)

Acknowledgements

This work was partially funded by ARL DCIST CRA W911NF-17-2-0181, ONR RAIDER N00014-18-1-2828, Lincoln Laboratory “Resilient Perception in Degraded Environments”, and the Google Daydream Research Program.