diff --git a/.clang-format b/.clang-format index 6761564..df7b1ca 100644 --- a/.clang-format +++ b/.clang-format @@ -1,8 +1,7 @@ --- -Language: Cpp +Language: Cpp ColumnLimit: 100 DerivePointerAlignment: false PointerAlignment: Left SortIncludes: false -InsertNewlineAtEOF: true ... diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 8b3d198..ec58a06 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -1,29 +1,35 @@ -name: build -on: +name: build + +on: push: - branches: + branches: - master pull_request: branches: - master + jobs: + build-debug: name: CMake Debug Build - runs-on: ubuntu-latest + runs-on: ubuntu-20.04 + steps: - - uses: actions/checkout@v4 - - name: install-dependencies + - uses: actions/checkout@v2 + - name: install-eigen run: sudo apt install libeigen3-dev - name: configure run: mkdir build && cd build && cmake -DCMAKE_BUILD_TYPE=Debug .. - name: build run: cmake --build build + build-release: name: CMake Release Build - runs-on: ubuntu-latest + runs-on: ubuntu-20.04 + steps: - - uses: actions/checkout@v4 - - name: install-dependencies + - uses: actions/checkout@v2 + - name: install-eigen run: sudo apt install libeigen3-dev - name: configure run: mkdir build && cd build && cmake -DCMAKE_BUILD_TYPE=Release .. @@ -31,19 +37,4 @@ jobs: run: cmake --build build - name: test run: cd build && ctest all - python: - name: Python Build - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - name: setup-python - uses: actions/setup-python@v5 - with: {python-version: '3.x'} - - name: install-dependencies - run: sudo apt install libeigen3-dev - - name: upgrade-pip - run: python -m pip install --upgrade pip - - name: build - run: python -m pip install ${{github.workspace}} - - name: test - run: python -c "import teaserpp_python; print('☺')" + diff --git a/CMakeLists.txt b/CMakeLists.txt index be1e677..2a9c194 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,10 +1,9 @@ -cmake_minimum_required(VERSION 3.16) +cmake_minimum_required(VERSION 3.10) project(teaserpp VERSION 1.0.0) set(CMAKE_CXX_STANDARD 14) set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules/" ${CMAKE_MODULE_PATH}) -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # Check build types if (NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) @@ -13,16 +12,11 @@ if (NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) STRING "Choose the type of build." FORCE) endif () -if (DEFINED SKBUILD) - message(STATUS "Building with Scikit-Build") -endif () - # Options -option(BUILD_SHARED_LIBS "Build shared libraries" ON) option(BUILD_TESTS "Build tests" ON) option(BUILD_TEASER_FPFH "Build TEASER++ wrappers for PCL FPFH estimation." OFF) option(BUILD_MATLAB_BINDINGS "Build MATLAB bindings" OFF) -option(BUILD_PYTHON_BINDINGS "Build Python bindings" OFF) +option(BUILD_PYTHON_BINDINGS "Build Python bindings" ON) option(BUILD_DOC "Build documentation" ON) option(BUILD_WITH_MARCH_NATIVE "Build with flag march=native" OFF) option(ENABLE_MKL "Try to use Eigen with MKL" OFF) @@ -33,6 +27,11 @@ if (ENABLE_DIAGNOSTIC_PRINT) add_definitions(-DTEASER_DIAG_PRINT) endif () +# Cache Variables +if (NOT TEASERPP_PYTHON_VERSION) + set(TEASERPP_PYTHON_VERSION "" CACHE STRING "Python version to use for TEASER++ bindings.") +endif () + # Find dependencies # Eigen3 find_package(Eigen3 3.2 QUIET REQUIRED NO_MODULE) @@ -74,6 +73,44 @@ if (BUILD_TEASER_FPFH) endif() endif () +# googletest +configure_file(cmake/GoogleTest.CMakeLists.txt.in googletest-download/CMakeLists.txt) +execute_process(COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/googletest-download") +execute_process(COMMAND "${CMAKE_COMMAND}" --build . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/googletest-download") +set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) +add_subdirectory("${CMAKE_BINARY_DIR}/googletest-src" + "${CMAKE_BINARY_DIR}/googletest-build" EXCLUDE_FROM_ALL) + +# pmc (Parallel Maximum Clique) +configure_file(cmake/pmc.CMakeLists.txt.in pmc-download/CMakeLists.txt) +execute_process(COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/pmc-download") +execute_process(COMMAND "${CMAKE_COMMAND}" --build . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/pmc-download") +add_subdirectory("${CMAKE_BINARY_DIR}/pmc-src" + "${CMAKE_BINARY_DIR}/pmc-build") + +# tinyply +configure_file(cmake/tinyply.CMakeLists.txt.in tinyply-download/CMakeLists.txt) +execute_process(COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/tinyply-download") +execute_process(COMMAND "${CMAKE_COMMAND}" --build . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/tinyply-download") +add_subdirectory("${CMAKE_BINARY_DIR}/tinyply-src" + "${CMAKE_BINARY_DIR}/tinyply-build") +target_include_directories(tinyply PUBLIC + $) + +# spectra +configure_file(cmake/spectra.CMakeLists.txt.in spectra-download/CMakeLists.txt) +execute_process(COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/spectra-download") +execute_process(COMMAND "${CMAKE_COMMAND}" --build . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/spectra-download") +set(SPECTRA_INCLUDE_DIRS "${CMAKE_BINARY_DIR}/spectra-src/include") + # Building Targets set(TEASERPP_ROOT ${CMAKE_CURRENT_LIST_DIR}) add_subdirectory(teaser) @@ -93,6 +130,17 @@ if (BUILD_MATLAB_BINDINGS) endif () if (BUILD_PYTHON_BINDINGS) + set(PYBIND11_PYTHON_VERSION ${TEASERPP_PYTHON_VERSION}) + + # download the pybind11 repo + configure_file(cmake/pybind11.CMakeLists.txt.in pybind11-download/CMakeLists.txt) + execute_process(COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/pybind11-download") + execute_process(COMMAND "${CMAKE_COMMAND}" --build . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/pybind11-download") + add_subdirectory("${CMAKE_BINARY_DIR}/pybind11-src" + "${CMAKE_BINARY_DIR}/pybind11-build") + message(STATUS "TEASER++ Python binding will be built.") add_subdirectory(python) endif () diff --git a/cmake/GoogleTest.CMakeLists.txt.in b/cmake/GoogleTest.CMakeLists.txt.in new file mode 100644 index 0000000..83bdfd0 --- /dev/null +++ b/cmake/GoogleTest.CMakeLists.txt.in @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.10) + +project(googletest-download NONE) + +include(ExternalProject) +ExternalProject_Add(googletest + URL https://github.com/google/googletest/archive/release-1.8.1.zip + SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src" + BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build" + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + TEST_COMMAND "" + ) \ No newline at end of file diff --git a/cmake/pmc.CMakeLists.txt.in b/cmake/pmc.CMakeLists.txt.in new file mode 100644 index 0000000..220eba0 --- /dev/null +++ b/cmake/pmc.CMakeLists.txt.in @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.10) + +project(pmc-download NONE) + +include(ExternalProject) +# Notice that this project uses a forked version of PMC with minor fixes & changes +ExternalProject_Add(pmc + GIT_REPOSITORY https://github.com/jingnanshi/pmc.git + SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/pmc-src" + BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/pmc-build" + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + TEST_COMMAND "" + ) \ No newline at end of file diff --git a/cmake/pybind11.CMakeLists.txt.in b/cmake/pybind11.CMakeLists.txt.in index e62ac51..b270063 100644 --- a/cmake/pybind11.CMakeLists.txt.in +++ b/cmake/pybind11.CMakeLists.txt.in @@ -5,11 +5,11 @@ project(pybind11-download NONE) include(ExternalProject) ExternalProject_Add(pmc GIT_REPOSITORY https://github.com/pybind/pybind11.git - GIT_TAG v2.13.6 + GIT_TAG v2.11.1 SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/pybind11-src" BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/pybind11-build" CONFIGURE_COMMAND "" BUILD_COMMAND "" INSTALL_COMMAND "" TEST_COMMAND "" - ) + ) \ No newline at end of file diff --git a/cmake/spectra.CMakeLists.txt.in b/cmake/spectra.CMakeLists.txt.in new file mode 100644 index 0000000..c4c65f3 --- /dev/null +++ b/cmake/spectra.CMakeLists.txt.in @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.10) + +project(spectra-download NONE) + +include(ExternalProject) +ExternalProject_Add(spectral + GIT_REPOSITORY https://github.com/jingnanshi/spectra + GIT_TAG 5c4fb1de050847988faaaaa50f60e7d3d5f16143 + SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/spectra-src" + BINARY_DIR "" + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + TEST_COMMAND "" + ) \ No newline at end of file diff --git a/cmake/tinyply.CMakeLists.txt.in b/cmake/tinyply.CMakeLists.txt.in new file mode 100644 index 0000000..52ec469 --- /dev/null +++ b/cmake/tinyply.CMakeLists.txt.in @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.10) + +project(tinyply-download NONE) + +include(ExternalProject) +ExternalProject_Add(pmc + GIT_REPOSITORY https://github.com/jingnanshi/tinyply.git + GIT_TAG 0b9fff8e8bd4d37256554fe40cf76b2f3134377b + SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/tinyply-src" + BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/tinyply-build" + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + TEST_COMMAND "" + ) \ No newline at end of file diff --git a/examples/teaser_cpp_fpfh/quatro_cpp_fpfh.cc b/examples/teaser_cpp_fpfh/quatro_cpp_fpfh.cc index 5130107..d802e56 100644 --- a/examples/teaser_cpp_fpfh/quatro_cpp_fpfh.cc +++ b/examples/teaser_cpp_fpfh/quatro_cpp_fpfh.cc @@ -16,8 +16,8 @@ inline double getAngularError(Eigen::Matrix3d R_exp, Eigen::Matrix3d R_est) { return std::abs(std::acos(fmin(fmax(((R_exp.transpose() * R_est).trace() - 1) / 2, -1.0), 1.0))); } -inline void calcErrors(const Eigen::Matrix4d& T, const Eigen::Matrix3d est_rot, - const Eigen::Vector3d est_ts, double& rot_error, double& ts_error) { +inline void calcErrors(const Eigen::Matrix4d& T, const Eigen::Matrix3d est_rot, const Eigen::Vector3d est_ts, + double &rot_error, double& ts_error) { rot_error = getAngularError(T.topLeftCorner(3, 3), est_rot); ts_error = (T.topRightCorner(3, 1) - est_ts).norm(); } @@ -32,11 +32,11 @@ inline void getParams(const double noise_bound, const std::string reg_type, if (reg_type == "Quatro") { params.rotation_estimation_algorithm = teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::QUATRO; - params.inlier_selection_mode = teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_HEU; - } else if (reg_type == "TEASER") { + params.inlier_selection_mode == teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_HEU; + } else if (reg_type == "TEASER") { params.rotation_estimation_algorithm = teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS; - params.inlier_selection_mode = teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_EXACT; + params.inlier_selection_mode == teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_EXACT; } else { throw std::invalid_argument("Not implemented!"); } diff --git a/examples/teaser_cpp_ply/teaser_cpp_ply.cc b/examples/teaser_cpp_ply/teaser_cpp_ply.cc index d4aa804..a32d1c5 100644 --- a/examples/teaser_cpp_ply/teaser_cpp_ply.cc +++ b/examples/teaser_cpp_ply/teaser_cpp_ply.cc @@ -117,4 +117,4 @@ int main() { << std::chrono::duration_cast(end - begin).count() / 1000000.0 << std::endl; -} +} \ No newline at end of file diff --git a/examples/teaser_python_3dsmooth/teaser_python_3dsmooth.py b/examples/teaser_python_3dsmooth/teaser_python_3dsmooth.py index 7390989..406d956 100644 --- a/examples/teaser_python_3dsmooth/teaser_python_3dsmooth.py +++ b/examples/teaser_python_3dsmooth/teaser_python_3dsmooth.py @@ -109,7 +109,7 @@ def visualize_correspondences( frag2_temp.translate(translate) # estimate normals - vis_list = [target, source, frag1_temp, frag2_temp] + vis_list = [target, source, frag1_temp, frag2_temp]; for ii in vis_list: ii.estimate_normals() vis_list.extend([*inlier_line_mesh_geoms, *outlier_line_mesh_geoms]) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 4b3bb85..e85cf24 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -1,22 +1,45 @@ -find_package(pybind11 REQUIRED) +cmake_minimum_required(VERSION 3.10) + +project(teaser_python_bindings) pybind11_add_module(teaserpp_python teaserpp_python/teaserpp_python.cc) + +message(STATUS "Python Interpreter Version: ${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}") +if (NOT (PYTHON_VERSION_MAJOR EQUAL 2 AND PYTHON_VERSION_MINOR EQUAL 7)) + # Hack: VTK used in PCL might add /usr/include/python2.7 to all targets' + # INCLUDE_DIRECTORIES properties. We need to remove it. + get_target_property(TEASERPY_NEW_INCLUDE_DIRS teaserpp_python INTERFACE_INCLUDE_DIRECTORIES) + list(FILTER TEASERPY_NEW_INCLUDE_DIRS EXCLUDE REGEX ".*python2.7$") + set_target_properties(teaserpp_python + PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${TEASERPY_NEW_INCLUDE_DIRS}") +endif () + target_link_libraries(teaserpp_python PUBLIC teaser_registration) # fix for clang # see: https://github.com/pybind/pybind11/issues/1818 if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - target_compile_options(teaserpp_python PUBLIC -fno-sized-deallocation) + target_compile_options(teaserpp_python PUBLIC -fno-sized-deallocation) endif () # make sure to output the build file to teaserpp_python folder SET_TARGET_PROPERTIES(teaserpp_python - PROPERTIES - OUTPUT_NAME "_teaserpp" - PREFIX "" - LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/teaserpp_python" -) - -if (DEFINED SKBUILD) - install(TARGETS teaserpp_python DESTINATION "../teaserpp_python") -endif () + PROPERTIES + PREFIX "" + LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/teaserpp_python" + ) + +# copy package __init__.py file +configure_file(teaserpp_python/__init__.py + ${CMAKE_CURRENT_BINARY_DIR}/teaserpp_python/__init__.py + ) + +# copy setup.py file +configure_file(setup.py.in + ${CMAKE_CURRENT_BINARY_DIR}/setup.py + ) + +file(COPY . + DESTINATION . + FILES_MATCHING + PATTERN *.py) diff --git a/python/setup.py.in b/python/setup.py.in new file mode 100644 index 0000000..882ed4e --- /dev/null +++ b/python/setup.py.in @@ -0,0 +1,12 @@ +from setuptools import setup + +setup( + name='teaserpp_python', + version='1.0.0', + author='Jingnan Shi', + author_email='jnshi@mit.edu', + description='Python binding for TEASER++', + package_dir={'': '${CMAKE_CURRENT_BINARY_DIR}'}, + packages=['teaserpp_python'], + package_data={'': ['*.so']} +) diff --git a/python/teaserpp_python/__init__.py b/python/teaserpp_python/__init__.py index 6f982eb..8300277 100644 --- a/python/teaserpp_python/__init__.py +++ b/python/teaserpp_python/__init__.py @@ -1,58 +1 @@ -from functools import wraps -from typing import Callable, NamedTuple - -from ._teaserpp import ( - OMP_MAX_THREADS, - CertificationResult, - DRSCertifier, - EigSolverType, - InlierGraphFormulation, - InlierSelectionMode, - RegistrationSolution, - RobustRegistrationSolver, - RotationEstimationAlgorithm, -) - -# Backwards compatibility with v1.0 so we don't break code -RobustRegistrationSolver.ROTATION_ESTIMATION_ALGORITHM = RotationEstimationAlgorithm -RobustRegistrationSolver.INLIER_SELECTION_MODE = InlierSelectionMode -RobustRegistrationSolver.INLIER_GRAPH_FORMULATION = InlierGraphFormulation -DRSCertifier.EIG_SOLVER_TYPE = EigSolverType - - -class RobustRegistrationSolverParams(NamedTuple): - noise_bound: float = 0.01 - cbar2: float = 1 - estimate_scaling: bool = True - rotation_estimation_algorithm: RotationEstimationAlgorithm = ( - RotationEstimationAlgorithm.GNC_TLS - ) - rotation_gnc_factor: float = 1.4 - rotation_max_iterations: int = 100 - rotation_cost_threshold: float = 1e-6 - rotation_tim_graph: InlierGraphFormulation = InlierGraphFormulation.CHAIN - inlier_selection_mode: InlierSelectionMode = InlierSelectionMode.PMC_EXACT - kcore_heuristic_threshold: float = 0.5 - use_max_clique: bool = True - max_clique_exact_solution: bool = True - max_clique_time_limit: int = 3000 - max_clique_num_threads: int = OMP_MAX_THREADS - - -# Do some Python magic -def __init_deco(f: Callable[..., None]): - @wraps(f) - def wrapper(self, *args, **kwargs): - f(self, *args, **kwargs) - self._params = args - - return wrapper - - -@property -def __params_getter(self) -> RobustRegistrationSolverParams: - return self._params - - -RobustRegistrationSolver.__init__ = __init_deco(RobustRegistrationSolver.__init__) -setattr(RobustRegistrationSolver, "params", __params_getter) +from .teaserpp_python import * \ No newline at end of file diff --git a/python/teaserpp_python/teaserpp_example.py b/python/teaserpp_python/teaserpp_example.py index f9ba24a..3bae060 100644 --- a/python/teaserpp_python/teaserpp_example.py +++ b/python/teaserpp_python/teaserpp_example.py @@ -21,28 +21,37 @@ dst[:, 1] += 10 dst[:, 9] += 15 - - params = teaserpp_python.RobustRegistrationSolverParams( - cbar2=1, - noise_bound=1, - estimate_scaling=True, - rotation_estimation_algorithm=teaserpp_python.RotationEstimationAlgorithm.GNC_TLS, - rotation_gnc_factor=1.4, - rotation_max_iterations=100, - rotation_cost_threshold=1e-12 - ) - - solver = teaserpp_python.RobustRegistrationSolver(*params) + # Populating the parameters + solver_params = teaserpp_python.RobustRegistrationSolver.Params() + solver_params.cbar2 = 1 + solver_params.noise_bound = 0.01 + solver_params.estimate_scaling = True + solver_params.rotation_estimation_algorithm = teaserpp_python.RobustRegistrationSolver.ROTATION_ESTIMATION_ALGORITHM.GNC_TLS + solver_params.rotation_gnc_factor = 1.4 + solver_params.rotation_max_iterations = 100 + solver_params.rotation_cost_threshold = 1e-12 + print("Parameters are:", solver_params) + + solver = teaserpp_python.RobustRegistrationSolver(solver_params) solver.solve(src, dst) + solution = solver.getSolution() + + # Print the solution + print("Solution is:", solution) + + # Print the inliers + scale_inliers = solver.getScaleInliers() + scale_inliers_map = solver.getScaleInliersMap() + translation_inliers = solver.getTranslationInliers() + translation_inliers_map = solver.getTranslationInliersMap() - print("Solution is:", solver.solution) print("=======================================") print("Scale inliers (TIM pairs) are:") print("Note: they should not include the outlier points.") - for i in range(len(solver.scale_inliers)): - print(solver.scale_inliers[i], end=',') + for i in range(len(scale_inliers)): + print(scale_inliers[i], end=',') print("\n=======================================") - print("Translation inliers are:", solver.translation_inliers) - print("Translation inliers map is:", solver.translation_inliers_map) + print("Translation inliers are:", translation_inliers) + print("Translation inliers map is:", translation_inliers_map) diff --git a/python/teaserpp_python/teaserpp_python.cc b/python/teaserpp_python/teaserpp_python.cc index 13f88ac..c1885b1 100644 --- a/python/teaserpp_python/teaserpp_python.cc +++ b/python/teaserpp_python/teaserpp_python.cc @@ -6,7 +6,6 @@ * See LICENSE for the license information */ -#include #include #include @@ -22,7 +21,7 @@ namespace py = pybind11; /** * Python interface with pybind11 */ -PYBIND11_MODULE(_teaserpp, m) { +PYBIND11_MODULE(teaserpp_python, m) { m.doc() = "Python binding for TEASER++"; // Python bound for teaser::RegistrationSolution @@ -42,140 +41,61 @@ PYBIND11_MODULE(_teaserpp, m) { return print_string.str(); }); - m.attr("OMP_MAX_THREADS") = omp_get_max_threads(); - - // Python bound for teaser::RobustRegistrationSolver::ROTATION_ESTIMATE_ALGORITHM - py::enum_( - m, "RotationEstimationAlgorithm" - ) - .value("GNC_TLS", teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS) - .value("FGR", teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::FGR) - .value("QUATRO", teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::QUATRO); - - // Python bound for teaser::RobustRegistrationSolver::INLIER_GRAPH_FORMULATION - py::enum_( - m, "InlierGraphFormulation" - ) - .value("CHAIN", teaser::RobustRegistrationSolver::INLIER_GRAPH_FORMULATION::CHAIN) - .value("COMPLETE", teaser::RobustRegistrationSolver::INLIER_GRAPH_FORMULATION::COMPLETE); - - // Python bound for teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE - py::enum_( - m, "InlierSelectionMode" - ) - .value("PMC_EXACT", teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_EXACT) - .value("PMC_HEU", teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_HEU) - .value("KCORE_HEU", teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::KCORE_HEU) - .value("NONE", teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::NONE); - - // Python bound for DRSCertifier::EIG_SOLVER_TYPE - py::enum_(m, "EigSolverType") - .value("EIGEN", teaser::DRSCertifier::EIG_SOLVER_TYPE::EIGEN) - .value("SPECTRA", teaser::DRSCertifier::EIG_SOLVER_TYPE::SPECTRA); - // Python bound for teaser::RobustRegistraionSolver - py::class_ solver( - m, "RobustRegistrationSolver", py::dynamic_attr() - ); + py::class_ solver(m, "RobustRegistrationSolver"); // Python bound for teaser::RobustRegistrationSolver functions - solver.def(py::init()) - .def(py::init(), - py::arg("noise_bound") = 0.01, - py::arg("cbar2") = 1, - py::arg("estimate_scaling") = true, - py::arg("rotation_estimation_algorithm") = - teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS, - py::arg("rotation_gnc_factor") = 1.4, - py::arg("rotation_max_iterations") = 100, - py::arg("rotation_cost_threshold") = 1e-6, - py::arg("rotation_tim_graph") = - teaser::RobustRegistrationSolver::INLIER_GRAPH_FORMULATION::CHAIN, - py::arg("inlier_selection_mode") = - teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_EXACT, - py::arg("kcore_heuristic_threshold") = 0.5, - py::arg("use_max_clique") = true, - py::arg("max_clique_exact_solution") = true, - py::arg("max_clique_time_limit") = 3000, - py::arg("max_clique_num_threads") = omp_get_max_threads()) + solver.def(py::init<>()) + .def(py::init()) .def("getParams", &teaser::RobustRegistrationSolver::getParams) + .def("reset", &teaser::RobustRegistrationSolver::reset) .def("solve", py::overload_cast&, const Eigen::Matrix&>( &teaser::RobustRegistrationSolver::solve)) - - .def_property_readonly("solution", &teaser::RobustRegistrationSolver::getSolution) .def("getSolution", &teaser::RobustRegistrationSolver::getSolution) - - .def_property_readonly("gnc_rotation_cost_at_termination", - &teaser::RobustRegistrationSolver::getGNCRotationCostAtTermination) .def("getGNCRotationCostAtTermination", &teaser::RobustRegistrationSolver::getGNCRotationCostAtTermination) - - .def_property_readonly("scale_inliers_mask", &teaser::RobustRegistrationSolver::getScaleInliersMask) .def("getScaleInliersMask", &teaser::RobustRegistrationSolver::getScaleInliersMask) - - .def_property_readonly("scale_inliers_map", &teaser::RobustRegistrationSolver::getScaleInliersMap) .def("getScaleInliersMap", &teaser::RobustRegistrationSolver::getScaleInliersMap) - - .def_property_readonly("scale_inliers", &teaser::RobustRegistrationSolver::getScaleInliers) .def("getScaleInliers", &teaser::RobustRegistrationSolver::getScaleInliers) - - .def_property_readonly("rotation_inliers_mask", &teaser::RobustRegistrationSolver::getRotationInliersMask) .def("getRotationInliersMask", &teaser::RobustRegistrationSolver::getRotationInliersMask) - .def("getRotationInliersMap", &teaser::RobustRegistrationSolver::getRotationInliersMap) - - .def_property_readonly("rotation_inliers", &teaser::RobustRegistrationSolver::getRotationInliers) .def("getRotationInliers", &teaser::RobustRegistrationSolver::getRotationInliers) - - .def_property_readonly("translation_inliers_mask", - &teaser::RobustRegistrationSolver::getTranslationInliersMask) .def("getTranslationInliersMask", &teaser::RobustRegistrationSolver::getTranslationInliersMask) - - - .def_property_readonly("translation_inliers_map", &teaser::RobustRegistrationSolver::getTranslationInliersMap) .def("getTranslationInliersMap", &teaser::RobustRegistrationSolver::getTranslationInliersMap) - - .def_property_readonly("translation_inliers", &teaser::RobustRegistrationSolver::getTranslationInliers) .def("getTranslationInliers", &teaser::RobustRegistrationSolver::getTranslationInliers) - - .def_property_readonly("inlier_max_clique", &teaser::RobustRegistrationSolver::getInlierMaxClique) .def("getInlierMaxClique", &teaser::RobustRegistrationSolver::getInlierMaxClique) - - .def_property_readonly("inlier_graph", &teaser::RobustRegistrationSolver::getInlierGraph) .def("getInlierGraph", &teaser::RobustRegistrationSolver::getInlierGraph) - - .def_property_readonly("src_tims_map", &teaser::RobustRegistrationSolver::getSrcTIMsMap) .def("getSrcTIMsMap", &teaser::RobustRegistrationSolver::getSrcTIMsMap) - - .def_property_readonly("dst_tims_map", &teaser::RobustRegistrationSolver::getDstTIMsMap) .def("getDstTIMsMap", &teaser::RobustRegistrationSolver::getDstTIMsMap) - - .def_property_readonly("src_tims_map_for_rotation", &teaser::RobustRegistrationSolver::getSrcTIMsMapForRotation) .def("getSrcTIMsMapForRotation", &teaser::RobustRegistrationSolver::getSrcTIMsMapForRotation) - - .def_property_readonly("dst_tims_map_for_rotation", &teaser::RobustRegistrationSolver::getSrcTIMsMapForRotation) .def("getDstTIMsMapForRotation", &teaser::RobustRegistrationSolver::getDstTIMsMapForRotation) - - .def_property_readonly("max_clique_src_tims", &teaser::RobustRegistrationSolver::getMaxCliqueSrcTIMs) .def("getMaxCliqueSrcTIMs", &teaser::RobustRegistrationSolver::getMaxCliqueSrcTIMs) - - .def_property_readonly("max_clique_dst_tims", &teaser::RobustRegistrationSolver::getMaxCliqueSrcTIMs) .def("getMaxCliqueDstTIMs", &teaser::RobustRegistrationSolver::getMaxCliqueDstTIMs) - - .def_property_readonly("src_tims", &teaser::RobustRegistrationSolver::getSrcTIMs) .def("getSrcTIMs", &teaser::RobustRegistrationSolver::getSrcTIMs) - - .def_property_readonly("dst_tims", &teaser::RobustRegistrationSolver::getSrcTIMs) .def("getDstTIMs", &teaser::RobustRegistrationSolver::getDstTIMs); + // Python bound for teaser::RobustRegistrationSolver::ROTATION_ESTIMATE_ALGORITHM + py::enum_( + solver, "ROTATION_ESTIMATION_ALGORITHM") + .value("GNC_TLS", teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS) + .value("FGR", teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::FGR); + + // Python bound for teaser::RobustRegistrationSolver::INLIER_GRAPH_FORMULATION + py::enum_(solver, + "INLIER_GRAPH_FORMULATION") + .value("CHAIN", teaser::RobustRegistrationSolver::INLIER_GRAPH_FORMULATION::CHAIN) + .value("COMPLETE", teaser::RobustRegistrationSolver::INLIER_GRAPH_FORMULATION::COMPLETE); + + // Python bound for teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE + py::enum_(solver, + "INLIER_SELECTION_MODE") + .value("PMC_EXACT", teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_EXACT) + .value("PMC_HEU", teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_HEU) + .value("KCORE_HEU", teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::KCORE_HEU) + .value("NONE", teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::NONE); + // Python bound for teaser::RobustRegistrationSolver::Params py::class_(solver, "Params") .def(py::init<>()) @@ -205,19 +125,11 @@ PYBIND11_MODULE(_teaserpp, m) { .def("__repr__", [](const teaser::RobustRegistrationSolver::Params& a) { std::ostringstream print_string; - std::string rot_alg; - if (a.rotation_estimation_algorithm == - teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::FGR) { - rot_alg = "FGR"; - } - if (a.rotation_estimation_algorithm == - teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS) { - rot_alg = "GNC_TLS"; - } - if (a.rotation_estimation_algorithm == - teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::QUATRO) { - rot_alg = "QUATRO"; - } + std::string rot_alg = + a.rotation_estimation_algorithm == + teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::FGR + ? "FGR" + : "GNC-TLS"; std::string inlier_selection_alg; if (a.inlier_selection_mode == @@ -278,6 +190,11 @@ PYBIND11_MODULE(_teaserpp, m) { const Eigen::Matrix&>( &teaser::DRSCertifier::certify)); + // Python bound for DRSCertifier::EIG_SOLVER_TYPE + py::enum_(certifier, "EIG_SOLVER_TYPE") + .value("EIGEN", teaser::DRSCertifier::EIG_SOLVER_TYPE::EIGEN) + .value("SPECTRA", teaser::DRSCertifier::EIG_SOLVER_TYPE::SPECTRA); + // Python bound for DRSCertifier parameter struct py::class_(certifier, "Params") .def(py::init<>()) @@ -288,4 +205,4 @@ PYBIND11_MODULE(_teaserpp, m) { .def_readwrite("gamma_tau", &teaser::DRSCertifier::Params::gamma_tau) .def_readwrite("eig_decomposition_solver", &teaser::DRSCertifier::Params::eig_decomposition_solver); -} +} \ No newline at end of file diff --git a/teaser/CMakeLists.txt b/teaser/CMakeLists.txt index b96e2f3..89e9209 100644 --- a/teaser/CMakeLists.txt +++ b/teaser/CMakeLists.txt @@ -1,74 +1,37 @@ +project(teaser_source) include(GNUInstallDirs) -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -include(FetchContent) - -FetchContent_Declare(pmc - GIT_REPOSITORY https://github.com/jingnanshi/pmc.git -) -if (NOT pmc_POPULATED) - FetchContent_Populate(pmc) - set(PMC_BUILD_SHARED OFF CACHE INTERNAL "") - add_subdirectory(${pmc_SOURCE_DIR} ${pmc_BINARY_DIR} EXCLUDE_FROM_ALL) -endif() - -FetchContent_Declare(tinyply - GIT_REPOSITORY https://github.com/ddiakopoulos/tinyply.git - BUILD_COMMAND "" - INSTALL_COMMAND "" -) -if (NOT tinyply_POPULATED) - FetchContent_Populate(tinyply) -endif() - -FetchContent_Declare(spectra - GIT_REPOSITORY https://github.com/jingnanshi/spectra - GIT_TAG 5c4fb1de050847988faaaaa50f60e7d3d5f16143 - BUILD_COMMAND "" - INSTALL_COMMAND "" -) -if (NOT spectra_POPULATED) - FetchContent_Populate(spectra) -endif() # teaser_io library -add_library(teaser_io src/ply_io.cc) -target_include_directories(teaser_io - PUBLIC +add_library(teaser_io SHARED src/ply_io.cc) +target_link_libraries(teaser_io PRIVATE tinyply) +target_include_directories(teaser_io PUBLIC $ $ - $ - PRIVATE - $ -) -if(NOT BUILD_SHARED_LIBS) - set_property(TARGET teaser_io PROPERTY POSITION_INDEPENDENT_CODE 1) -endif() + $) +install(TARGETS teaser_io + EXPORT teaserpp-export + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) +list(APPEND TEASERPP_EXPORTED_TARGETS teaser_io tinyply) add_library(teaserpp::teaser_io ALIAS teaser_io) # teaser_registration library -add_library(teaser_registration +add_library(teaser_registration SHARED src/certification.cc src/registration.cc src/graph.cc -) - + ) target_link_libraries(teaser_registration PUBLIC Eigen3::Eigen PRIVATE pmc ${TEASERPP_BLAS_LAPACK_LIBS} -) -target_include_directories(teaser_registration - PUBLIC + ) +target_include_directories(teaser_registration PUBLIC $ $ - $ - PRIVATE - $ -) -if(NOT BUILD_SHARED_LIBS) - set_property(TARGET teaser_registration PROPERTY POSITION_INDEPENDENT_CODE 1) -endif() + $ + $) find_package(OpenMP) if(OpenMP_CXX_FOUND) @@ -76,6 +39,12 @@ if(OpenMP_CXX_FOUND) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") endif() +install(TARGETS teaser_registration + EXPORT teaserpp-export + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} +) +list(APPEND TEASERPP_EXPORTED_TARGETS teaser_registration pmc) add_library(teaserpp::teaser_registration ALIAS teaser_registration) # teaser_features library @@ -84,12 +53,15 @@ if (BUILD_TEASER_FPFH) add_library(teaser_features SHARED src/fpfh.cc src/matcher.cc - ) + ) target_link_libraries(teaser_features - PRIVATE ${PCL_LIBRARIES} Eigen3::Eigen - ) + PRIVATE ${PCL_LIBRARIES} + PRIVATE Eigen3::Eigen + ) if (BUILD_WITH_MKL AND MKL_FOUND) - target_link_libraries(teaser_features PRIVATE ${MKL_LIBRARIES}) + target_link_libraries(teaser_features + PRIVATE ${MKL_LIBRARIES} + ) endif () target_include_directories(teaser_features PUBLIC $ @@ -112,21 +84,15 @@ if (BUILD_WITH_MARCH_NATIVE) target_compile_options(teaser_registration PUBLIC -march=native) endif () -# installation -if (NOT DEFINED SKBUILD) - install(TARGETS teaser_io teaser_registration - EXPORT teaserpp-export - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - ) - list(APPEND TEASERPP_EXPORTED_TARGETS teaser_io teaser_registration) - set(TEASERPP_EXPORTED_TARGETS "${TEASERPP_EXPORTED_TARGETS}" PARENT_SCOPE) +# set exported targets in parent scope +set(TEASERPP_EXPORTED_TARGETS "${TEASERPP_EXPORTED_TARGETS}" PARENT_SCOPE) - install(EXPORT teaserpp-export - DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/teaserpp - NAMESPACE teaserpp:: - FILE teaserppTargets.cmake - ) +# installation +install(EXPORT teaserpp-export + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/teaserpp + NAMESPACE teaserpp:: + FILE teaserppTargets.cmake +) - install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) -endif () +install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) +install(DIRECTORY ${SPECTRA_INCLUDE_DIRS} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) diff --git a/teaser/include/teaser/certification.h b/teaser/include/teaser/certification.h index d006fa6..227d1a2 100644 --- a/teaser/include/teaser/certification.h +++ b/teaser/include/teaser/certification.h @@ -31,7 +31,7 @@ class AbstractRotationCertifier { /** * Abstract function for certifying rotation estimation results - * @param rotation_solution [in] a solution to the rotation registration problem + * @param rotation_solution [in] a solution to the rotatoin registration problem * @param src [in] Assume dst = R * src * @param dst [in] Assume dst = R * src * @param theta [in] a binary vector indicating inliers vs. outliers @@ -52,6 +52,7 @@ class AbstractRotationCertifier { */ class DRSCertifier : public AbstractRotationCertifier { public: + /** * Solver for eigendecomposition solver / spectral decomposition. * @@ -59,7 +60,7 @@ class DRSCertifier : public AbstractRotationCertifier { * For extremely large matrices, it may make sense to use Spectra instead. */ enum class EIG_SOLVER_TYPE { - EIGEN = 0, ///< Use solvers in the Eigen library + EIGEN = 0, ///< Use solvers in the Eigen library SPECTRA = 1, ///< Use solvers in the Spectra library }; @@ -185,7 +186,7 @@ class DRSCertifier : public AbstractRotationCertifier { (1) off-diagonal blocks must be skew-symmetric (2) diagonal blocks must satisfy W_00 = - sum(W_ii) (3) W_dual must also satisfy complementary slackness (because M_init satisfies complementary - slackness) This projection is optimal in the sense of minimum Frobenius norm + slackness) This projection is optimal in the sense of minimum Frobenious norm */ void getOptimalDualProjection(const Eigen::Matrix& W, const Eigen::Matrix& theta_prepended, @@ -236,4 +237,4 @@ class DRSCertifier : public AbstractRotationCertifier { Params params_; }; -} // namespace teaser +} // namespace teaser \ No newline at end of file diff --git a/teaser/include/teaser/fpfh.h b/teaser/include/teaser/fpfh.h index 9df00e7..6d7b9eb 100644 --- a/teaser/include/teaser/fpfh.h +++ b/teaser/include/teaser/fpfh.h @@ -11,7 +11,6 @@ #include #include #include -#include #include "teaser/geometry.h" @@ -22,11 +21,10 @@ using FPFHCloudPtr = pcl::PointCloud::Ptr; class FPFHEstimation { public: - FPFHEstimation() { - fpfh_estimation_.reset( - new pcl::FPFHEstimationOMP()); - normals_.reset(new pcl::PointCloud()); - } + FPFHEstimation() + : fpfh_estimation_( + new pcl::FPFHEstimationOMP){}; + /** * Compute FPFH features. * @@ -48,18 +46,10 @@ class FPFHEstimation { return fpfh_estimation_; } - /** - * Return the normal vectors of the input cloud that are used in the calculation of FPFH - * @return - */ - inline pcl::PointCloud getNormals() { return *normals_; } - private: // pcl::FPFHEstimation::Ptr fpfh_estimation_; pcl::FPFHEstimationOMP::Ptr fpfh_estimation_; - pcl::PointCloud::Ptr normals_; - /** * Wrapper function for the corresponding PCL function. * @param output_cloud diff --git a/teaser/include/teaser/geometry.h b/teaser/include/teaser/geometry.h index 5474273..42ed50f 100644 --- a/teaser/include/teaser/geometry.h +++ b/teaser/include/teaser/geometry.h @@ -69,4 +69,5 @@ class PointCloud { std::vector points_; }; + } // namespace teaser diff --git a/teaser/include/teaser/graph.h b/teaser/include/teaser/graph.h index e7ab542..4ac849f 100644 --- a/teaser/include/teaser/graph.h +++ b/teaser/include/teaser/graph.h @@ -35,7 +35,7 @@ class Graph { * vertices v1 & v2, we assume that v2 exists in v1's list, and v1 also exists in v2's list. This * condition is not enforced. If violated, removeEdge() function might exhibit undefined * behaviors. - * @param [in] adj_list a map representing an adjacency list + * @param [in] adj_list an map representing an adjacency list */ explicit Graph(const std::map>& adj_list) { adj_list_.resize(adj_list.size()); @@ -128,26 +128,26 @@ class Graph { * Get the number of vertices * @return total number of vertices */ - int numVertices() const { return adj_list_.size(); } + [[nodiscard]] int numVertices() const { return adj_list_.size(); } /** * Get the number of edges * @return total number of edges */ - int numEdges() const { return num_edges_; } + [[nodiscard]] int numEdges() const { return num_edges_; } /** * Get edges originated from a specific vertex * @param [in] id * @return an unordered set of edges */ - const std::vector& getEdges(int id) const { return adj_list_[id]; } + [[nodiscard]] const std::vector& getEdges(int id) const { return adj_list_[id]; } /** * Get all vertices * @return a vector of all vertices */ - std::vector getVertices() const { + [[nodiscard]] std::vector getVertices() const { std::vector v; for (int i = 0; i < adj_list_.size(); ++i) { v.push_back(i); @@ -155,7 +155,7 @@ class Graph { return v; } - Eigen::MatrixXi getAdjMatrix() const { + [[nodiscard]] Eigen::MatrixXi getAdjMatrix() const { const int num_v = numVertices(); Eigen::MatrixXi adj_matrix(num_v, num_v); for (size_t i = 0; i < num_v; ++i) { @@ -171,7 +171,7 @@ class Graph { return adj_matrix; } - std::vector> getAdjList() const { return adj_list_; } + [[nodiscard]] std::vector> getAdjList() const { return adj_list_; } /** * Preallocate spaces for vertices @@ -258,7 +258,7 @@ class MaxCliqueSolver { /** * Number of threads to use for the solver */ - int num_threads = 1; + int num_threads = 1; }; MaxCliqueSolver() = default; diff --git a/teaser/include/teaser/linalg.h b/teaser/include/teaser/linalg.h index 8ee6963..4ede4c6 100644 --- a/teaser/include/teaser/linalg.h +++ b/teaser/include/teaser/linalg.h @@ -8,11 +8,11 @@ #pragma once -#include - #include -#include #include +#include +#include +#include namespace teaser { @@ -41,9 +41,10 @@ Eigen::Matrix hatmap(const Eigen::Matrix& u) { * @param output [out] output vector */ template -void vectorKron(const Eigen::Matrix& v1, const Eigen::Matrix& v2, +void vectorKron(const Eigen::Matrix& v1, + const Eigen::Matrix& v2, Eigen::Matrix* output) { -#pragma omp parallel for collapse(2) shared(v1, v2, output) +#pragma omp parallel for collapse(2) shared(v1, v2, output) default(none) for (size_t i = 0; i < N; ++i) { for (size_t j = 0; j < M; ++j) { (*output)[i * M + j] = v1[i] * v2[j]; @@ -59,10 +60,10 @@ void vectorKron(const Eigen::Matrix& v1, const Eigen::Matrix -Eigen::Matrix vectorKron(const Eigen::Matrix& v1, - const Eigen::Matrix& v2) { +Eigen::Matrix vectorKron( + const Eigen::Matrix& v1, const Eigen::Matrix& v2) { Eigen::Matrix output(v1.rows() * v2.rows(), 1); -#pragma omp parallel for collapse(2) shared(v1, v2, output) +#pragma omp parallel for collapse(2) shared(v1, v2, output) default(none) for (size_t i = 0; i < v1.rows(); ++i) { for (size_t j = 0; j < v2.rows(); ++j) { output[i * v2.rows() + j] = v1[i] * v2[j]; @@ -78,12 +79,14 @@ Eigen::Matrix vectorKron(const Eigen::Matrix -void getNearestPSD(const Eigen::Matrix& A, - Eigen::Matrix* nearestPSD) { +void getNearestPSD( + const Eigen::Matrix& A, + Eigen::Matrix* nearestPSD) { assert(A.rows() == A.cols()); nearestPSD->resize(A.rows(), A.cols()); @@ -98,4 +101,4 @@ void getNearestPSD(const Eigen::Matrix& A, *nearestPSD = Ve * De_positive * Ve.transpose(); } -} // namespace teaser +} // namespace teaser \ No newline at end of file diff --git a/teaser/include/teaser/macros.h b/teaser/include/teaser/macros.h index 383b6a6..b6be4c9 100644 --- a/teaser/include/teaser/macros.h +++ b/teaser/include/teaser/macros.h @@ -65,5 +65,5 @@ auto t_end_##s = clock_##s.now(); \ std::chrono::duration diff_dur_##s = t_end_##s - t_start_##s; \ double diff_##s = diff_dur_##s.count(); -#define TEASER_DEBUG_GET_TIMING(s) (double)(diff_##s / 1000.0) -#endif +#define TEASER_DEBUG_GET_TIMING(s)(double)(diff_##s / 1000.0) +#endif \ No newline at end of file diff --git a/teaser/include/teaser/matcher.h b/teaser/include/teaser/matcher.h index e5e0fcb..1b89d8f 100644 --- a/teaser/include/teaser/matcher.h +++ b/teaser/include/teaser/matcher.h @@ -36,11 +36,11 @@ class Matcher { * @param use_tuple_test * @return */ - std::vector> calculateCorrespondences( - const teaser::PointCloud& source_points, const teaser::PointCloud& target_points, - const teaser::FPFHCloud& source_features, const teaser::FPFHCloud& target_features, - bool use_absolute_scale = true, bool use_crosscheck = true, bool use_tuple_test = true, - float tuple_scale = 0); + std::vector> + calculateCorrespondences(teaser::PointCloud& source_points, teaser::PointCloud& target_points, + teaser::FPFHCloud& source_features, teaser::FPFHCloud& target_features, + bool use_absolute_scale = true, bool use_crosscheck = true, + bool use_tuple_test = true, float tuple_scale = 0); private: template void buildKDTree(const std::vector& data, KDTree* tree); @@ -56,7 +56,7 @@ class Matcher { std::vector> corres_; std::vector pointcloud_; std::vector features_; - std::vector> means_; // for normalization + std::vector > means_; // for normalization float global_scale_; }; diff --git a/teaser/include/teaser/registration.h b/teaser/include/teaser/registration.h index a778f7f..4e7bc2a 100644 --- a/teaser/include/teaser/registration.h +++ b/teaser/include/teaser/registration.h @@ -8,21 +8,21 @@ #pragma once -#include -#include -#include - #include -#include #include +#include +#include +#include +#include +#include #include "omp.h" - -#include "teaser/graph.h" #include "teaser/geometry.h" +#include "teaser/graph.h" -// TODO: might be a good idea to template Eigen::Vector3f and Eigen::VectorXf such that later on we -// can decide to use double if we want. Double vs float might give nontrivial differences.. +// TODO: might be a good idea to template Eigen::Vector3f and Eigen::VectorXf +// such that later on we can decide to use doulbe if we want. Double vs float +// might give nontrivial differences.. namespace teaser { @@ -39,71 +39,75 @@ struct RegistrationSolution { }; /** - * Abstract virtual class for decoupling specific scale estimation methods with interfaces. + * Abstract virtual class for decoupling specific scale estimation methods with + * interfaces. */ class AbstractScaleSolver { -public: + public: virtual ~AbstractScaleSolver() {} /** - * Pure virtual method for solving scale. Different implementations may have different assumptions - * about input data. + * Pure virtual method for solving scale. Different implementations may have + * different assumptions about input data. * @param src * @param dst * @return estimated scale (s) */ - virtual void solveForScale(const Eigen::Matrix& src, - const Eigen::Matrix& dst, double* scale, - Eigen::Matrix* inliers) = 0; + virtual void solveForScale( + const Eigen::Matrix& src, + const Eigen::Matrix& dst, double* scale, + Eigen::Matrix* inliers) = 0; }; /** - * Abstract virtual class for decoupling specific rotation estimation method implementations with - * interfaces. + * Abstract virtual class for decoupling specific rotation estimation method + * implementations with interfaces. */ class AbstractRotationSolver { -public: + public: virtual ~AbstractRotationSolver() {} /** - * Pure virtual method for solving rotation. Different implementations may have different - * assumptions about input data. + * Pure virtual method for solving rotation. Different implementations may + * have different assumptions about input data. * @param src * @param dst * @return estimated rotation matrix (R) */ - virtual void solveForRotation(const Eigen::Matrix& src, - const Eigen::Matrix& dst, - Eigen::Matrix3d* rotation, - Eigen::Matrix* inliers) = 0; + virtual void solveForRotation( + const Eigen::Matrix& src, + const Eigen::Matrix& dst, + Eigen::Matrix3d* rotation, + Eigen::Matrix* inliers) = 0; }; /** - * Abstract virtual class for decoupling specific translation estimation method implementations with - * interfaces. + * Abstract virtual class for decoupling specific translation estimation method + * implementations with interfaces. */ class AbstractTranslationSolver { -public: + public: virtual ~AbstractTranslationSolver() {} /** - * Pure virtual method for solving translation. Different implementations may have different - * assumptions about input data. + * Pure virtual method for solving translation. Different implementations may + * have different assumptions about input data. * @param src * @param dst * @return estimated translation vector */ - virtual void solveForTranslation(const Eigen::Matrix& src, - const Eigen::Matrix& dst, - Eigen::Vector3d* translation, - Eigen::Matrix* inliers) = 0; + virtual void solveForTranslation( + const Eigen::Matrix& src, + const Eigen::Matrix& dst, + Eigen::Vector3d* translation, + Eigen::Matrix* inliers) = 0; }; /** * Performs scalar truncated least squares estimation */ class ScalarTLSEstimator { -public: + public: ScalarTLSEstimator() = default; /** * Use truncated least squares method to estimate true x given measurements X @@ -114,27 +118,30 @@ class ScalarTLSEstimator { * @param estimate (output) pointer to a double holding the estimate * @param inliers (output) pointer to a Eigen row vector of inliers */ - void estimate(const Eigen::RowVectorXd& X, const Eigen::RowVectorXd& ranges, double* estimate, + void estimate(const Eigen::RowVectorXd& X, const Eigen::RowVectorXd& ranges, + double* estimate, Eigen::Matrix* inliers); /** - * A slightly different implementation of TLS estimate. Use loop tiling to achieve potentially - * faster performance. + * A slightly different implementation of TLS estimate. Use loop tiling to + * achieve potentially faster performance. * @param X Available measurements * @param ranges Maximum admissible errors for measurements X * @param s scale for tiling * @param estimate (output) pointer to a double holding the estimate * @param inliers (output) pointer to a Eigen row vector of inliers */ - void estimate_tiled(const Eigen::RowVectorXd& X, const Eigen::RowVectorXd& ranges, const int& s, - double* estimate, Eigen::Matrix* inliers); + void estimate_tiled(const Eigen::RowVectorXd& X, + const Eigen::RowVectorXd& ranges, const int& s, + double* estimate, + Eigen::Matrix* inliers); }; /** * Perform scale estimation using truncated least-squares (TLS) */ class TLSScaleSolver : public AbstractScaleSolver { -public: + public: TLSScaleSolver() = delete; explicit TLSScaleSolver(double noise_bound, double cbar2) @@ -150,51 +157,55 @@ class TLSScaleSolver : public AbstractScaleSolver { * @return a double indicating the estimated scale */ void solveForScale(const Eigen::Matrix& src, - const Eigen::Matrix& dst, double* scale, + const Eigen::Matrix& dst, + double* scale, Eigen::Matrix* inliers) override; -private: + private: double noise_bound_; - double cbar2_; // maximal allowed residual^2 to noise bound^2 ratio + double cbar2_; // maximal allowed residual^2 to noise bound^2 ratio ScalarTLSEstimator tls_estimator_; }; /** - * Perform outlier pruning / inlier selection based on scale. This class does not perform scale - * estimation. Rather, it estimates outliers based on the assumption that there is no scale - * difference between the two provided vector of points. + * Perform outlier pruning / inlier selection based on scale. This class does + * not perform scale estimation. Rather, it estimates outliers based on the + * assumption that there is no scale difference between the two provided vector + * of points. */ class ScaleInliersSelector : public AbstractScaleSolver { -public: + public: ScaleInliersSelector() = delete; explicit ScaleInliersSelector(double noise_bound, double cbar2) - : noise_bound_(noise_bound), cbar2_(cbar2){}; + : noise_bound_(noise_bound), cbar2_(cbar2) {}; /** * Assume dst = src + noise. The scale output will always be set to 1. * @param src [in] a vector of points * @param dst [in] a vector of points * @param scale [out] a constant of 1 - * @param inliers [out] a row vector of booleans indicating whether a measurement is an inlier + * @param inliers [out] a row vector of booleans indicating whether a + * measurement is an inlier */ void solveForScale(const Eigen::Matrix& src, - const Eigen::Matrix& dst, double* scale, + const Eigen::Matrix& dst, + double* scale, Eigen::Matrix* inliers) override; -private: + private: double noise_bound_; - double cbar2_; // maximal allowed residual^2 to noise bound^2 ratio + double cbar2_; // maximal allowed residual^2 to noise bound^2 ratio }; /** * Perform translation estimation using truncated least-squares (TLS) */ class TLSTranslationSolver : public AbstractTranslationSolver { -public: + public: TLSTranslationSolver() = delete; explicit TLSTranslationSolver(double noise_bound, double cbar2) - : noise_bound_(noise_bound), cbar2_(cbar2){}; + : noise_bound_(noise_bound), cbar2_(cbar2) {}; /** * Estimate translation between src and dst points. Assume dst = src + t. @@ -203,14 +214,15 @@ class TLSTranslationSolver : public AbstractTranslationSolver { * @param translation output parameter for the translation vector * @param inliers output parameter for detected outliers */ - void solveForTranslation(const Eigen::Matrix& src, - const Eigen::Matrix& dst, - Eigen::Vector3d* translation, - Eigen::Matrix* inliers) override; + void solveForTranslation( + const Eigen::Matrix& src, + const Eigen::Matrix& dst, + Eigen::Vector3d* translation, + Eigen::Matrix* inliers) override; -private: + private: double noise_bound_; - double cbar2_; // maximal allowed residual^2 to noise bound^2 ratio + double cbar2_; // maximal allowed residual^2 to noise bound^2 ratio ScalarTLSEstimator tls_estimator_; }; @@ -218,8 +230,7 @@ class TLSTranslationSolver : public AbstractTranslationSolver { * Base class for GNC-based rotation solvers */ class GNCRotationSolver : public AbstractRotationSolver { - -public: + public: struct Params { size_t max_iterations; double cost_threshold; @@ -234,14 +245,15 @@ class GNCRotationSolver : public AbstractRotationSolver { void setParams(Params params) { params_ = params; } /** - * Return the cost of the GNC solver at termination. Details of the cost function is dependent on - * the specific solver implementation. + * Return the cost of the GNC solver at termination. Details of the cost + * function is dependent on the specific solver implementation. * - * @return cost at termination of the GNC solver. Undefined if run before running the solver. + * @return cost at termination of the GNC solver. Undefined if run before + * running the solver. */ double getCostAtTermination() { return cost_; } -protected: + protected: Params params_; double cost_; }; @@ -250,19 +262,19 @@ class GNCRotationSolver : public AbstractRotationSolver { * Use GNC-TLS to solve rotation estimation problems. * * For more information, please refer to: - * H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated Non-Convexity for Robust Spatial - * Perception: From Non-Minimal Solvers to Global Outlier Rejection,” arXiv:1909.08605 [cs, math], - * Sep. 2019. + * H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated Non-Convexity + * for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier + * Rejection,” arXiv:1909.08605 [cs, math], Sep. 2019. */ class GNCTLSRotationSolver : public GNCRotationSolver { -public: + public: GNCTLSRotationSolver() = delete; /** * Parametrized constructor * @param params */ - explicit GNCTLSRotationSolver(Params params) : GNCRotationSolver(params){}; + explicit GNCTLSRotationSolver(Params params) : GNCRotationSolver(params) {}; /** * Estimate rotation between src & dst using GNC-TLS method @@ -271,24 +283,25 @@ class GNCTLSRotationSolver : public GNCRotationSolver { * @param rotation * @param inliers */ - void solveForRotation(const Eigen::Matrix& src, - const Eigen::Matrix& dst, - Eigen::Matrix3d* rotation, - Eigen::Matrix* inliers) override; + void solveForRotation( + const Eigen::Matrix& src, + const Eigen::Matrix& dst, + Eigen::Matrix3d* rotation, + Eigen::Matrix* inliers) override; }; /** * Use Fast Global Registration to solve for pairwise registration problems * * For more information, please see the original paper on FGR: - * Q.-Y. Zhou, J. Park, and V. Koltun, “Fast Global Registration,” in Computer Vision – ECCV 2016, - * Cham, 2016, vol. 9906, pp. 766–782. - * Notice that our implementation differs from the paper on the estimation of T matrix. We - * only estimate rotation, instead of rotation and translation. + * Q.-Y. Zhou, J. Park, and V. Koltun, “Fast Global Registration,” in Computer + * Vision – ECCV 2016, Cham, 2016, vol. 9906, pp. 766–782. Notice that our + * implementation differ from the paper on the estimation of T matrix. We only + * estimate rotation, instead of rotation and translation. * */ class FastGlobalRegistrationSolver : public GNCRotationSolver { -public: + public: /** * Remove default constructor */ @@ -299,7 +312,8 @@ class FastGlobalRegistrationSolver : public GNCRotationSolver { * @param params * @param rotation_only */ - explicit FastGlobalRegistrationSolver(Params params) : GNCRotationSolver(params){}; + explicit FastGlobalRegistrationSolver(Params params) + : GNCRotationSolver(params) {}; /** * Solve a pairwise registration problem given two sets of points. @@ -308,24 +322,26 @@ class FastGlobalRegistrationSolver : public GNCRotationSolver { * @param dst * @return a RegistrationSolution struct. */ - void solveForRotation(const Eigen::Matrix& src, - const Eigen::Matrix& dst, - Eigen::Matrix3d* rotation, - Eigen::Matrix* inliers) override; + void solveForRotation( + const Eigen::Matrix& src, + const Eigen::Matrix& dst, + Eigen::Matrix3d* rotation, + Eigen::Matrix* inliers) override; }; /** * Use Quatro to solve for pairwise registration problems avoiding degeneracy * - * For more information, please see the original paper on Quatro: + * For more information, please see the original paper on FGR: * H. Lim et al., "A Single Correspondence Is Enough: Robust Global Registration * to Avoid Degeneracy in Urban Environments," in Robotics - ICRA 2022, * Accepted. To appear. arXiv:2203.06612 [cs], Mar. 2022. - * Quatro and TEASER++ differ in the estimation of rotation. Quatro forgoes roll and pitch estimation, - * yet it is empirically found that it makes the algorithm more robust against degeneracy. + * Quatro and TEASER++ differ in the estimation of rotation. Quatro forgoes roll + * and pitch estimation, yet it is empirically found that it makes the algorithm + * more robust against degeneracy. */ class QuatroSolver : public GNCRotationSolver { -public: + public: /** * Remove default constructor */ @@ -336,7 +352,7 @@ class QuatroSolver : public GNCRotationSolver { * @param params * @param rotation_only */ - explicit QuatroSolver(Params params) : GNCRotationSolver(params){}; + explicit QuatroSolver(Params params) : GNCRotationSolver(params) {}; /** * Solve a pairwise registration problem given two sets of points. @@ -345,39 +361,41 @@ class QuatroSolver : public GNCRotationSolver { * @param dst * @return a RegistrationSolution struct. */ - void solveForRotation(const Eigen::Matrix& src, - const Eigen::Matrix& dst, - Eigen::Matrix3d* rotation, - Eigen::Matrix* inliers) override; + void solveForRotation( + const Eigen::Matrix& src, + const Eigen::Matrix& dst, + Eigen::Matrix3d* rotation, + Eigen::Matrix* inliers) override; }; /** * Solve registration problems robustly. * * For more information, please refer to: - * H. Yang, J. Shi, and L. Carlone, “TEASER: Fast and Certifiable Point Cloud Registration,” - * arXiv:2001.07715 [cs, math], Jan. 2020. + * H. Yang, J. Shi, and L. Carlone, “TEASER: Fast and Certifiable Point Cloud + * Registration,” arXiv:2001.07715 [cs, math], Jan. 2020. */ class RobustRegistrationSolver { -public: + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /** - * An enum class representing the available GNC rotation estimation algorithms. - * - * GNC_TLS: see H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated Non-Convexity for - * Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection,” - * arXiv:1909.08605 [cs, math], Sep. 2019. + * An enum class representing the available GNC rotation estimation + * algorithms. * - * FGR: see Q.-Y. Zhou, J. Park, and V. Koltun, “Fast Global Registration,” in Computer Vision – - * ECCV 2016, Cham, 2016, vol. 9906, pp. 766–782. and H. Yang, P. Antonante, V. Tzoumas, and L. - * Carlone, “Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to + * GNC_TLS: see H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated + * Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to * Global Outlier Rejection,” arXiv:1909.08605 [cs, math], Sep. 2019. * - * QUATRO: H. Lim et al., "A Single Correspondence Is Enough: Robust Global Registration - * to Avoid Degeneracy in Urban Environments," in Robotics - - * ICRA 2022, pp. 8010-8017 - * arXiv:2203.06612 [cs], Mar. 2022. + * FGR: see Q.-Y. Zhou, J. Park, and V. Koltun, “Fast Global Registration,” in + * Computer Vision – ECCV 2016, Cham, 2016, vol. 9906, pp. 766–782. and H. + * Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated Non-Convexity + * for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier + * Rejection,” arXiv:1909.08605 [cs, math], Sep. 2019. + * + * QUATRO: H. Lim et al., "A Single Correspondence Is Enough: Robust Global + * Registration to Avoid Degeneracy in Urban Environments," in Robotics - ICRA + * 2022, pp. 8010-8017 arXiv:2203.06612 [cs], Mar. 2022. */ enum class ROTATION_ESTIMATION_ALGORITHM { GNC_TLS = 0, @@ -401,10 +419,11 @@ class RobustRegistrationSolver { }; /** - * Enum representing the formulation of the TIM graph after maximum clique filtering. + * Enum representing the formulation of the TIM graph after maximum clique + * filtering. * - * CHAIN: formulate TIMs by only calculating the TIMs for consecutive measurements - * COMPLETE: formulate a fully connected TIM graph + * CHAIN: formulate TIMs by only calculating the TIMs for consecutive + * measurements COMPLETE: formulate a fully connected TIM graph */ enum class INLIER_GRAPH_FORMULATION { CHAIN = 0, @@ -414,25 +433,28 @@ class RobustRegistrationSolver { /** * A struct representing params for initializing the RobustRegistrationSolver * - * Note: the default values needed to be changed accordingly for best performance. + * Note: the default values needed to be changed accordingly for best + * performance. */ struct Params { - /** * A bound on the noise of each provided measurement. */ double noise_bound = 0.01; /** - * Square of the ratio between acceptable noise and noise bound. Usually set to 1. + * Square of the ratio between acceptable noise and noise bound. Usually set + * to 1. */ double cbar2 = 1; /** - * Whether the scale is known. If set to False, the solver assumes no scale differences - * between the src and dst points. If set to True, the solver will first solve for scale. + * Whether the scale is known. If set to False, the solver assumes no scale + * differences between the src and dst points. If set to True, the solver + * will first solve for scale. * - * When the solver does not estimate scale, it solves the registration problem much faster. + * When the solver does not estimate scale, it solves the registration + * problem much faster. */ bool estimate_scaling = true; @@ -445,8 +467,9 @@ class RobustRegistrationSolver { /** * Factor to multiple/divide the control parameter in the GNC algorithm. * - * For FGR: the algorithm divides the control parameter by the factor every iteration. - * For GNC-TLS: the algorithm multiples the control parameter by the factor every iteration. + * For FGR: the algorithm divides the control parameter by the factor every + * iteration. For GNC-TLS: the algorithm multiples the control parameter by + * the factor every iteration. */ double rotation_gnc_factor = 1.4; @@ -458,34 +481,37 @@ class RobustRegistrationSolver { /** * Cost threshold for the GNC rotation estimators. * - * For FGR / GNC-TLS algorithm, the cost thresholds represent different meanings. - * For FGR: the cost threshold compares with the computed cost at each iteration - * For GNC-TLS: the cost threshold compares with the difference between costs of consecutive - * iterations. + * For FGR / GNC-TLS algorithm, the cost thresholds represent different + * meanings. For FGR: the cost threshold compares with the computed cost at + * each iteration For GNC-TLS: the cost threshold compares with the + * difference between costs of consecutive iterations. */ double rotation_cost_threshold = 1e-6; /** * Type of TIM graph given to GNC rotation solver */ - INLIER_GRAPH_FORMULATION rotation_tim_graph = INLIER_GRAPH_FORMULATION::CHAIN; + INLIER_GRAPH_FORMULATION rotation_tim_graph = + INLIER_GRAPH_FORMULATION::CHAIN; /** * \brief Type of the inlier selection */ - INLIER_SELECTION_MODE inlier_selection_mode = INLIER_SELECTION_MODE::PMC_EXACT; + INLIER_SELECTION_MODE inlier_selection_mode = + INLIER_SELECTION_MODE::PMC_EXACT; /** - * \brief The threshold ratio for determining whether to skip max clique and go straightly to - * GNC rotation estimation. Set this to 1 to always use exact max clique selection, 0 to always - * skip exact max clique selection. + * \brief The threshold ratio for determining whether to skip max clique and + * go straightly to GNC rotation estimation. Set this to 1 to always use + * exact max clique selection, 0 to always skip exact max clique selection. * - * \attention Note that the use_max_clique option takes precedence. In other words, if - * use_max_clique is set to false, then kcore_heuristic_threshold will be ignored. If - * use_max_clique is set to true, then the following will happen: if the max core number of the - * inlier graph is lower than the kcore_heuristic_threshold as a percentage of the total nodes - * in the inlier graph, then the code will preceed to call the max clique finder. Otherwise, the - * graph will be directly fed to the GNC rotation solver. + * \attention Note that the use_max_clique option takes precedence. In other + * words, if use_max_clique is set to false, then kcore_heuristic_threshold + * will be ignored. If use_max_clique is set to true, then the following + * will happen: if the max core number of the inlier graph is lower than the + * kcore_heuristic_threshold as a percentage of the total nodes in the + * inlier graph, then the code will preceed to call the max clique finder. + * Otherwise, the graph will be directly fed to the GNC rotation solver. * */ double kcore_heuristic_threshold = 0.5; @@ -515,66 +541,47 @@ class RobustRegistrationSolver { RobustRegistrationSolver() = default; - /** * A constructor that takes in parameters and initialize the estimators - * accordingly. If the parameters need to be reused consider instantiating - * a Params struct. - */ - RobustRegistrationSolver( - double noise_bound, - double cbar2, - bool estimate_scaling, - ROTATION_ESTIMATION_ALGORITHM rotation_estimation_algorithm, - double rotation_gnc_factor, - size_t rotation_max_iterations, - double rotation_cost_threshold, - INLIER_GRAPH_FORMULATION rotation_tim_graph, - INLIER_SELECTION_MODE inlier_selection_mode, - double kcore_heuristic_threshold, - bool use_max_clique, // deprecated - bool max_clique_exact_solution, // deprecated - double max_clique_time_limit, - int max_clique_num_threads = 0 - ); - - /** - * A constructor that takes in parameters and initialize the estimators accordingly. + * accordingly. * - * This is the preferred way of initializing the different estimators, instead of setting - * each estimator one by one. + * This is the preferred way of initializing the different estimators, instead + * of setting each estimator one by one. * @param params */ RobustRegistrationSolver(const Params& params); /** - * Given a 3-by-N matrix representing points, return Translation Invariant Measurements (TIMs) + * Given a 3-by-N matrix representing points, return Translation Invariant + * Measurements (TIMs) * @param v a 3-by-N matrix * @return a 3-by-(N-1)*N matrix representing TIMs */ - Eigen::Matrix - computeTIMs(const Eigen::Matrix& v, - Eigen::Matrix* map); + Eigen::Matrix computeTIMs( + const Eigen::Matrix& v, + Eigen::Matrix* map); /** * Solve for scale, translation and rotation. * * @param src_cloud source point cloud (to be transformed) * @param dst_cloud target point cloud (after transformation) - * @param correspondences A vector of tuples representing the correspondences between pairs of - * points in the two clouds + * @param correspondences A vector of tuples representing the correspondences + * between pairs of points in the two clouds */ - RegistrationSolution solve(const teaser::PointCloud& src_cloud, - const teaser::PointCloud& dst_cloud, - const std::vector> correspondences); + RegistrationSolution solve( + const teaser::PointCloud& src_cloud, const teaser::PointCloud& dst_cloud, + const std::vector> correspondences); /** - * Solve for scale, translation and rotation. Assumes dst is src after transformation. + * Solve for scale, translation and rotation. Assumes dst is src after + * transformation. * @param src * @param dst */ - RegistrationSolution solve(const Eigen::Matrix& src, - const Eigen::Matrix& dst); + RegistrationSolution solve( + const Eigen::Matrix& src, + const Eigen::Matrix& dst); /** * Solve for scale. Assume v2 = s * R * v1, this function estimates s. @@ -589,22 +596,25 @@ class RobustRegistrationSolver { * @param v1 * @param v2 */ - Eigen::Vector3d solveForTranslation(const Eigen::Matrix& v1, - const Eigen::Matrix& v2); + Eigen::Vector3d solveForTranslation( + const Eigen::Matrix& v1, + const Eigen::Matrix& v2); /** * Solve for rotation. Assume v2 = R * v1, this function estimates find R. * @param v1 * @param v2 */ - Eigen::Matrix3d solveForRotation(const Eigen::Matrix& v1, - const Eigen::Matrix& v2); + Eigen::Matrix3d solveForRotation( + const Eigen::Matrix& v1, + const Eigen::Matrix& v2); /** * Return the cost at termination of the GNC rotation solver. Can be used to * assess the quality of the solution. * - * @return cost at termination of the GNC solver. Undefined if run before running the solver. + * @return cost at termination of the GNC solver. Undefined if run before + * running the solver. */ inline double getGNCRotationCostAtTermination() { return rotation_solver_->getCostAtTermination(); @@ -620,18 +630,21 @@ class RobustRegistrationSolver { * Set the scale estimator used * @param estimator */ - inline void setScaleEstimator(std::unique_ptr estimator) { + inline void setScaleEstimator( + std::unique_ptr estimator) { scale_solver_ = std::move(estimator); } /** * Set the rotation estimator used. * - * Note: due to the fact that rotation estimator takes in a noise bound that is dependent on the - * estimated scale, make sure to set the correct params before calling solve. + * Note: due to the fact that rotation estimator takes in a noise bound that + * is dependent on the estimated scale, make sure to set the correct params + * before calling solve. * @param estimator */ - inline void setRotationEstimator(std::unique_ptr estimator) { + inline void setRotationEstimator( + std::unique_ptr estimator) { rotation_solver_ = std::move(estimator); } @@ -639,13 +652,14 @@ class RobustRegistrationSolver { * Set the translation estimator used. * @param estimator */ - inline void setTranslationEstimator(std::unique_ptr estimator) { + inline void setTranslationEstimator( + std::unique_ptr estimator) { translation_solver_ = std::move(estimator); } /** - * Return a boolean Eigen row vector indicating whether specific measurements are inliers - * according to scales. + * Return a boolean Eigen row vector indicating whether specific measurements + * are inliers according to scales. * * @return a 1-by-(number of TIMs) boolean Eigen matrix */ @@ -654,19 +668,23 @@ class RobustRegistrationSolver { } /** - * Return the index map for scale inliers (equivalent to the index map for TIMs). + * Return the index map for scale inliers (equivalent to the index map for + * TIMs). * - * @return a 2-by-(number of TIMs) Eigen matrix. Entries in one column represent the indices of - * the two measurements used to calculate the corresponding TIM. + * @return a 2-by-(number of TIMs) Eigen matrix. Entries in one column + * represent the indices of the two measurements used to calculate the + * corresponding TIM. */ - inline Eigen::Matrix getScaleInliersMap() { return src_tims_map_; } + inline Eigen::Matrix getScaleInliersMap() { + return src_tims_map_; + } /** * Return inlier TIMs from scale estimation * * @return a vector of tuples. Entries in each tuple represent the indices of - * the two measurements used to calculate the corresponding TIM: measurement at indice 0 minus - * measurement at indice 1. + * the two measurements used to calculate the corresponding TIM: measurement + * at indice 0 minus measurement at indice 1. */ inline std::vector> getScaleInliers() { std::vector> result; @@ -679,12 +697,13 @@ class RobustRegistrationSolver { } /** - * Return a boolean Eigen row vector indicating whether specific measurements are inliers - * according to the rotation solver. + * Return a boolean Eigen row vector indicating whether specific measurements + * are inliers according to the rotation solver. * - * @return a 1-by-(size of TIMs used in rotation estimation) boolean Eigen matrix. It is - * equivalent to a binary mask on the TIMs used in rotation estimation, with true representing - * that the measurement is an inlier after rotation estimation. + * @return a 1-by-(size of TIMs used in rotation estimation) boolean Eigen + * matrix. It is equivalent to a binary mask on the TIMs used in rotation + * estimation, with true representing that the measurement is an inlier after + * rotation estimation. */ inline Eigen::Matrix getRotationInliersMask() { return rotation_inliers_mask_; @@ -694,31 +713,33 @@ class RobustRegistrationSolver { * Return the index map for translation inliers (equivalent to max clique). * /TODO: This is obsolete now. Remove or update * - * @return a 1-by-(size of max clique) Eigen matrix. Entries represent the indices of the original - * measurements. + * @return a 1-by-(size of max clique) Eigen matrix. Entries represent the + * indices of the original measurements. */ inline Eigen::Matrix getRotationInliersMap() { - Eigen::Matrix map = Eigen::Map>( - max_clique_.data(), 1, max_clique_.size()); + Eigen::Matrix map = + Eigen::Map>(max_clique_.data(), 1, + max_clique_.size()); return map; } /** * Return inliers from rotation estimation * - * @return a vector of indices of TIMs passed to rotation estimator deemed as inliers by rotation - * estimation. Note that depending on the rotation_tim_graph parameter, number of TIMs passed to - * rotation estimation will be different. + * @return a vector of indices of TIMs passed to rotation estimator deemed as + * inliers by rotation estimation. Note that depending on the + * rotation_tim_graph parameter, number of TIMs passed to rotation estimation + * will be different. */ inline std::vector getRotationInliers() { return rotation_inliers_; } /** - * Return a boolean Eigen row vector indicating whether specific measurements are inliers - * according to translation measurements. + * Return a boolean Eigen row vector indicating whether specific measurements + * are inliers according to translation measurements. * - * @return a 1-by-(size of max clique) boolean Eigen matrix. It is equivalent to a binary mask on - * the inlier max clique, with true representing that the measurement is an inlier after - * translation estimation. + * @return a 1-by-(size of max clique) boolean Eigen matrix. It is equivalent + * to a binary mask on the inlier max clique, with true representing that the + * measurement is an inlier after translation estimation. */ inline Eigen::Matrix getTranslationInliersMask() { return translation_inliers_mask_; @@ -727,85 +748,84 @@ class RobustRegistrationSolver { /** * Return the index map for translation inliers (equivalent to max clique). * - * @return a 1-by-(size of max clique) Eigen matrix. Entries represent the indices of the original - * measurements. + * @return a 1-by-(size of max clique) Eigen matrix. Entries represent the + * indices of the original measurements. */ inline Eigen::Matrix getTranslationInliersMap() { - Eigen::Matrix map = Eigen::Map>( - max_clique_.data(), 1, max_clique_.size()); + Eigen::Matrix map = + Eigen::Map>(max_clique_.data(), 1, + max_clique_.size()); return map; } /** - * Return inliers from translation estimation - * - * @return a vector of indices of measurements deemed as inliers by translation estimation - */ - inline std::vector getTranslationInliers() { return translation_inliers_; } - - /** - * Return input-ordered inliers from translation estimation + * Return inliers from rotation estimation * - * @return a vector of indices of given input correspondences deemed as inliers - * by translation estimation. + * @return a vector of indices of measurements deemed as inliers by rotation + * estimation */ - inline std::vector getInputOrderedTranslationInliers() { - if (params_.rotation_estimation_algorithm == ROTATION_ESTIMATION_ALGORITHM::FGR) { - throw std::runtime_error( - "This function is not supported when using FGR since FGR does not use max clique."); - } - std::vector translation_inliers; - translation_inliers.reserve(translation_inliers_.size()); - for (const auto& i : translation_inliers_) { - translation_inliers.emplace_back(max_clique_[i]); - } - return translation_inliers; + inline std::vector getTranslationInliers() { + return translation_inliers_; } /** - * Return a boolean Eigen row vector indicating whether specific measurements are inliers - * according to translation measurements. + * Return a boolean Eigen row vector indicating whether specific measurements + * are inliers according to translation measurements. * @return */ inline std::vector getInlierMaxClique() { return max_clique_; } - inline std::vector> getInlierGraph() { return inlier_graph_.getAdjList(); } + inline std::vector> getInlierGraph() { + return inlier_graph_.getAdjList(); + } /** * Get TIMs built from source point cloud. * @return */ - inline Eigen::Matrix getSrcTIMs() { return src_tims_; } + inline Eigen::Matrix getSrcTIMs() { + return src_tims_; + } /** * Get TIMs built from target point cloud. * @return */ - inline Eigen::Matrix getDstTIMs() { return dst_tims_; } + inline Eigen::Matrix getDstTIMs() { + return dst_tims_; + } /** * Get src TIMs built after max clique pruning. * @return */ - inline Eigen::Matrix getMaxCliqueSrcTIMs() { return pruned_src_tims_; } + inline Eigen::Matrix getMaxCliqueSrcTIMs() { + return pruned_src_tims_; + } /** * Get dst TIMs built after max clique pruning. * @return */ - inline Eigen::Matrix getMaxCliqueDstTIMs() { return pruned_dst_tims_; } + inline Eigen::Matrix getMaxCliqueDstTIMs() { + return pruned_dst_tims_; + } /** * Get the index map of the TIMs built from source point cloud. * @return */ - inline Eigen::Matrix getSrcTIMsMap() { return src_tims_map_; } + inline Eigen::Matrix getSrcTIMsMap() { + return src_tims_map_; + } /** * Get the index map of the TIMs built from target point cloud. * @return */ - inline Eigen::Matrix getDstTIMsMap() { return dst_tims_map_; } + inline Eigen::Matrix getDstTIMsMap() { + return dst_tims_map_; + } /** * Get the index map of the TIMs used in rotation estimation. @@ -827,55 +847,44 @@ class RobustRegistrationSolver { * Reset the solver using the provided params * @param params a Params struct */ - void reset( - const double noise_bound, - const double cbar2, - const bool estimate_scaling, - const ROTATION_ESTIMATION_ALGORITHM rotation_estimation_algorithm, - const double rotation_gnc_factor, - const size_t rotation_max_iterations, - const double rotation_cost_threshold, - const INLIER_GRAPH_FORMULATION rotation_tim_graph, - const INLIER_SELECTION_MODE inlier_selection_mode, - const double kcore_heuristic_threshold, - const bool use_max_clique , // deprecated - const bool max_clique_exact_solution, // deprecated - const double max_clique_time_limit, - const int max_clique_num_threads - ) { + void reset(const Params& params) { + params_ = params; + // Initialize the scale estimator - if (estimate_scaling) { - setScaleEstimator( - std::make_unique(noise_bound, cbar2)); + if (params_.estimate_scaling) { + setScaleEstimator(std::make_unique( + params_.noise_bound, params_.cbar2)); } else { - setScaleEstimator( - std::make_unique(noise_bound, cbar2)); + setScaleEstimator(std::make_unique( + params_.noise_bound, params_.cbar2)); } // Initialize the rotation estimator - teaser::GNCRotationSolver::Params rotation_params { - rotation_max_iterations, rotation_cost_threshold, - rotation_gnc_factor, noise_bound - }; - - switch (rotation_estimation_algorithm) { - case ROTATION_ESTIMATION_ALGORITHM::GNC_TLS: { // GNC-TLS method - setRotationEstimator(std::make_unique(rotation_params)); + teaser::GNCRotationSolver::Params rotation_params{ + params_.rotation_max_iterations, params_.rotation_cost_threshold, + params_.rotation_gnc_factor, params_.noise_bound}; + switch (params_.rotation_estimation_algorithm) { + case ROTATION_ESTIMATION_ALGORITHM::GNC_TLS: { // GNC-TLS method + setRotationEstimator( + std::make_unique(rotation_params)); break; } - case ROTATION_ESTIMATION_ALGORITHM::FGR: { // FGR method - setRotationEstimator(std::make_unique(rotation_params)); + case ROTATION_ESTIMATION_ALGORITHM::FGR: { // FGR method + setRotationEstimator( + std::make_unique( + rotation_params)); break; } - case ROTATION_ESTIMATION_ALGORITHM::QUATRO: { // Quatro method - setRotationEstimator(std::make_unique(rotation_params)); + case ROTATION_ESTIMATION_ALGORITHM::QUATRO: { // Quatro method + setRotationEstimator( + std::make_unique(rotation_params)); break; } } // Initialize the translation estimator - setTranslationEstimator( - std::make_unique(noise_bound, cbar2)); + setTranslationEstimator(std::make_unique( + params_.noise_bound, params_.cbar2)); // Clear member variables max_clique_.clear(); @@ -884,36 +893,13 @@ class RobustRegistrationSolver { inlier_graph_.clear(); } - /** - * Reset the solver using the provided params - * @param params a Params struct - */ - void reset(const Params& params) { - reset( - params.noise_bound, - params.cbar2, - params.estimate_scaling, - params.rotation_estimation_algorithm, - params.rotation_gnc_factor, - params.rotation_max_iterations, - params.rotation_cost_threshold, - params.rotation_tim_graph, - params.inlier_selection_mode, - params.kcore_heuristic_threshold, - params.use_max_clique, - params.max_clique_exact_solution, - params.max_clique_time_limit, - params.max_clique_num_threads - ); - } - /** * Return the params * @return a Params struct */ Params getParams() { return params_; } -private: + private: Params params_; RegistrationSolution solution_; @@ -956,4 +942,4 @@ class RobustRegistrationSolver { std::unique_ptr translation_solver_; }; -} // namespace teaser +} // namespace teaser diff --git a/teaser/include/teaser/utils.h b/teaser/include/teaser/utils.h index 9419275..3063b9c 100644 --- a/teaser/include/teaser/utils.h +++ b/teaser/include/teaser/utils.h @@ -22,7 +22,7 @@ namespace utils { * randsample() * @tparam T A number type * @tparam URBG A UniformRandomBitGenerator type - * @param input An input vector containing the whole population + * @param input A input vector containing the whole population * @param num_samples Number of samples we want * @param g * @return @@ -56,7 +56,7 @@ std::vector randomSample(std::vector input, size_t num_samples, URBG&& g) } /** - * Remove one row from a matrix. + * Remove one row from matrix. * Credit to: https://stackoverflow.com/questions/13290395 * @param matrix an Eigen::Matrix. * @param rowToRemove index of row to remove. If >= matrix.rows(), no operation will be taken @@ -78,7 +78,7 @@ void removeRow(Eigen::Matrix& matrix, unsigned int rowToRemove) { } /** - * Remove one column from a matrix. + * Remove one column from matrix. * Credit to: https://stackoverflow.com/questions/13290395 * @param matrix * @param colToRemove index of col to remove. If >= matrix.cols(), no operation will be taken @@ -112,7 +112,7 @@ template float calculateDiameter(const Eigen::Matrix& } /** - * Use a boolean Eigen matrix to mask a vector + * Use an boolean Eigen matrix to mask a vector * @param mask a 1-by-N boolean Eigen matrix * @param elements vector to be masked * @return diff --git a/teaser/src/certification.cc b/teaser/src/certification.cc index cb4cc58..3e068ee 100644 --- a/teaser/src/certification.cc +++ b/teaser/src/certification.cc @@ -6,24 +6,26 @@ * See LICENSE for the license information */ -#include -#include -#include -#include +#include "teaser/certification.h" -#include -#include #include #include +#include +#include + +#include +#include +#include +#include +#include -#include "teaser/certification.h" #include "teaser/linalg.h" -teaser::CertificationResult -teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution, - const Eigen::Matrix& src, - const Eigen::Matrix& dst, - const Eigen::Matrix& theta) { +teaser::CertificationResult teaser::DRSCertifier::certify( + const Eigen::Matrix3d& R_solution, + const Eigen::Matrix& src, + const Eigen::Matrix& dst, + const Eigen::Matrix& theta) { // convert theta to a double Eigen matrix Eigen::Matrix theta_double(1, theta.cols()); for (size_t i = 0; i < theta.cols(); ++i) { @@ -36,11 +38,11 @@ teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution, return certify(R_solution, src, dst, theta_double); } -teaser::CertificationResult -teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution, - const Eigen::Matrix& src, - const Eigen::Matrix& dst, - const Eigen::Matrix& theta) { +teaser::CertificationResult teaser::DRSCertifier::certify( + const Eigen::Matrix3d& R_solution, + const Eigen::Matrix& src, + const Eigen::Matrix& dst, + const Eigen::Matrix& theta) { int N = src.cols(); int Npm = 4 + 4 * N; @@ -56,7 +58,8 @@ teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution, getLinearProjection(theta_prepended, &inverse_map); TEASER_DEBUG_STOP_TIMING(LProj); TEASER_DEBUG_INFO_MSG("Obtained linear inverse map."); - TEASER_DEBUG_INFO_MSG("Linear projection time: " << TEASER_DEBUG_GET_TIMING(LProj)); + TEASER_DEBUG_INFO_MSG( + "Linear projection time: " << TEASER_DEBUG_GET_TIMING(LProj)); // recall data matrix from QUASAR Eigen::MatrixXd Q_cost(Npm, Npm); @@ -67,12 +70,14 @@ teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution, Eigen::Quaterniond q_solution(R_solution); q_solution.normalize(); Eigen::VectorXd q_solution_vec(4, 1); - q_solution_vec << q_solution.x(), q_solution.y(), q_solution.z(), q_solution.w(); + q_solution_vec << q_solution.x(), q_solution.y(), q_solution.z(), + q_solution.w(); // this would have been the rank-1 decomposition of Z if Z were the globally // optimal solution of the QUASAR SDP Eigen::VectorXd x = - teaser::vectorKron(theta_prepended, q_solution_vec); + teaser::vectorKron( + theta_prepended, q_solution_vec); // build the "rotation matrix" D_omega Eigen::MatrixXd D_omega; @@ -88,7 +93,8 @@ teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution, } // verify optimality in the "rotated" space using projection - // this is the cost of the primal, when strong duality holds, mu is also the cost of the dual + // this is the cost of the primal, when strong duality holds, mu is also the + // cost of the dual double mu = x.transpose().dot(Q_cost * x); // get initial guess @@ -96,15 +102,19 @@ teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution, getLambdaGuess(R_solution, theta, src, dst, &lambda_bar_init); // this initial guess lives in the affine subspace - // use 2 separate steps to limit slow evaluation on only the few non-zeros in the sparse matrix + // use 2 separate steps to limit slow evaluation on only the few non-zeros in + // the sparse matrix #if EIGEN_VERSION_AT_LEAST(3, 3, 0) Eigen::SparseMatrix M_init = Q_bar - mu * J_bar - lambda_bar_init; #else - // fix for this bug in Eigen 3.2: https://eigen.tuxfamily.org/bz/show_bug.cgi?id=632 - Eigen::SparseMatrix M_init = Q_bar.sparseView() - mu * J_bar - lambda_bar_init; + // fix for this bug in Eigen 3.2: + // https://eigen.tuxfamily.org/bz/show_bug.cgi?id=632 + Eigen::SparseMatrix M_init = + Q_bar.sparseView() - mu * J_bar - lambda_bar_init; #endif - // flag to indicate whether we exceeded iterations or reach the desired sub-optim gap + // flag to indicate whether we exceeded iterations or reach the desired + // sub-optim gap bool exceeded_maxiters = true; // vector to store suboptim trajectory @@ -138,7 +148,8 @@ teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution, #if EIGEN_VERSION_AT_LEAST(3, 3, 0) temp_W = 2 * M_PSD - M - M_init; #else - // fix for this bug in Eigen 3.2: https://eigen.tuxfamily.org/bz/show_bug.cgi?id=632 + // fix for this bug in Eigen 3.2: + // https://eigen.tuxfamily.org/bz/show_bug.cgi?id=632 temp_W = 2 * M_PSD - M - M_init.toDense(); #endif @@ -146,11 +157,13 @@ teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution, TEASER_DEBUG_START_TIMING(DualProjection); getOptimalDualProjection(temp_W, theta_prepended, inverse_map, &W_dual); TEASER_DEBUG_STOP_TIMING(DualProjection); - TEASER_DEBUG_INFO_MSG("Dual Projection time: " << TEASER_DEBUG_GET_TIMING(DualProjection)); + TEASER_DEBUG_INFO_MSG( + "Dual Projection time: " << TEASER_DEBUG_GET_TIMING(DualProjection)); #if EIGEN_VERSION_AT_LEAST(3, 3, 0) M_affine = M_init + W_dual; #else - // fix for this bug in Eigen 3.2: https://eigen.tuxfamily.org/bz/show_bug.cgi?id=632 + // fix for this bug in Eigen 3.2: + // https://eigen.tuxfamily.org/bz/show_bug.cgi?id=632 M_affine = M_init.toDense() + W_dual; #endif @@ -159,7 +172,8 @@ teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution, TEASER_DEBUG_START_TIMING(Gap); current_suboptim = computeSubOptimalityGap(M_affine, mu, N); TEASER_DEBUG_STOP_TIMING(Gap); - TEASER_DEBUG_INFO_MSG("Sub Optimality Gap time: " << TEASER_DEBUG_GET_TIMING(Gap)); + TEASER_DEBUG_INFO_MSG( + "Sub Optimality Gap time: " << TEASER_DEBUG_GET_TIMING(Gap)); TEASER_DEBUG_INFO_MSG("Current sub-optimality gap: " << current_suboptim); // termination check and update trajectory @@ -171,8 +185,8 @@ teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution, } if (current_suboptim < params_.sub_optimality) { - TEASER_DEBUG_INFO_MSG("Suboptimality condition reached in " << iter + 1 - << " iterations. Stopping DRS."); + TEASER_DEBUG_INFO_MSG("Suboptimality condition reached in " + << iter + 1 << " iterations. Stopping DRS."); exceeded_maxiters = false; break; } @@ -189,7 +203,8 @@ teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution, return cert_result; } -double teaser::DRSCertifier::computeSubOptimalityGap(const Eigen::MatrixXd& M, double mu, int N) { +double teaser::DRSCertifier::computeSubOptimalityGap(const Eigen::MatrixXd& M, + double mu, int N) { Eigen::MatrixXd new_M = (M + M.transpose()) / 2; bool successful = false; @@ -206,8 +221,9 @@ double teaser::DRSCertifier::computeSubOptimalityGap(const Eigen::MatrixXd& M, d } else { // Spectra Spectra::DenseSymMatProd op(new_M); - Spectra::SymEigsSolver> eigs( - &op, 1, 30); + Spectra::SymEigsSolver> + eigs(&op, 1, 30); eigs.init(); int nconv = eigs.compute(); if (eigs.info() == Spectra::SUCCESSFUL) { @@ -219,7 +235,8 @@ double teaser::DRSCertifier::computeSubOptimalityGap(const Eigen::MatrixXd& M, d if (!successful) { TEASER_DEBUG_ERROR_MSG( - "Failed to find the minimal eigenvalue for suboptimality gap calculaiton."); + "Failed to find the minimal eigenvalue for suboptimality gap " + "calculaiton."); return std::numeric_limits::infinity(); } @@ -230,9 +247,10 @@ double teaser::DRSCertifier::computeSubOptimalityGap(const Eigen::MatrixXd& M, d return (-min_eig * (N + 1)) / mu; } -void teaser::DRSCertifier::getQCost(const Eigen::Matrix& v1, - const Eigen::Matrix& v2, - Eigen::Matrix* Q) { +void teaser::DRSCertifier::getQCost( + const Eigen::Matrix& v1, + const Eigen::Matrix& v2, + Eigen::Matrix* Q) { int N = v1.cols(); int Npm = 4 + 4 * N; double noise_bound_scaled = params_.cbar2 * std::pow(params_.noise_bound, 2); @@ -270,11 +288,12 @@ void teaser::DRSCertifier::getQCost(const Eigen::Matrix(temp_B.data()); // ck = 0.5 * ( v1(:,k)'*v1(:,k)+v2(:,k)'*v2(:,k) - barc2 ); - double ck = 0.5 * (v1.col(k).squaredNorm() + v2.col(k).squaredNorm() - noise_bound_scaled); - Q1.block<4, 4>(0, start_idx) = - Q1.block<4, 4>(0, start_idx) - 0.5 * P_k + ck / 2 * Eigen::Matrix4d::Identity(); - Q1.block<4, 4>(start_idx, 0) = - Q1.block<4, 4>(start_idx, 0) - 0.5 * P_k + ck / 2 * Eigen::Matrix4d::Identity(); + double ck = 0.5 * (v1.col(k).squaredNorm() + v2.col(k).squaredNorm() - + noise_bound_scaled); + Q1.block<4, 4>(0, start_idx) = Q1.block<4, 4>(0, start_idx) - 0.5 * P_k + + ck / 2 * Eigen::Matrix4d::Identity(); + Q1.block<4, 4>(start_idx, 0) = Q1.block<4, 4>(start_idx, 0) - 0.5 * P_k + + ck / 2 * Eigen::Matrix4d::Identity(); } // Q2 matrix @@ -290,9 +309,11 @@ void teaser::DRSCertifier::getQCost(const Eigen::Matrix(temp_B.data()); // ck = 0.5 * ( v1(:,k)'*v1(:,k)+v2(:,k)'*v2(:,k) + barc2 ); - double ck = 0.5 * (v1.col(k).squaredNorm() + v2.col(k).squaredNorm() + noise_bound_scaled); + double ck = 0.5 * (v1.col(k).squaredNorm() + v2.col(k).squaredNorm() + + noise_bound_scaled); Q2.block<4, 4>(start_idx, start_idx) = - Q2.block<4, 4>(start_idx, start_idx) - P_k + ck * Eigen::Matrix4d::Identity(); + Q2.block<4, 4>(start_idx, start_idx) - P_k + + ck * Eigen::Matrix4d::Identity(); } *Q = Q1 + Q2; @@ -322,8 +343,8 @@ void teaser::DRSCertifier::getBlockDiagOmega( void teaser::DRSCertifier::getOptimalDualProjection( const Eigen::Matrix& W, - const Eigen::Matrix& theta_prepended, const SparseMatrix& A_inv, - Eigen::MatrixXd* W_dual) { + const Eigen::Matrix& theta_prepended, + const SparseMatrix& A_inv, Eigen::MatrixXd* W_dual) { // prepare some variables int Npm = W.rows(); int N = Npm / 4 - 1; @@ -358,14 +379,18 @@ void teaser::DRSCertifier::getOptimalDualProjection( temp_B << -1, theta_ij; // W([row_idx(4) col_idx(4)],row_idx(1:3)) - Eigen::Matrix temp_C = W.block<1, 3>(row_idx_end, row_idx_start); - Eigen::Matrix temp_D = W.block<1, 3>(col_idx_end, row_idx_start); + Eigen::Matrix temp_C = + W.block<1, 3>(row_idx_end, row_idx_start); + Eigen::Matrix temp_D = + W.block<1, 3>(col_idx_end, row_idx_start); Eigen::Matrix temp_CD; temp_CD << temp_C, temp_D; // W([row_idx(4) col_idx(4)], col_idx(1:3)) - Eigen::Matrix temp_E = W.block<1, 3>(row_idx_end, col_idx_start); - Eigen::Matrix temp_F = W.block<1, 3>(col_idx_end, col_idx_start); + Eigen::Matrix temp_E = + W.block<1, 3>(row_idx_end, col_idx_start); + Eigen::Matrix temp_F = + W.block<1, 3>(col_idx_end, col_idx_start); Eigen::Matrix temp_EF; temp_EF << temp_E, temp_F; @@ -427,35 +452,39 @@ void teaser::DRSCertifier::getOptimalDualProjection( Eigen::Matrix3d W_diag_sum_33 = Eigen::Matrix3d::Zero(); for (size_t i = 0; i < N + 1; ++i) { int idx_start = i * 4; - // Eigen::Vector4d W_dual_row_sum_last_column= W_dual->middleRows<4>(idx_start).rowwise().sum(); + // Eigen::Vector4d W_dual_row_sum_last_column= + // W_dual->middleRows<4>(idx_start).rowwise().sum(); Eigen::Vector4d W_dual_row_sum_last_column; // sum 4 rows - getBlockRowSum(*W_dual, idx_start, theta_prepended, &W_dual_row_sum_last_column); + getBlockRowSum(*W_dual, idx_start, theta_prepended, + &W_dual_row_sum_last_column); W_ii = W.block<4, 4>(idx_start, idx_start); // modify W_ii's last column/row to satisfy complementary slackness W_ii.block<4, 1>(0, 3) = -theta_prepended(i) * W_dual_row_sum_last_column; - W_ii.block<1, 4>(3, 0) = -theta_prepended(i) * W_dual_row_sum_last_column.transpose(); + W_ii.block<1, 4>(3, 0) = + -theta_prepended(i) * W_dual_row_sum_last_column.transpose(); (*W_dual).block<4, 4>(idx_start, idx_start) = W_ii; W_diag_sum_33 += W_ii.topLeftCorner<3, 3>(); } W_diag_mean.topLeftCorner<3, 3>() = W_diag_sum_33 / (N + 1); // update diagonal blocks - Eigen::Matrix temp_A((N + 1) * W_diag_mean.rows(), - (N + 1) * W_diag_mean.cols()); + Eigen::Matrix temp_A( + (N + 1) * W_diag_mean.rows(), (N + 1) * W_diag_mean.cols()); temp_A.setZero(); for (int i = 0; i < N + 1; i++) { - temp_A.block(i * W_diag_mean.rows(), i * W_diag_mean.cols(), W_diag_mean.rows(), - W_diag_mean.cols()) = W_diag_mean; + temp_A.block(i * W_diag_mean.rows(), i * W_diag_mean.cols(), + W_diag_mean.rows(), W_diag_mean.cols()) = W_diag_mean; } *W_dual -= temp_A; } -void teaser::DRSCertifier::getLambdaGuess(const Eigen::Matrix& R, - const Eigen::Matrix& theta, - const Eigen::Matrix& src, - const Eigen::Matrix& dst, - SparseMatrix* lambda_guess) { +void teaser::DRSCertifier::getLambdaGuess( + const Eigen::Matrix& R, + const Eigen::Matrix& theta, + const Eigen::Matrix& src, + const Eigen::Matrix& dst, + SparseMatrix* lambda_guess) { int K = theta.cols(); int Npm = 4 * K + 4; @@ -477,40 +506,49 @@ void teaser::DRSCertifier::getLambdaGuess(const Eigen::Matrix& R, Eigen::Matrix src_i_hatmap = teaser::hatmap(src.col(i)); if (theta(0, i) > 0) { // residual - Eigen::Matrix xi = R.transpose() * (dst.col(i) - R * src.col(i)); + Eigen::Matrix xi = + R.transpose() * (dst.col(i) - R * src.col(i)); Eigen::Matrix xi_hatmap = teaser::hatmap(xi); - // compute the (4,4) entry of the current block, obtained from KKT complementary slackness - current_block(3, 3) = -0.75 * xi.squaredNorm() - 0.25 * noise_bound_scaled; + // compute the (4,4) entry of the current block, obtained from KKT + // complementary slackness + current_block(3, 3) = + -0.75 * xi.squaredNorm() - 0.25 * noise_bound_scaled; // compute the top-left 3-by-3 block current_block.topLeftCorner<3, 3>() = - src_i_hatmap * src_i_hatmap - 0.5 * (src.col(i)).dot(xi) * Eigen::Matrix3d::Identity() + + src_i_hatmap * src_i_hatmap - + 0.5 * (src.col(i)).dot(xi) * Eigen::Matrix3d::Identity() + 0.5 * xi_hatmap * src_i_hatmap + 0.5 * xi * src.col(i).transpose() - 0.75 * xi.squaredNorm() * Eigen::Matrix3d::Identity() - 0.25 * noise_bound_scaled * Eigen::Matrix3d::Identity(); // compute the vector part current_block.topRightCorner<3, 1>() = -1.5 * xi_hatmap * src.col(i); - current_block.bottomLeftCorner<1, 3>() = (current_block.topRightCorner<3, 1>()).transpose(); + current_block.bottomLeftCorner<1, 3>() = + (current_block.topRightCorner<3, 1>()).transpose(); } else { // residual - Eigen::Matrix phi = R.transpose() * (dst.col(i) - R * src.col(i)); + Eigen::Matrix phi = + R.transpose() * (dst.col(i) - R * src.col(i)); Eigen::Matrix phi_hatmap = teaser::hatmap(phi); // compute lambda_i, (4,4) entry - current_block(3, 3) = -0.25 * phi.squaredNorm() - 0.75 * noise_bound_scaled; + current_block(3, 3) = + -0.25 * phi.squaredNorm() - 0.75 * noise_bound_scaled; // compute E_ii, top-left 3-by-3 block current_block.topLeftCorner<3, 3>() = - src_i_hatmap * src_i_hatmap - 0.5 * (src.col(i)).dot(phi) * Eigen::Matrix3d::Identity() + + src_i_hatmap * src_i_hatmap - + 0.5 * (src.col(i)).dot(phi) * Eigen::Matrix3d::Identity() + 0.5 * phi_hatmap * src_i_hatmap + 0.5 * phi * src.col(i).transpose() - 0.25 * phi.squaredNorm() * Eigen::Matrix3d::Identity() - 0.25 * noise_bound_scaled * Eigen::Matrix3d::Identity(); // compute x_i current_block.topRightCorner<3, 1>() = -0.5 * phi_hatmap * src.col(i); - current_block.bottomLeftCorner<1, 3>() = (current_block.topRightCorner<3, 1>()).transpose(); + current_block.bottomLeftCorner<1, 3>() = + (current_block.topRightCorner<3, 1>()).transpose(); } // put the current block to the sparse triplets @@ -519,7 +557,8 @@ void teaser::DRSCertifier::getLambdaGuess(const Eigen::Matrix& R, // assume current block is column major for (size_t col = 0; col < 4; ++col) { for (size_t row = 0; row < 4; ++row) { - lambda_guess->insert((i + 1) * 4 + row, (i + 1) * 4 + col) = -current_block(row, col); + lambda_guess->insert((i + 1) * 4 + row, (i + 1) * 4 + col) = + -current_block(row, col); } } @@ -536,7 +575,8 @@ void teaser::DRSCertifier::getLambdaGuess(const Eigen::Matrix& R, } void teaser::DRSCertifier::getLinearProjection( - const Eigen::Matrix& theta_prepended, SparseMatrix* A_inv) { + const Eigen::Matrix& theta_prepended, + SparseMatrix* A_inv) { // number of off-diagonal entries in the inverse map int N0 = theta_prepended.cols() - 1; @@ -548,7 +588,8 @@ void teaser::DRSCertifier::getLinearProjection( // build the mapping from independent var idx to matrix index int nr_vals = N * (N - 1) / 2; - Eigen::Matrix mat2vec = Eigen::MatrixXd::Zero(N, N); + Eigen::Matrix mat2vec = + Eigen::MatrixXd::Zero(N, N); int count = 0; for (size_t i = 0; i < N - 1; ++i) { for (size_t j = i + 1; j < N; ++j) { @@ -574,17 +615,18 @@ void teaser::DRSCertifier::getLinearProjection( TEASER_INFO_MSG_THROTTLE("Linear proj at i=" << i << "\n", i, 10); for (size_t j = i + 1; j < N; ++j) { // get current column index - // var_j_idx is unique for all each loop, i.e., each var_j_idx only occurs once and the loops - // won't enter the same column twice + // var_j_idx is unique for all each loop, i.e., each var_j_idx only occurs + // once and the loops won't enter the same column twice int var_j_idx = mat2vec(i, j); // start a inner vector - // A_inv is column major so this will enable us to insert values to the end of column - // var_j_idx + // A_inv is column major so this will enable us to insert values to the + // end of column var_j_idx A_inv->startVec(var_j_idx); - // flag to indicated whether a diagonal entry on this column has been inserted - // this is used to save computation time for adding x to all the diagonal entries + // flag to indicated whether a diagonal entry on this column has been + // inserted this is used to save computation time for adding x to all the + // diagonal entries bool diag_inserted = false; size_t diag_idx = 0; @@ -637,15 +679,16 @@ void teaser::DRSCertifier::getLinearProjection( } // sort by row index (ascending) - std::sort(temp_column.begin(), temp_column.end(), - [](const Eigen::Triplet& t1, const Eigen::Triplet& t2) { - return t1.row() < t2.row(); - }); + std::sort( + temp_column.begin(), temp_column.end(), + [](const Eigen::Triplet& t1, + const Eigen::Triplet& t2) { return t1.row() < t2.row(); }); // populate A_inv with the temporary column for (size_t tidx = 0; tidx < temp_column.size(); ++tidx) { // take care of the diagonal entries - A_inv->insertBack(temp_column[tidx].row(), var_j_idx) = temp_column[tidx].value(); + A_inv->insertBack(temp_column[tidx].row(), var_j_idx) = + temp_column[tidx].value(); } temp_column.clear(); temp_column.reserve(nrNZ_per_row_off_diag); @@ -656,13 +699,15 @@ void teaser::DRSCertifier::getLinearProjection( TEASER_DEBUG_INFO_MSG("A_inv finalized."); } -void teaser::DRSCertifier::getBlockRowSum(const Eigen::MatrixXd& A, const int& row, - const Eigen::Matrix& theta, - Eigen::Vector4d* output) { +void teaser::DRSCertifier::getBlockRowSum( + const Eigen::MatrixXd& A, const int& row, + const Eigen::Matrix& theta, + Eigen::Vector4d* output) { // unit = sparse(4,1); unit(end) = 1; // vector = kron(theta,unit); % vector of size 4N+4 by 1 // entireRow = A(blkIndices(row,4),:); % entireRow of size 4 by 4N+4 - // row_sum_last_column = entireRow * vector; % last column sum has size 4 by 1; + // row_sum_last_column = entireRow * vector; % last column sum has size 4 by + // 1; Eigen::Matrix unit = Eigen::Matrix::Zero(); unit(3, 0) = 1; Eigen::Matrix vector = diff --git a/teaser/src/fpfh.cc b/teaser/src/fpfh.cc index fa046ed..27d6d10 100644 --- a/teaser/src/fpfh.cc +++ b/teaser/src/fpfh.cc @@ -16,6 +16,7 @@ teaser::FPFHCloudPtr teaser::FPFHEstimation::computeFPFHFeatures( const teaser::PointCloud& input_cloud, double normal_search_radius, double fpfh_search_radius) { // Intermediate variables + pcl::PointCloud::Ptr normals(new pcl::PointCloud); teaser::FPFHCloudPtr descriptors(new pcl::PointCloud()); pcl::PointCloud::Ptr pcl_input_cloud(new pcl::PointCloud); for (auto& i : input_cloud) { @@ -24,17 +25,16 @@ teaser::FPFHCloudPtr teaser::FPFHEstimation::computeFPFHFeatures( } // Estimate normals - normals_->clear(); - pcl::NormalEstimationOMP normalEstimation; + pcl::NormalEstimation normalEstimation; normalEstimation.setInputCloud(pcl_input_cloud); normalEstimation.setRadiusSearch(normal_search_radius); pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree); normalEstimation.setSearchMethod(kdtree); - normalEstimation.compute(*normals_); + normalEstimation.compute(*normals); // Estimate FPFH setInputCloud(pcl_input_cloud); - setInputNormals(normals_); + setInputNormals(normals); setSearchMethod(kdtree); setRadiusSearch(fpfh_search_radius); compute(*descriptors); @@ -58,5 +58,4 @@ void teaser::FPFHEstimation::setSearchMethod( void teaser::FPFHEstimation::compute(pcl::PointCloud& output_cloud) { fpfh_estimation_->compute(output_cloud); } - void teaser::FPFHEstimation::setRadiusSearch(double r) { fpfh_estimation_->setRadiusSearch(r); } diff --git a/teaser/src/graph.cc b/teaser/src/graph.cc index 5ed4872..59ddc92 100644 --- a/teaser/src/graph.cc +++ b/teaser/src/graph.cc @@ -7,10 +7,12 @@ */ #include "teaser/graph.h" + +#include + #include "pmc/pmc.h" std::vector teaser::MaxCliqueSolver::findMaxClique(teaser::Graph graph) { - // Handle deprecated field if (!params_.solve_exactly) { params_.solver_mode = CLIQUE_SOLVER_MODE::PMC_HEU; @@ -85,7 +87,7 @@ std::vector teaser::MaxCliqueSolver::findMaxClique(teaser::Graph graph) { } // lower-bound of max clique - if (in.lb == 0 && in.heu_strat != "0") { // skip if given as input + if (in.lb == 0 && in.heu_strat != "0") { // skip if given as input pmc::pmc_heu maxclique(G, in); in.lb = maxclique.search(G, C); } @@ -108,9 +110,9 @@ std::vector teaser::MaxCliqueSolver::findMaxClique(teaser::Graph graph) { // 2. neigh-core pruning/ordering // 3. dynamic coloring bounds/sort // see the original PMC paper and implementation for details: - // R. A. Rossi, D. F. Gleich, and A. H. Gebremedhin, “Parallel Maximum Clique Algorithms with - // Applications to Network Analysis,” SIAM J. Sci. Comput., vol. 37, no. 5, pp. C589–C616, Jan. - // 2015. + // R. A. Rossi, D. F. Gleich, and A. H. Gebremedhin, “Parallel Maximum + // Clique Algorithms with Applications to Network Analysis,” SIAM J. Sci. + // Comput., vol. 37, no. 5, pp. C589–C616, Jan. 2015. if (G.num_vertices() < in.adj_limit) { G.create_adj(); pmc::pmcx_maxclique finder(G, in); diff --git a/teaser/src/matcher.cc b/teaser/src/matcher.cc index 0156697..b87d1df 100644 --- a/teaser/src/matcher.cc +++ b/teaser/src/matcher.cc @@ -6,6 +6,7 @@ * See LICENSE for the license information */ + #include #include #include @@ -19,9 +20,9 @@ namespace teaser { std::vector> Matcher::calculateCorrespondences( - const teaser::PointCloud& source_points, const teaser::PointCloud& target_points, - const teaser::FPFHCloud& source_features, const teaser::FPFHCloud& target_features, - bool use_absolute_scale, bool use_crosscheck, bool use_tuple_test, float tuple_scale) { + teaser::PointCloud& source_points, teaser::PointCloud& target_points, + teaser::FPFHCloud& source_features, teaser::FPFHCloud& target_features, bool use_absolute_scale, + bool use_crosscheck, bool use_tuple_test, float tuple_scale) { Feature cloud_features; pointcloud_.push_back(source_points); @@ -137,8 +138,9 @@ void Matcher::advancedMatching(bool use_crosscheck, bool use_tuple_test, float t KDTree feature_tree_j(flann::KDTreeSingleIndexParams(15)); buildKDTree(features_[fj], &feature_tree_j); - std::vector corres_K; + std::vector corres_K, corres_K2; std::vector dis; + std::vector ind; std::vector> corres; std::vector> corres_cross; diff --git a/teaser/src/ply_io.cc b/teaser/src/ply_io.cc index 21ed73e..8bbf406 100644 --- a/teaser/src/ply_io.cc +++ b/teaser/src/ply_io.cc @@ -13,8 +13,6 @@ #include #include "teaser/ply_io.h" - -#define TINYPLY_IMPLEMENTATION #include "tinyply.h" // Internal datatypes for storing ply vertices diff --git a/teaser/src/registration.cc b/teaser/src/registration.cc index 01b6174..4f78d54 100644 --- a/teaser/src/registration.cc +++ b/teaser/src/registration.cc @@ -8,27 +8,30 @@ #include "teaser/registration.h" +#include #include #include #include -#include #include +#include -#include "teaser/utils.h" #include "teaser/graph.h" #include "teaser/macros.h" +#include "teaser/utils.h" -void teaser::ScalarTLSEstimator::estimate(const Eigen::RowVectorXd& X, - const Eigen::RowVectorXd& ranges, double* estimate, - Eigen::Matrix* inliers) { +void teaser::ScalarTLSEstimator::estimate( + const Eigen::RowVectorXd& X, const Eigen::RowVectorXd& ranges, + double* estimate, Eigen::Matrix* inliers) { // check input parameters - bool dimension_inconsistent = (X.rows() != ranges.rows()) || (X.cols() != ranges.cols()); + bool dimension_inconsistent = + (X.rows() != ranges.rows()) || (X.cols() != ranges.cols()); if (inliers) { - dimension_inconsistent |= ((inliers->rows() != 1) || (inliers->cols() != ranges.cols())); + dimension_inconsistent |= + ((inliers->rows() != 1) || (inliers->cols() != ranges.cols())); } bool only_one_element = (X.rows() == 1) && (X.cols() == 1); assert(!dimension_inconsistent); - assert(!only_one_element); // TODO: admit a trivial solution + assert(!only_one_element); // TODO: admit a trivial solution int N = X.cols(); std::vector> h; @@ -39,7 +42,9 @@ void teaser::ScalarTLSEstimator::estimate(const Eigen::RowVectorXd& X, // ascending order std::sort(h.begin(), h.end(), - [](std::pair a, std::pair b) { return a.first < b.first; }); + [](std::pair a, std::pair b) { + return a.first < b.first; + }); // calculate weights Eigen::RowVectorXd weights = ranges.array().square(); @@ -56,8 +61,7 @@ void teaser::ScalarTLSEstimator::estimate(const Eigen::RowVectorXd& X, double sum_xi_square = 0; for (size_t i = 0; i < nr_centers; ++i) { - - int idx = int(std::abs(h.at(i).second)) - 1; // Indices starting at 1 + int idx = int(std::abs(h.at(i).second)) - 1; // Indices starting at 1 int epsilon = (h.at(i).second > 0) ? 1 : -1; consensus_set_cardinal += epsilon; @@ -69,8 +73,8 @@ void teaser::ScalarTLSEstimator::estimate(const Eigen::RowVectorXd& X, x_hat(i) = dot_X_weights / dot_weights_consensus; - double residual = - consensus_set_cardinal * x_hat(i) * x_hat(i) + sum_xi_square - 2 * sum_xi * x_hat(i); + double residual = consensus_set_cardinal * x_hat(i) * x_hat(i) + + sum_xi_square - 2 * sum_xi * x_hat(i); x_cost(i) = residual + ranges_inverse_sum; } @@ -87,27 +91,30 @@ void teaser::ScalarTLSEstimator::estimate(const Eigen::RowVectorXd& X, } } -void teaser::ScalarTLSEstimator::estimate_tiled(const Eigen::RowVectorXd& X, - const Eigen::RowVectorXd& ranges, const int& s, - double* estimate, - Eigen::Matrix* inliers) { +void teaser::ScalarTLSEstimator::estimate_tiled( + const Eigen::RowVectorXd& X, const Eigen::RowVectorXd& ranges, const int& s, + double* estimate, Eigen::Matrix* inliers) { // check input parameters - bool dimension_inconsistent = (X.rows() != ranges.rows()) || (X.cols() != ranges.cols()); + bool dimension_inconsistent = + (X.rows() != ranges.rows()) || (X.cols() != ranges.cols()); if (inliers) { - dimension_inconsistent |= ((inliers->rows() != 1) || (inliers->cols() != ranges.cols())); + dimension_inconsistent |= + ((inliers->rows() != 1) || (inliers->cols() != ranges.cols())); } bool only_one_element = (X.rows() == 1) && (X.cols() == 1); assert(!dimension_inconsistent); - assert(!only_one_element); // TODO: admit a trivial solution + assert(!only_one_element); // TODO: admit a trivial solution // Prepare variables for calculations int N = X.cols(); Eigen::RowVectorXd h(N * 2); h << X - ranges, X + ranges; // ascending order - std::sort(h.data(), h.data() + h.cols(), [](double a, double b) { return a < b; }); + std::sort(h.data(), h.data() + h.cols(), + [](double a, double b) { return a < b; }); // calculate interval centers - Eigen::RowVectorXd h_centers = (h.head(h.cols() - 1) + h.tail(h.cols() - 1)) / 2; + Eigen::RowVectorXd h_centers = + (h.head(h.cols() - 1) + h.tail(h.cols() - 1)) / 2; auto nr_centers = h_centers.cols(); // calculate weights @@ -124,9 +131,11 @@ void teaser::ScalarTLSEstimator::estimate_tiled(const Eigen::RowVectorXd& X, std::vector ranges_inverse_sum_vec(nr_centers, 0); std::vector dot_X_weights_vec(nr_centers, 0); std::vector dot_weights_consensus_vec(nr_centers, 0); - std::vector> X_consensus_table(nr_centers, std::vector()); + std::vector> X_consensus_table(nr_centers, + std::vector()); - auto inner_loop_f = [&](const size_t& i, const size_t& jh, const size_t& jl_lower_bound, + auto inner_loop_f = [&](const size_t& i, const size_t& jh, + const size_t& jl_lower_bound, const size_t& jl_upper_bound) { double& ranges_inverse_sum = ranges_inverse_sum_vec[i]; double& dot_X_weights = dot_X_weights_vec[i]; @@ -147,11 +156,13 @@ void teaser::ScalarTLSEstimator::estimate_tiled(const Eigen::RowVectorXd& X, } if (j == N - 1) { - // x_hat(i) = dot(X(consensus), weights(consensus)) / dot(weights, consensus); + // x_hat(i) = dot(X(consensus), weights(consensus)) / dot(weights, + // consensus); x_hat(i) = dot_X_weights / dot_weights_consensus; // residual = X(consensus)-x_hat(i); - Eigen::Map X_consensus(X_consensus_vec.data(), X_consensus_vec.size()); + Eigen::Map X_consensus(X_consensus_vec.data(), + X_consensus_vec.size()); Eigen::VectorXd residual = X_consensus.array() - x_hat(i); // x_cost(i) = dot(residual,residual) + sum(ranges(~consensus)); @@ -159,9 +170,10 @@ void teaser::ScalarTLSEstimator::estimate_tiled(const Eigen::RowVectorXd& X, } }; -#pragma omp parallel for shared( \ - jh_bound, ih_bound, ranges_inverse_sum_vec, dot_X_weights_vec, dot_weights_consensus_vec, \ - X_consensus_table, h_centers, weights, N, X, x_hat, x_cost, s, ranges, inner_loop_f) +#pragma omp parallel for default(none) \ + shared(jh_bound, ih_bound, ranges_inverse_sum_vec, dot_X_weights_vec, \ + dot_weights_consensus_vec, X_consensus_table, h_centers, \ + weights, N, X, x_hat, x_cost, s, ranges, inner_loop_f) for (size_t ih = 0; ih < ih_bound; ih += s) { for (size_t jh = 0; jh < jh_bound; jh += s) { for (size_t il = 0; il < s; ++il) { @@ -173,19 +185,19 @@ void teaser::ScalarTLSEstimator::estimate_tiled(const Eigen::RowVectorXd& X, // finish the left over entries // 1. Finish the unfinished js -#pragma omp parallel for \ - shared(jh_bound, ih_bound, ranges_inverse_sum_vec, dot_X_weights_vec, \ - dot_weights_consensus_vec, X_consensus_table, h_centers, weights, N, X, x_hat, x_cost, \ - s, ranges, nr_centers, inner_loop_f) +#pragma omp parallel for default(none) shared( \ + jh_bound, ih_bound, ranges_inverse_sum_vec, dot_X_weights_vec, \ + dot_weights_consensus_vec, X_consensus_table, h_centers, weights, \ + N, X, x_hat, x_cost, s, ranges, nr_centers, inner_loop_f) for (size_t i = 0; i < nr_centers; ++i) { inner_loop_f(i, 0, jh_bound, N); } // 2. Finish the unfinished is -#pragma omp parallel for \ - shared(jh_bound, ih_bound, ranges_inverse_sum_vec, dot_X_weights_vec, \ - dot_weights_consensus_vec, X_consensus_table, h_centers, weights, N, X, x_hat, x_cost, \ - s, ranges, nr_centers, inner_loop_f) +#pragma omp parallel for default(none) shared( \ + jh_bound, ih_bound, ranges_inverse_sum_vec, dot_X_weights_vec, \ + dot_weights_consensus_vec, X_consensus_table, h_centers, weights, \ + N, X, x_hat, x_cost, s, ranges, nr_centers, inner_loop_f) for (size_t i = ih_bound; i < nr_centers; ++i) { inner_loop_f(i, 0, 0, N); } @@ -205,12 +217,13 @@ void teaser::ScalarTLSEstimator::estimate_tiled(const Eigen::RowVectorXd& X, void teaser::FastGlobalRegistrationSolver::solveForRotation( const Eigen::Matrix& src, - const Eigen::Matrix& dst, Eigen::Matrix3d* rotation, + const Eigen::Matrix& dst, + Eigen::Matrix3d* rotation, Eigen::Matrix* inliers) { - assert(rotation); // make sure R is not a nullptr - assert(src.cols() == dst.cols()); // check dimensions of input data - assert(params_.gnc_factor > 1); // make sure mu will decrease - assert(params_.noise_bound != 0); // make sure noise bound is not zero + assert(rotation); // make sure R is not a nullptr + assert(src.cols() == dst.cols()); // check dimensions of input data + assert(params_.gnc_factor > 1); // make sure mu will decrease + assert(params_.noise_bound != 0); // make sure noise bound is not zero if (inliers) { assert(inliers->cols() == src.cols()); } @@ -223,13 +236,14 @@ void teaser::FastGlobalRegistrationSolver::solveForRotation( // Calculate the initial mu double src_diameter = teaser::utils::calculateDiameter(src); double dest_diameter = teaser::utils::calculateDiameter(dst); - double global_scale = src_diameter > dest_diameter ? src_diameter : dest_diameter; + double global_scale = + src_diameter > dest_diameter ? src_diameter : dest_diameter; global_scale /= noise_bound_sq; double mu = std::pow(global_scale, 2) / noise_bound_sq; // stopping condition for mu double min_mu = 1.0; - *rotation = Eigen::Matrix3d::Identity(3, 3); // rotation matrix + *rotation = Eigen::Matrix3d::Identity(3, 3); // rotation matrix Eigen::Matrix l_pq(1, match_size); l_pq.setOnes(1, match_size); @@ -254,7 +268,8 @@ void teaser::FastGlobalRegistrationSolver::solveForRotation( *rotation = teaser::utils::svdRot(src, dst, l_pq); // update cost - Eigen::Matrix diff = (dst - (*rotation) * src).array().square(); + Eigen::Matrix diff = + (dst - (*rotation) * src).array().square(); cost_ = ((scaled_mu * diff.colwise().sum()).array() / (scaled_mu + diff.colwise().sum().array()).array()) .sum(); @@ -279,12 +294,13 @@ void teaser::FastGlobalRegistrationSolver::solveForRotation( void teaser::QuatroSolver::solveForRotation( const Eigen::Matrix& src, - const Eigen::Matrix& dst, Eigen::Matrix3d* rotation, + const Eigen::Matrix& dst, + Eigen::Matrix3d* rotation, Eigen::Matrix* inliers) { - assert(rotation); // make sure R is not a nullptr - assert(src.cols() == dst.cols()); // check dimensions of input data - assert(params_.gnc_factor > 1); // make sure mu will increase - assert(params_.noise_bound != 0); // make sure noise sigma is not zero + assert(rotation); // make sure R is not a nullptr + assert(src.cols() == dst.cols()); // check dimensions of input data + assert(params_.gnc_factor > 1); // make sure mu will increase + assert(params_.noise_bound != 0); // make sure noise sigma is not zero if (inliers) { assert(inliers->cols() == src.cols()); } @@ -310,7 +326,8 @@ void teaser::QuatroSolver::solveForRotation( /** * Loop: terminate when: - * 1. the change in cost in two consecutive runs is smaller than a user-defined threshold + * 1. the change in cost in two consecutive runs is smaller than a + * user-defined threshold * 2. # iterations exceeds the maximum allowed * * Within each loop: @@ -319,9 +336,9 @@ void teaser::QuatroSolver::solveForRotation( */ // Prepare some variables - size_t match_size = src.cols(); // number of correspondences + size_t match_size = src.cols(); // number of correspondences - double mu = 1; // arbitrary starting mu + double mu = 1; // arbitrary starting mu double prev_cost = std::numeric_limits::infinity(); cost_ = std::numeric_limits::infinity(); @@ -331,8 +348,10 @@ void teaser::QuatroSolver::solveForRotation( if (noise_bound_sq < 1e-16) { noise_bound_sq = 1e-2; } - TEASER_DEBUG_INFO_MSG("GNC rotation estimation noise bound:" << rot_noise_bound); - TEASER_DEBUG_INFO_MSG("GNC rotation estimation noise bound squared:" << noise_bound_sq); + TEASER_DEBUG_INFO_MSG( + "GNC rotation estimation noise bound:" << rot_noise_bound); + TEASER_DEBUG_INFO_MSG( + "GNC rotation estimation noise bound squared:" << noise_bound_sq); Eigen::Matrix diffs(2, match_size); Eigen::Matrix weights(1, match_size); @@ -341,7 +360,6 @@ void teaser::QuatroSolver::solveForRotation( // Loop for performing GNC-TLS for (size_t i = 0; i < params_.max_iterations; ++i) { - // Fix weights and perform SVD 2d rotation estimation rotation_2d = teaser::utils::svdRot2d(src_2d, dst_2d, weights); @@ -356,7 +374,8 @@ void teaser::QuatroSolver::solveForRotation( // i.e., little to none noise if (mu <= 0) { TEASER_DEBUG_INFO_MSG( - "GNC-TLS terminated because maximum residual at initialization is very small."); + "GNC-TLS terminated because maximum residual at initialization is " + "very small."); break; } } @@ -375,7 +394,8 @@ void teaser::QuatroSolver::solveForRotation( } else if (residuals_sq(j) <= th2) { weights(j) = 1; } else { - weights(j) = sqrt(noise_bound_sq * mu * (mu + 1) / residuals_sq(j)) - mu; + weights(j) = + sqrt(noise_bound_sq * mu * (mu + 1) / residuals_sq(j)) - mu; assert(weights(j) >= 0 && weights(j) <= 1); } } @@ -388,7 +408,8 @@ void teaser::QuatroSolver::solveForRotation( prev_cost = cost_; if (cost_diff < params_.cost_threshold) { - TEASER_DEBUG_INFO_MSG("GNC-TLS solver terminated due to cost convergence."); + TEASER_DEBUG_INFO_MSG( + "GNC-TLS solver terminated due to cost convergence."); TEASER_DEBUG_INFO_MSG("Cost diff: " << cost_diff); TEASER_DEBUG_INFO_MSG("Iterations: " << i); break; @@ -407,19 +428,20 @@ void teaser::QuatroSolver::solveForRotation( (*rotation).block<2, 2>(0, 0) = rotation_2d; } -void teaser::TLSScaleSolver::solveForScale(const Eigen::Matrix& src, - const Eigen::Matrix& dst, - double* scale, - Eigen::Matrix* inliers) { - +void teaser::TLSScaleSolver::solveForScale( + const Eigen::Matrix& src, + const Eigen::Matrix& dst, double* scale, + Eigen::Matrix* inliers) { Eigen::Matrix v1_dist = src.array().square().colwise().sum().array().sqrt(); Eigen::Matrix v2_dist = dst.array().square().colwise().sum().array().sqrt(); - Eigen::Matrix raw_scales = v2_dist.array() / v1_dist.array(); + Eigen::Matrix raw_scales = + v2_dist.array() / v1_dist.array(); double beta = 2 * noise_bound_ * sqrt(cbar2_); - Eigen::Matrix alphas = beta * v1_dist.cwiseInverse(); + Eigen::Matrix alphas = + beta * v1_dist.cwiseInverse(); tls_estimator_.estimate(raw_scales, alphas, scale, inliers); } @@ -444,7 +466,8 @@ void teaser::ScaleInliersSelector::solveForScale( void teaser::TLSTranslationSolver::solveForTranslation( const Eigen::Matrix& src, - const Eigen::Matrix& dst, Eigen::Vector3d* translation, + const Eigen::Matrix& dst, + Eigen::Vector3d* translation, Eigen::Matrix* inliers) { assert(src.cols() == dst.cols()); if (inliers) { @@ -457,86 +480,54 @@ void teaser::TLSTranslationSolver::solveForTranslation( // Error bounds for each measurements int N = src.cols(); double beta = noise_bound_ * sqrt(cbar2_); - Eigen::Matrix alphas = beta * Eigen::MatrixXd::Ones(1, N); + Eigen::Matrix alphas = + beta * Eigen::MatrixXd::Ones(1, N); // Estimate x, y, and z component of translation: perform TLS on each row *inliers = Eigen::Matrix::Ones(1, N); Eigen::Matrix inliers_temp(1, N); for (size_t i = 0; i < raw_translation.rows(); ++i) { - tls_estimator_.estimate(raw_translation.row(i), alphas, &((*translation)(i)), &inliers_temp); + tls_estimator_.estimate(raw_translation.row(i), alphas, + &((*translation)(i)), &inliers_temp); // element-wise AND using component-wise product (Eigen 3.2 compatible) // a point is an inlier iff. x,y,z are all inliers *inliers = (*inliers).cwiseProduct(inliers_temp); } } -teaser::RobustRegistrationSolver::RobustRegistrationSolver( - double noise_bound, - double cbar2, - bool estimate_scaling, - ROTATION_ESTIMATION_ALGORITHM rotation_estimation_algorithm, - double rotation_gnc_factor, - size_t rotation_max_iterations, - double rotation_cost_threshold, - INLIER_GRAPH_FORMULATION rotation_tim_graph, - INLIER_SELECTION_MODE inlier_selection_mode, - double kcore_heuristic_threshold, - bool use_max_clique, // deprecated - bool max_clique_exact_solution, // deprecated - double max_clique_time_limit, - int max_clique_num_threads -) { - reset( - noise_bound, - cbar2, - estimate_scaling, - rotation_estimation_algorithm, - rotation_gnc_factor, - rotation_max_iterations, - rotation_cost_threshold, - rotation_tim_graph, - inlier_selection_mode, - kcore_heuristic_threshold, - use_max_clique, - max_clique_exact_solution, - max_clique_time_limit, - max_clique_num_threads ? max_clique_num_threads : omp_get_max_threads() - ); -} - teaser::RobustRegistrationSolver::RobustRegistrationSolver( const teaser::RobustRegistrationSolver::Params& params) { reset(params); } Eigen::Matrix -teaser::RobustRegistrationSolver::computeTIMs(const Eigen::Matrix& v, - Eigen::Matrix* map) { - +teaser::RobustRegistrationSolver::computeTIMs( + const Eigen::Matrix& v, + Eigen::Matrix* map) { auto N = v.cols(); Eigen::Matrix vtilde(3, N * (N - 1) / 2); map->resize(2, N * (N - 1) / 2); -#pragma omp parallel for shared(N, v, vtilde, map) +#pragma omp parallel for default(none) shared(N, v, vtilde, map) for (size_t i = 0; i < N - 1; i++) { // Calculate some important indices - // For each measurement, we compute the TIMs between itself and all the measurements after it. - // For example: - // i=0: add N-1 TIMs - // i=1: add N-2 TIMs + // For each measurement, we compute the TIMs between itself and all the + // measurements after it. For example: i=0: add N-1 TIMs i=1: add N-2 TIMs // etc.. // i=k: add N-1-k TIMs - // And by arithmatic series, we can get the starting index of each segment be: - // k*N - k*(k+1)/2 + // And by arithmatic series, we can get the starting index of each segment + // be: k*N - k*(k+1)/2 size_t segment_start_idx = i * N - i * (i + 1) / 2; size_t segment_cols = N - 1 - i; // calculate TIM Eigen::Matrix m = v.col(i); - Eigen::Matrix temp = v - m * Eigen::MatrixXd::Ones(1, N); + Eigen::Matrix temp = + v - m * Eigen::MatrixXd::Ones(1, N); // concatenate to the end of the tilde vector - vtilde.middleCols(segment_start_idx, segment_cols) = temp.rightCols(segment_cols); + vtilde.middleCols(segment_start_idx, segment_cols) = + temp.rightCols(segment_cols); // populate the index map Eigen::Matrix map_addition(2, N); @@ -544,41 +535,45 @@ teaser::RobustRegistrationSolver::computeTIMs(const Eigen::MatrixmiddleCols(segment_start_idx, segment_cols) = map_addition.rightCols(segment_cols); + map->middleCols(segment_start_idx, segment_cols) = + map_addition.rightCols(segment_cols); } return vtilde; } -teaser::RegistrationSolution -teaser::RobustRegistrationSolver::solve(const teaser::PointCloud& src_cloud, - const teaser::PointCloud& dst_cloud, - const std::vector> correspondences) { +teaser::RegistrationSolution teaser::RobustRegistrationSolver::solve( + const teaser::PointCloud& src_cloud, const teaser::PointCloud& dst_cloud, + const std::vector> correspondences) { Eigen::Matrix src(3, correspondences.size()); Eigen::Matrix dst(3, correspondences.size()); for (size_t i = 0; i < correspondences.size(); ++i) { auto src_idx = std::get<0>(correspondences[i]); auto dst_idx = std::get<1>(correspondences[i]); - src.col(i) << src_cloud[src_idx].x, src_cloud[src_idx].y, src_cloud[src_idx].z; - dst.col(i) << dst_cloud[dst_idx].x, dst_cloud[dst_idx].y, dst_cloud[dst_idx].z; + src.col(i) << src_cloud[src_idx].x, src_cloud[src_idx].y, + src_cloud[src_idx].z; + dst.col(i) << dst_cloud[dst_idx].x, dst_cloud[dst_idx].y, + dst_cloud[dst_idx].z; } return solve(src, dst); } -teaser::RegistrationSolution -teaser::RobustRegistrationSolver::solve(const Eigen::Matrix& src, - const Eigen::Matrix& dst) { +teaser::RegistrationSolution teaser::RobustRegistrationSolver::solve( + const Eigen::Matrix& src, + const Eigen::Matrix& dst) { assert(scale_solver_ && rotation_solver_ && translation_solver_); // Handle deprecated params if (!params_.use_max_clique) { TEASER_DEBUG_INFO_MSG( - "Using deprecated param field use_max_clique. Switch to inlier_selection_mode instead."); + "Using deprecated param field use_max_clique. Switch to " + "inlier_selection_mode instead."); params_.inlier_selection_mode = INLIER_SELECTION_MODE::NONE; } if (!params_.max_clique_exact_solution) { - TEASER_DEBUG_INFO_MSG("Using deprecated param field max_clique_exact_solution. Switch to " - "inlier_selection_mode instead."); + TEASER_DEBUG_INFO_MSG( + "Using deprecated param field max_clique_exact_solution. Switch to " + "inlier_selection_mode instead."); params_.inlier_selection_mode = INLIER_SELECTION_MODE::PMC_HEU; } @@ -599,18 +594,18 @@ teaser::RobustRegistrationSolver::solve(const Eigen::Matrix(std::cout, " ")); + std::copy(max_clique_.begin(), max_clique_.end(), + std::ostream_iterator(std::cout, " ")); std::cout << std::endl; #endif // Abort if max clique size <= 1 @@ -646,7 +646,8 @@ teaser::RobustRegistrationSolver::solve(const Eigen::Matrix rotation_pruned_src(3, max_clique_.size()); - Eigen::Matrix rotation_pruned_dst(3, max_clique_.size()); + Eigen::Matrix rotation_pruned_src( + 3, max_clique_.size()); + Eigen::Matrix rotation_pruned_dst( + 3, max_clique_.size()); for (size_t i = 0; i < max_clique_.size(); ++i) { rotation_pruned_src.col(i) = src.col(max_clique_[i]); rotation_pruned_dst.col(i) = dst.col(max_clique_[i]); @@ -723,8 +726,9 @@ teaser::RobustRegistrationSolver::solve(const Eigen::Matrix& v1, const Eigen::Matrix& v2) { scale_inliers_mask_.resize(1, v1.cols()); - scale_solver_->solveForScale(v1, v2, &(solution_.scale), &scale_inliers_mask_); + scale_solver_->solveForScale(v1, v2, &(solution_.scale), + &scale_inliers_mask_); return solution_.scale; } @@ -757,25 +762,28 @@ Eigen::Matrix3d teaser::RobustRegistrationSolver::solveForRotation( const Eigen::Matrix& v1, const Eigen::Matrix& v2) { rotation_inliers_mask_.resize(1, v1.cols()); - rotation_solver_->solveForRotation(v1, v2, &(solution_.rotation), &rotation_inliers_mask_); + rotation_solver_->solveForRotation(v1, v2, &(solution_.rotation), + &rotation_inliers_mask_); return solution_.rotation; } void teaser::GNCTLSRotationSolver::solveForRotation( const Eigen::Matrix& src, - const Eigen::Matrix& dst, Eigen::Matrix3d* rotation, + const Eigen::Matrix& dst, + Eigen::Matrix3d* rotation, Eigen::Matrix* inliers) { - assert(rotation); // make sure R is not a nullptr - assert(src.cols() == dst.cols()); // check dimensions of input data - assert(params_.gnc_factor > 1); // make sure mu will increase - assert(params_.noise_bound != 0); // make sure noise sigma is not zero + assert(rotation); // make sure R is not a nullptr + assert(src.cols() == dst.cols()); // check dimensions of input data + assert(params_.gnc_factor > 1); // make sure mu will increase + assert(params_.noise_bound != 0); // make sure noise sigma is not zero if (inliers) { assert(inliers->cols() == src.cols()); } /** * Loop: terminate when: - * 1. the change in cost in two consecutive runs is smaller than a user-defined threshold + * 1. the change in cost in two consecutive runs is smaller than a + * user-defined threshold * 2. # iterations exceeds the maximum allowed * * Within each loop: @@ -784,9 +792,9 @@ void teaser::GNCTLSRotationSolver::solveForRotation( */ // Prepare some variables - size_t match_size = src.cols(); // number of correspondences + size_t match_size = src.cols(); // number of correspondences - double mu = 1; // arbitrary starting mu + double mu = 1; // arbitrary starting mu double prev_cost = std::numeric_limits::infinity(); cost_ = std::numeric_limits::infinity(); @@ -794,8 +802,10 @@ void teaser::GNCTLSRotationSolver::solveForRotation( if (noise_bound_sq < 1e-16) { noise_bound_sq = 1e-2; } - TEASER_DEBUG_INFO_MSG("GNC rotation estimation noise bound:" << params_.noise_bound); - TEASER_DEBUG_INFO_MSG("GNC rotation estimation noise bound squared:" << noise_bound_sq); + TEASER_DEBUG_INFO_MSG( + "GNC rotation estimation noise bound:" << params_.noise_bound); + TEASER_DEBUG_INFO_MSG( + "GNC rotation estimation noise bound squared:" << noise_bound_sq); Eigen::Matrix diffs(3, match_size); Eigen::Matrix weights(1, match_size); @@ -804,7 +814,6 @@ void teaser::GNCTLSRotationSolver::solveForRotation( // Loop for performing GNC-TLS for (size_t i = 0; i < params_.max_iterations; ++i) { - // Fix weights and perform SVD rotation estimation *rotation = teaser::utils::svdRot(src, dst, weights); @@ -819,7 +828,8 @@ void teaser::GNCTLSRotationSolver::solveForRotation( // i.e., little to none noise if (mu <= 0) { TEASER_DEBUG_INFO_MSG( - "GNC-TLS terminated because maximum residual at initialization is very small."); + "GNC-TLS terminated because maximum residual at initialization is " + "very small."); break; } } @@ -838,7 +848,8 @@ void teaser::GNCTLSRotationSolver::solveForRotation( } else if (residuals_sq(j) <= th2) { weights(j) = 1; } else { - weights(j) = sqrt(noise_bound_sq * mu * (mu + 1) / residuals_sq(j)) - mu; + weights(j) = + sqrt(noise_bound_sq * mu * (mu + 1) / residuals_sq(j)) - mu; assert(weights(j) >= 0 && weights(j) <= 1); } } @@ -851,7 +862,8 @@ void teaser::GNCTLSRotationSolver::solveForRotation( prev_cost = cost_; if (cost_diff < params_.cost_threshold) { - TEASER_DEBUG_INFO_MSG("GNC-TLS solver terminated due to cost convergence."); + TEASER_DEBUG_INFO_MSG( + "GNC-TLS solver terminated due to cost convergence."); TEASER_DEBUG_INFO_MSG("Cost diff: " << cost_diff); TEASER_DEBUG_INFO_MSG("Iterations: " << i); break; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 7f042c1..14ae1e6 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,16 +1,4 @@ project(teaser_all_test) - -include(FetchContent) -FetchContent_Declare(googletest - GIT_REPOSITORY https://github.com/google/googletest.git - GIT_TAG main -) -if (NOT googletest_POPULATED) - set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) - FetchContent_Populate(googletest) - add_subdirectory(${googletest_SOURCE_DIR} ${googletest_BINARY_DIR} EXCLUDE_FROM_ALL) -endif() - include(GoogleTest) add_subdirectory(test-tools) diff --git a/test/teaser/feature-test.cc b/test/teaser/feature-test.cc index 15ec149..91ea08d 100644 --- a/test/teaser/feature-test.cc +++ b/test/teaser/feature-test.cc @@ -26,7 +26,7 @@ TEST(FPFHTest, CalculateFPFHFeaturesWithPCL) { // Read a PCD file from disk. pcl::PCLPointCloud2 cloud_blob; pcl::io::loadPCDFile("./data/bunny.pcd", cloud_blob); - pcl::fromPCLPointCloud2(cloud_blob, *cloud); + pcl::fromPCLPointCloud2 (cloud_blob, *cloud); // Estimate the normals. pcl::NormalEstimation normalEstimation; @@ -53,7 +53,7 @@ TEST(FPFHTest, CalculateFPFHFeaturesWithTeaserInterface) { pcl::PointCloud::Ptr pcl_cloud(new pcl::PointCloud); pcl::PCLPointCloud2 cloud_blob; pcl::io::loadPCDFile("./data/bunny.pcd", cloud_blob); - pcl::fromPCLPointCloud2(cloud_blob, *pcl_cloud); + pcl::fromPCLPointCloud2 (cloud_blob, *pcl_cloud); teaser::PointCloud input_cloud; for (auto& p : *pcl_cloud) { input_cloud.push_back({p.x, p.y, p.z}); diff --git a/test/teaser/graph-test.cc b/test/teaser/graph-test.cc index 238f4c8..4057e3f 100644 --- a/test/teaser/graph-test.cc +++ b/test/teaser/graph-test.cc @@ -47,7 +47,7 @@ pmc::input generateMockInput() { * @param adj a bool** to the adjcency matrix * @param nodes number of nodes in the graph */ -void printAdjMatirx(const std::vector>& adj, int nodes) { +void printAdjMatirx(const std::vector>& adj, int nodes) { std::cout << "Adjacency matrix: " << std::endl; for (auto& i : adj) { for (auto j : i) { @@ -63,7 +63,7 @@ TEST(GraphTest, BasicFunctions) { teaser::Graph graph; graph.populateVertices(2); graph.addEdge(1, 0); - EXPECT_TRUE(graph.hasEdge(1, 0)); + EXPECT_TRUE(graph.hasEdge(1,0)); EXPECT_TRUE(graph.hasVertex(1)); EXPECT_TRUE(graph.hasVertex(0)); EXPECT_EQ(graph.numEdges(), 1); @@ -158,7 +158,7 @@ TEST(PMCTest, FindMaximumClique1) { } // lower-bound of max clique - std::vector C; + vector C; if (in.lb == 0 && in.heu_strat != "0") { // skip if given as input pmc::pmc_heu maxclique(G, in); in.lb = maxclique.search(G, C); @@ -202,7 +202,7 @@ TEST(PMCTest, FindMaximumClique2) { } // lower-bound of max clique - std::vector C; + vector C; if (in.lb == 0 && in.heu_strat != "0") { // skip if given as input pmc::pmc_heu maxclique(G, in); in.lb = maxclique.search(G, C); @@ -245,7 +245,7 @@ TEST(PMCTest, FindMaximumClique3) { } // lower-bound of max clique - std::vector C; + vector C; if (in.lb == 0 && in.heu_strat != "0") { // skip if given as input pmc::pmc_heu maxclique(G, in); in.lb = maxclique.search(G, C); @@ -297,7 +297,7 @@ TEST(MaxCliqueSolverTest, FindMaxClique) { std::copy(clique.begin(), clique.end(), std::ostream_iterator(std::cout, " ")); std::cout << std::endl; std::set s(clique.begin(), clique.end()); - std::vector ref_clique{0, 1, 2, 3, 4}; + std::vector ref_clique{0,1,2,3,4}; for (const auto& i : ref_clique) { EXPECT_TRUE(s.find(i) != s.end()); } @@ -335,7 +335,7 @@ TEST(PMCTest, FindMaximumCliqueSingleThreaded) { } // lower-bound of max clique - std::vector C; + vector C; if (in.lb == 0 && in.heu_strat != "0") { // skip if given as input pmc::pmc_heu maxclique(G, in); in.lb = maxclique.search(G, C); @@ -356,6 +356,7 @@ TEST(PMCTest, FindMaximumCliqueSingleThreaded) { EXPECT_EQ(C.size(), 5); } + TEST(PMCTest, FindMaximumCliqueMultiThreaded) { // A complete graph with max clique # = 5 auto in = generateMockInput(); @@ -387,7 +388,7 @@ TEST(PMCTest, FindMaximumCliqueMultiThreaded) { } // lower-bound of max clique - std::vector C; + vector C; if (in.lb == 0 && in.heu_strat != "0") { // skip if given as input pmc::pmc_heu maxclique(G, in); in.lb = maxclique.search(G, C); diff --git a/test/teaser/matcher-test.cc b/test/teaser/matcher-test.cc index 549db58..2103a31 100644 --- a/test/teaser/matcher-test.cc +++ b/test/teaser/matcher-test.cc @@ -67,7 +67,7 @@ TEST(FPFHMatcherTest, MatchCase1) { ASSERT_EQ(tokens.size(), 2); ref_correspondences.emplace_back( // -1 because the ref correspondences use 1-index (MATLAB) - std::pair{std::stoi(tokens[0]) - 1, std::stoi(tokens[1]) - 1}); + std::pair{std::stoi(tokens[0])-1, std::stoi(tokens[1])-1}); } // compare calculated correspondences with reference correspondences diff --git a/test/teaser/rotation-solver-test.cc b/test/teaser/rotation-solver-test.cc index 8434736..fd9f3b1 100644 --- a/test/teaser/rotation-solver-test.cc +++ b/test/teaser/rotation-solver-test.cc @@ -31,7 +31,7 @@ TEST(RotationSolverTest, FGRRotation) { Eigen::Matrix dst_points = src_points; // Set up FGR - teaser::FastGlobalRegistrationSolver::Params params{1000, 0.0337, 1.4, 1e-3}; + teaser::FastGlobalRegistrationSolver::Params params{1000, 0.0337, 1.4, 1e-3}; teaser::FastGlobalRegistrationSolver fgr_solver(params); Eigen::Matrix3d result; diff --git a/test/teaser/utils-test.cc b/test/teaser/utils-test.cc index 32b74e7..193ff90 100644 --- a/test/teaser/utils-test.cc +++ b/test/teaser/utils-test.cc @@ -115,8 +115,8 @@ TEST(UtilsTest, CalculatePointClusterDiameter) { { Eigen::Matrix test_mat(3,3); test_mat << -1, 0, 1, - -1, 0, 1, - -1, 0, 1; + -1, 0, 1, + -1, 0, 1; float d = teaser::utils::calculateDiameter(test_mat); EXPECT_NEAR(d, 3.4641, 0.0001); } @@ -128,4 +128,4 @@ TEST(UtilsTest, CalculatePointClusterDiameter) { float d = teaser::utils::calculateDiameter(test_mat); EXPECT_NEAR(d, 5.1962, 0.0001); } -} +} \ No newline at end of file diff --git a/test/test-tools/test_utils.h b/test/test-tools/test_utils.h index e28e20f..878442a 100644 --- a/test/test-tools/test_utils.h +++ b/test/test-tools/test_utils.h @@ -132,4 +132,4 @@ inline std::vector readSubdirs(std::string path) { } } // namespace test -} // namespace teaser +} // namespace teaser \ No newline at end of file