diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a9c194..a7fd037 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,6 +4,7 @@ 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) @@ -12,6 +13,10 @@ 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_TESTS "Build tests" ON) option(BUILD_TEASER_FPFH "Build TEASER++ wrappers for PCL FPFH estimation." OFF) diff --git a/cmake/pybind11.CMakeLists.txt.in b/cmake/pybind11.CMakeLists.txt.in index b270063..e62ac51 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.11.1 + GIT_TAG v2.13.6 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/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..19a2558 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,155 @@ +[build-system] +requires = ["scikit_build_core", "pybind11"] +build-backend = "scikit_build_core.build" + +[project] +name = "teaserpp_python" +version = "1.1.0" +description = "Python binding for TEASER++" +readme = "README.md" +authors = [ + { name = "Jingnan Shi", email = "jnshi@mit.edu" }, +] +keywords = [ + "Point cloud", + "Registration", + "Non-minimal solver", + "Solver", +] +classifiers = [ + "Intended Audience :: Developers", + "Intended Audience :: Education", + "Intended Audience :: Other Audience", + "Intended Audience :: Science/Research", + "License :: OSI Approved :: MIT License", + "Operating System :: MacOS", + "Operating System :: Microsoft :: Windows", + "Operating System :: Unix", + "Programming Language :: C++", + "Programming Language :: Python :: 3", + "Programming Language :: Python :: 3.6", + "Programming Language :: Python :: 3.7", + "Programming Language :: Python :: 3.8", + "Programming Language :: Python :: 3.9", + "Programming Language :: Python :: 3.10", + "Programming Language :: Python :: 3.11", + "Programming Language :: Python :: 3.12", + "Programming Language :: Python :: 3.13", +] +dependencies = [ + "numpy", + "typing-extensions~=4.2", +] + +[project.urls] +Homepage = "https://github.com/MIT-SPARK/TEASER-plusplus" + +[tool.scikit-build] +build-dir = "build/{wheel_tag}" +build.verbose = false +cmake.version = ">=3.16" +wheel.install-dir = "teaserpp_python.libs" + +[tool.cibuildwheel] +archs = ["auto64"] +skip = ["*-musllinux*", "pp*", "cp36-*"] + +[tool.cibuildwheel.macos] +environment = "MACOSX_DEPLOYMENT_TARGET=10.14" +archs = ["auto64", "arm64"] + +[tool.pyright] +venvPath = "." +venv = ".venv" + +typeCheckingMode = "standard" +reportUnusedImport = "none" +reportUnusedFunction = "none" +reportUnusedVariable = "none" +reportUndefinedVariable = "none" + + +[tool.ruff] +# Exclude a variety of commonly ignored directories. +exclude = [ + ".bzr", + ".direnv", + ".eggs", + ".git", + ".git-rewrite", + ".hg", + ".ipynb_checkpoints", + ".mypy_cache", + ".nox", + ".pants.d", + ".pyenv", + ".pytest_cache", + ".pytype", + ".ruff_cache", + ".svn", + ".tox", + ".venv", + ".vscode", + "__pypackages__", + "_build", + "buck-out", + "build", + "dist", + "node_modules", + "site-packages", + "venv", +] + +# Same as Black. +line-length = 88 +indent-width = 4 + +# Assume Python 3.9 +target-version = "py39" + +[tool.ruff.lint] +# Enable Pyflakes (`F`) and a subset of the pycodestyle (`E`) codes by default. +# Unlike Flake8, Ruff doesn't enable pycodestyle warnings (`W`) or +# McCabe complexity (`C901`) by default. +select = ["E", "F", "I001"] +ignore = [] + +# Allow fix for all enabled rules (when `--fix`) is provided. +fixable = ["ALL"] +unfixable = [] + +# Allow unused variables when underscore-prefixed. +dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$" + +[tool.ruff.format] +# Like Black, use double quotes for strings. +quote-style = "double" + +# Like Black, indent with spaces, rather than tabs. +indent-style = "space" + +# Like Black, respect magic trailing commas. +skip-magic-trailing-comma = false + +# Like Black, automatically detect the appropriate line ending. +line-ending = "auto" + +# Enable auto-formatting of code examples in docstrings. Markdown, +# reStructuredText code/literal blocks and doctests are all supported. +# +# This is currently disabled by default, but it is planned for this +# to be opt-out in the future. +docstring-code-format = true + +# Set the line length limit used when formatting code snippets in +# docstrings. +# +# This only has an effect when the `docstring-code-format` setting is +# enabled. +docstring-code-line-length = "dynamic" + +[dependency-groups] +dev = [ + "basedpyright>=1.22.0", + "ruff>=0.8.1", +] diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index e85cf24..aa70aaa 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -1,4 +1,5 @@ cmake_minimum_required(VERSION 3.10) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) project(teaser_python_bindings) @@ -24,16 +25,23 @@ endif () # make sure to output the build file to teaserpp_python folder SET_TARGET_PROPERTIES(teaserpp_python - PROPERTIES - PREFIX "" - LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/teaserpp_python" - ) + PROPERTIES + OUTPUT_NAME "_teaserpp" + PREFIX "" + LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/teaserpp_python" + INSTALL_RPATH "$ORIGIN/../teaserpp_python.libs/lib;$ORIGIN/../../teaser/" + BUILD_WITH_INSTALL_RPATH TRUE +) # copy package __init__.py file configure_file(teaserpp_python/__init__.py ${CMAKE_CURRENT_BINARY_DIR}/teaserpp_python/__init__.py ) +configure_file(teaserpp_python/__init__.py + ${CMAKE_CURRENT_BINARY_DIR}/teaserpp_python/_teaserpp.pyi + ) + # copy setup.py file configure_file(setup.py.in ${CMAKE_CURRENT_BINARY_DIR}/setup.py @@ -43,3 +51,13 @@ file(COPY . DESTINATION . FILES_MATCHING PATTERN *.py) + +if (DEFINED SKBUILD) + install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/teaserpp/ + DESTINATION "../teaserpp" + FILES_MATCHING PATTERN "*.py" + PATTERN "*.pyi" + PATTERN "*.so" + ) + install(TARGETS teaserpp_python DESTINATION "../teaserpp_python") +endif () diff --git a/python/teaserpp_python/__init__.py b/python/teaserpp_python/__init__.py index 8300277..6f982eb 100644 --- a/python/teaserpp_python/__init__.py +++ b/python/teaserpp_python/__init__.py @@ -1 +1,58 @@ -from .teaserpp_python import * \ No newline at end of file +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) diff --git a/python/teaserpp_python/_teaserpp.pyi b/python/teaserpp_python/_teaserpp.pyi new file mode 100644 index 0000000..30fc273 --- /dev/null +++ b/python/teaserpp_python/_teaserpp.pyi @@ -0,0 +1,531 @@ +from enum import Enum +from typing import List, overload + +import numpy as np +from typing_extensions import Final + +from . import RobustRegistrationSolverParams + +OMP_MAX_THREADS: Final[int] + +class RegistrationSolution: + scale: float + translation: np.ndarray + rotation: np.ndarray + + def __repr__(self) -> str: ... + +class RotationEstimationAlgorithm(Enum): + """ + An enum class representing the available GNC rotation estimation + algorithms. + """ + + #: 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. + GNC_TLS = 0 + + #: 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. + FGR = 1 + + #: see 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. + QUARTO = 2 + +class InlierGraphFormulation(Enum): + """ + Enum representing the formulation of the TIM graph after maximum clique + filtering. + """ + + #: formulate TIMs by only calculating the TIMs for consecutive measurements + CHAIN = 0 + + #: formulate a fully connected TIM graph + COMPLETE = 1 + +class InlierSelectionMode(Enum): + """ + Enum representing the type of graph-based inlier selection algorithm to + use. + """ + + #: Use PMC to find exact clique from the inlier graph + PMC_EXACT = 0 + + #: Use PMC's heuristic finder to find approximate max clique + PMC_HEU = 1 + + #: Use k-core heuristic to select inliers + KCORE_HEU = 2 + + #: No inlier selection + NONE = 3 + +class EigSolverType(Enum): + EIGEN = 0 + SPECTRA = 1 + +class CertificationResult: + is_optimal: bool + best_suboptimality: float + suboptimality_traj: List[float] + + def __repr__(self) -> str: ... + +class RobustRegistrationSolver: + """ + 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. + """ + + ROTATION_ESTIMATION_ALGORITHM = RotationEstimationAlgorithm + INLIER_SELECTION_MODE = InlierSelectionMode + INLIER_GRAPH_FORMULATION = InlierGraphFormulation + + class Params: + 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 + + @overload + def __init__(self, params: Params): ... + + @overload + def __init__( + self, + 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, + ) -> None: + """ + Parameters + ---------- + noise_bound : float + A bound on the noise of each provided measurement. + + cbar2 : float + Square of the ratio between acceptable noise and noise bound. + Default value of 1 is typical. + + estimate_scaling : bool + 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. + + rotation_estimation_algorithm : RotationEstimationAlgorithm + The algorithm to use to estimate rotations. + + rotation_gnc_factor : float + 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. + + rotation_cost_threshold : float + 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. + + rotation_tim_graph : InlierGraphFormulation + Type of TIM graph given to GNC rotation solver. + + kcore_heuristic_threshold : InlierSelectionMode + 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. + + use_max_clique : bool + Deprecated. Use inlier_selection_mode instead + Set this to true to enable max clique inlier selection, + False to skip it. + + max_clique_exact_solution : bool + Deprecated. Use inlier_selection_mode instead + Set this to false to enable heuristic only max clique finding. + + max_clique_time_limit : int + Time limit on running the max clique algorithm (in seconds). + + max_clique_num_threads : Optional[int] + Number of threads to use to run the max clique algorithm. + Defaults to using the maximum number. + + Note + ---- + 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 proceed to call + the max clique finder. Otherwise, the graph will be directly fed to + the GNC rotation solver. + """ + ... + + @property + def params(self) -> RobustRegistrationSolverParams: ... + def getParams(self) -> Params: ... + def reset(self) -> None: ... + def solve(self, src: np.ndarray, dst: np.ndarray) -> None: + """ + Solve for scale, translation and rotation from src to dst. + """ + ... + + @property + def solution(self) -> RegistrationSolution: ... + def getSolution(self) -> RegistrationSolution: ... + @property + def gnc_rotation_cost_at_termination(self): ... + def getGNCRotationCostAtTermination(self) -> float: + """ + Return the cost at termination of the GNC rotation solver. Can be used + to assess the quality of the solution. + """ + ... + + @property + def scale_inliers_mask(self) -> np.ndarray: + """ + A boolean ndarray indicating whether specific measurements are + scale inliers according to scales. + """ + ... + + def getScaleInliersMask(self) -> np.ndarray: + """ + Return a boolean ndarray indicating whether specific measurements are + scale inliers according to scales. + """ + ... + + @property + def scale_inliers_map(self) -> np.ndarray: + """ + The index map for scale inliers (equivalent to the index map for + TIMs). + """ + ... + + def getScaleInliersMap(self) -> np.ndarray: + """ + Return the index map for scale inliers (equivalent to the index map for + TIMs). + + Returns + ------- + A 2-by-(number of TIMs) ndarray. Entries in one column represent + the indices of the two measurements used to calculate the corresponding + TIM. + """ + ... + + @property + def scale_inliers(self) -> List[int]: ... + def getScaleInliers(self) -> List[int]: + """ + Return inlier TIMs from scale estimation + + Returns + ------- + 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. + """ + ... + + @property + def rotation_inliers_mask(self) -> np.ndarray: ... + def getRotationInliersMask(self) -> np.ndarray: + """ + Return a boolean ndarray indicating whether specific + measurements are inliers according to the rotation solver. + + + Returns + ------- + A 1-by-(size of TIMs used in rotation estimation) boolean ndarray. + 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. + """ + ... + + # TODO: This is obsolete now. Remove or update + def getRotationInliersMap(self) -> np.ndarray: + """ + Return the index map for translation inliers (equivalent to max clique). + + Returns + ------- + A 1-by-(size of max clique) ndarray. Entries represent the indices of + the original measurements. + """ + ... + + @property + def rotation_inliers(self) -> List[int]: + """ + The index map for translation inliers (equivalent to max clique). + """ + ... + + def getRotationInliers(self) -> np.ndarray: + """ + Return a boolean ndarray indicating whether specific + measurements are inliers according to translation measurements. + + Returns + ------- + A 1-by-(size of max clique) boolean ndarray. It is equivalent to a + binary mask on the inlier max clique, with true representing that the + measurement is an inlier after translation estimation. + """ + ... + + @property + def translation_inliers_mask(self) -> np.ndarray: + """ + A boolean ndarray indicating whether specific + measurements are inliers according to translation measurements. + + """ + ... + + def getTranslationInliersMask(self) -> np.ndarray: + """ + Return the index map for translation inliers (equivalent to max clique). + + Returns + ------- + A 1-by-(size of max clique) ndarray. Entries represent the indices of + the original measurements. + """ + ... + + @property + def translation_inliers_map(self) -> np.ndarray: + """ + The index map for translation inliers (equivalent to max clique). + """ + ... + + def getTranslationInliersMap(self) -> np.ndarray: + """ + Return the index map for translation inliers (equivalent to max clique). + + Returns + ------- + A 1-by-(size of max clique) ndarray. Entries represent the indices of + the original measurements. + """ + ... + + @property + def translation_inliers(self) -> List[int]: + """ + Inliers from translation estimation + """ + + ... + + def getTranslationInliers(self) -> List[int]: + """ + Return inliers from translation estimation + + Returns + ------- + A vector of indices of measurements deemed as inliers by translation + estimation. + """ + ... + + @property + def inlier_max_clique(self) -> np.ndarray: + """ + A boolean ndarray indicating whether specific measurements are + inliers according to translation measurements. + """ + ... + + def getInlierMaxClique(self) -> np.ndarray: + """ + Return a boolean ndarray indicating whether specific measurements are + inliers according to translation measurements. + """ + ... + + @property + def inlier_graph(self) -> np.ndarray: ... + def getInlierGraph(self) -> np.ndarray: ... + @property + def src_tims_map(self) -> np.ndarray: + """ + The index map of the TIMs built from the source point cloud. + """ + ... + + def getSrcTIMsMap(self) -> np.ndarray: + """ + Get the index map of the TIMs built from the source point cloud. + """ + ... + + @property + def dst_tims_map(self) -> np.ndarray: + """ + The index map of the TIMs built from the target point cloud. + """ + ... + + def getDstTIMsMap(self) -> np.ndarray: + """ + Get the index map of the TIMs built from the target point cloud. + """ + ... + + @property + def src_tims_map_for_rotation(self) -> np.ndarray: + """ + The index map of the TIMs used in rotation estimation from + the source point cloud. + """ + ... + + def getSrcTIMsMapForRotation(self) -> np.ndarray: + """ + Get the index map of the TIMs used in rotation estimation from + the source point cloud. + """ + ... + + @property + def dst_tims_map_for_rotation(self) -> np.ndarray: + """ + The index map of the TIMs used in rotation estimation from + the target point cloud. + """ + ... + + def getDstTIMsMapForRotation(self) -> np.ndarray: + """ + Get the index map of the TIMs used in rotation estimation from the + target point cloud. + """ + ... + + @property + def max_clique_src_tims(self) -> np.ndarray: + """ + Source TIMs built after max clique pruning. + """ + ... + + def getMaxCliqueSrcTIMs(self) -> List[int]: + """ + Get src TIMs built after max clique pruning. + """ + ... + + @property + def max_clique_dst_tims(self) -> np.ndarray: + """ + Target TIMs built after max clique pruning. + """ + ... + + def getMaxCliqueDstTIMs(self) -> List[int]: + """ + Get dst TIMs built after max clique pruning. + """ + ... + + @property + def src_tims(self) -> np.ndarray: + """ + TIMs built from source point cloud. + """ + ... + + def getSrcTIMs(self) -> np.ndarray: + """ + Get TIMs built from source point cloud. + """ + ... + + @property + def dst_tims(self) -> np.ndarray: + """ + TIMs built from source target point cloud. + """ + ... + + def getDstTIMs(self) -> np.ndarray: + """ + Get TIMs built from target point cloud. + """ + ... + +class DRSCertifier: + EIG_SOLVER_TYPE = EigSolverType + + class Params: + noise_bound: float + cbar2: float + sub_optimality: float + max_iterations: int + gamma_tau: float + eig_decomposition_solver: EigSolverType + + def __init__(self) -> None: ... + + def __init__(self, params: Params) -> None: ... + def certify( + self, rotation: np.ndarray, src: np.ndarray, dst: np.ndarray, mask: np.ndarray + ) -> CertificationResult: ... diff --git a/python/teaserpp_python/teaserpp_example.py b/python/teaserpp_python/teaserpp_example.py index 3bae060..f9ba24a 100644 --- a/python/teaserpp_python/teaserpp_example.py +++ b/python/teaserpp_python/teaserpp_example.py @@ -21,37 +21,28 @@ dst[:, 1] += 10 dst[:, 9] += 15 - # 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) + 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) + solver.solve(src, dst) - # 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(scale_inliers)): - print(scale_inliers[i], end=',') + for i in range(len(solver.scale_inliers)): + print(solver.scale_inliers[i], end=',') print("\n=======================================") - print("Translation inliers are:", translation_inliers) - print("Translation inliers map is:", translation_inliers_map) + print("Translation inliers are:", solver.translation_inliers) + print("Translation inliers map is:", solver.translation_inliers_map) diff --git a/python/teaserpp_python/teaserpp_python.cc b/python/teaserpp_python/teaserpp_python.cc index ac7d3c6..13f88ac 100644 --- a/python/teaserpp_python/teaserpp_python.cc +++ b/python/teaserpp_python/teaserpp_python.cc @@ -6,6 +6,7 @@ * See LICENSE for the license information */ +#include #include #include @@ -21,7 +22,7 @@ namespace py = pybind11; /** * Python interface with pybind11 */ -PYBIND11_MODULE(teaserpp_python, m) { +PYBIND11_MODULE(_teaserpp, m) { m.doc() = "Python binding for TEASER++"; // Python bound for teaser::RegistrationSolution @@ -41,61 +42,139 @@ PYBIND11_MODULE(teaserpp_python, 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::class_ solver( + m, "RobustRegistrationSolver", py::dynamic_attr() + ); // Python bound for teaser::RobustRegistrationSolver functions - solver.def(py::init<>()) - .def(py::init()) + 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()) .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("getMaxCliqueDstTIMs", &teaser::RobustRegistrationSolver::getMaxCliqueDstTIMs) - .def("getSrcTIMs", &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) - .value("QUATRO", teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::QUATRO); + .def_property_readonly("max_clique_dst_tims", &teaser::RobustRegistrationSolver::getMaxCliqueSrcTIMs) + .def("getMaxCliqueDstTIMs", &teaser::RobustRegistrationSolver::getMaxCliqueDstTIMs) - // 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); + .def_property_readonly("src_tims", &teaser::RobustRegistrationSolver::getSrcTIMs) + .def("getSrcTIMs", &teaser::RobustRegistrationSolver::getSrcTIMs) - // 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); + .def_property_readonly("dst_tims", &teaser::RobustRegistrationSolver::getSrcTIMs) + .def("getDstTIMs", &teaser::RobustRegistrationSolver::getDstTIMs); // Python bound for teaser::RobustRegistrationSolver::Params py::class_(solver, "Params") @@ -199,11 +278,6 @@ PYBIND11_MODULE(teaserpp_python, 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<>()) diff --git a/teaser/CMakeLists.txt b/teaser/CMakeLists.txt index 89e9209..133ea6a 100644 --- a/teaser/CMakeLists.txt +++ b/teaser/CMakeLists.txt @@ -1,5 +1,11 @@ project(teaser_source) include(GNUInstallDirs) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +if (DEFINED SKBUILD) + set(CMAKE_INSTALL_RPATH "$ORIGIN") + set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE) +endif () # teaser_io library add_library(teaser_io SHARED src/ply_io.cc) @@ -9,6 +15,13 @@ target_include_directories(teaser_io PUBLIC $ $) + +SET_TARGET_PROPERTIES(teaser_io + PROPERTIES + INSTALL_RPATH "$ORIGIN;$ORIGIN/../tinyply-build" + BUILD_WITH_INSTALL_RPATH TRUE +) + install(TARGETS teaser_io EXPORT teaserpp-export LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} @@ -23,6 +36,12 @@ add_library(teaser_registration SHARED src/registration.cc src/graph.cc ) + +SET_TARGET_PROPERTIES(teaser_registration + PROPERTIES + INSTALL_RPATH "$ORIGIN;$ORIGIN/../pmc-build" + BUILD_WITH_INSTALL_RPATH TRUE +) target_link_libraries(teaser_registration PUBLIC Eigen3::Eigen PRIVATE pmc ${TEASERPP_BLAS_LAPACK_LIBS} @@ -88,11 +107,13 @@ endif () set(TEASERPP_EXPORTED_TARGETS "${TEASERPP_EXPORTED_TARGETS}" PARENT_SCOPE) # installation -install(EXPORT teaserpp-export +install(EXPORT teaserpp-export DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/teaserpp NAMESPACE teaserpp:: FILE teaserppTargets.cmake ) -install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) -install(DIRECTORY ${SPECTRA_INCLUDE_DIRS} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) +if (NOT DEFINED SKBUILD) + install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) + install(DIRECTORY ${SPECTRA_INCLUDE_DIRS} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) +endif () diff --git a/teaser/include/teaser/linalg.h b/teaser/include/teaser/linalg.h index f1e8d19..8ee6963 100644 --- a/teaser/include/teaser/linalg.h +++ b/teaser/include/teaser/linalg.h @@ -43,7 +43,7 @@ Eigen::Matrix hatmap(const Eigen::Matrix& u) { template void vectorKron(const Eigen::Matrix& v1, const Eigen::Matrix& v2, Eigen::Matrix* output) { -#pragma omp parallel for collapse(2) shared(v1, v2, output) default(none) +#pragma omp parallel for collapse(2) shared(v1, v2, output) for (size_t i = 0; i < N; ++i) { for (size_t j = 0; j < M; ++j) { (*output)[i * M + j] = v1[i] * v2[j]; @@ -62,7 +62,7 @@ template 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) default(none) +#pragma omp parallel for collapse(2) shared(v1, v2, output) 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]; diff --git a/teaser/include/teaser/registration.h b/teaser/include/teaser/registration.h index 6cf52bf..a778f7f 100644 --- a/teaser/include/teaser/registration.h +++ b/teaser/include/teaser/registration.h @@ -515,6 +515,29 @@ 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. * @@ -804,40 +827,55 @@ class RobustRegistrationSolver { * Reset the solver using the provided params * @param params a Params struct */ - void reset(const Params& params) { - params_ = params; - + 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 + ) { // Initialize the scale estimator - if (params_.estimate_scaling) { + if (estimate_scaling) { setScaleEstimator( - std::make_unique(params_.noise_bound, params_.cbar2)); + std::make_unique(noise_bound, cbar2)); } else { setScaleEstimator( - std::make_unique(params_.noise_bound, params_.cbar2)); + std::make_unique(noise_bound, cbar2)); } // Initialize the rotation estimator - 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)); - break; - } - case ROTATION_ESTIMATION_ALGORITHM::QUATRO: { // Quatro method - setRotationEstimator(std::make_unique(rotation_params)); - break; - } + 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)); + break; + } + 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)); + break; + } } // Initialize the translation estimator setTranslationEstimator( - std::make_unique(params_.noise_bound, params_.cbar2)); + std::make_unique(noise_bound, cbar2)); // Clear member variables max_clique_.clear(); @@ -846,6 +884,29 @@ 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 diff --git a/teaser/src/registration.cc b/teaser/src/registration.cc index c4e98f7..01b6174 100644 --- a/teaser/src/registration.cc +++ b/teaser/src/registration.cc @@ -159,7 +159,7 @@ void teaser::ScalarTLSEstimator::estimate_tiled(const Eigen::RowVectorXd& X, } }; -#pragma omp parallel for default(none) shared( \ +#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) for (size_t ih = 0; ih < ih_bound; ih += s) { @@ -173,7 +173,7 @@ void teaser::ScalarTLSEstimator::estimate_tiled(const Eigen::RowVectorXd& X, // finish the left over entries // 1. Finish the unfinished js -#pragma omp parallel for default(none) \ +#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) @@ -182,7 +182,7 @@ void teaser::ScalarTLSEstimator::estimate_tiled(const Eigen::RowVectorXd& X, } // 2. Finish the unfinished is -#pragma omp parallel for default(none) \ +#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) @@ -470,6 +470,40 @@ void teaser::TLSTranslationSolver::solveForTranslation( } } +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); @@ -483,7 +517,7 @@ teaser::RobustRegistrationSolver::computeTIMs(const Eigen::Matrix vtilde(3, N * (N - 1) / 2); map->resize(2, N * (N - 1) / 2); -#pragma omp parallel for default(none) shared(N, v, vtilde, map) +#pragma omp parallel for 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.