C++ API¶
Solver API¶
teaser/registration.h defines the core registration solver API which provides users with the ability to solve 3D registration problems given correspondences.
Registration Solution¶
Struct to hold solution to a registration problem
Public Members
7-DoF Solver¶
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.
Public Types
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.
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.
Values:
Enum representing the type of graph-based inlier selection algorithm to use
PMC_EXACT: Use PMC to find exact clique from the inlier graph PMC_HEU: Use PMC’s heuristic finder to find approximate max clique KCORE_HEU: Use k-core heuristic to select inliers NONE: No inlier selection
Values:
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
Values:
Public Functions
A constructor that takes in parameters and initialize the estimators accordingly.
This is the preferred way of initializing the different estimators, instead of setting each estimator one by one.
- Parameters
params –
Given a 3-by-N matrix representing points, return Translation Invariant Measurements (TIMs)
- Parameters
v – a 3-by-N matrix
- Returns
a 3-by-(N-1)*N matrix representing TIMs
Solve for scale, translation and rotation.
- Parameters
src_cloud – source point cloud (to be transformed)
dst_cloud – target point cloud (after transformation)
correspondences – A vector of tuples representing the correspondences between pairs of points in the two clouds
Solve for scale, translation and rotation. Assumes dst is src after transformation.
- Parameters
src –
dst –
Solve for scale. Assume v2 = s * R * v1, this function estimates s.
- Parameters
v1 –
v2 –
Solve for translation.
- Parameters
v1 –
v2 –
Solve for rotation. Assume v2 = R * v1, this function estimates find R.
- Parameters
v1 –
v2 –
Return the cost at termination of the GNC rotation solver. Can be used to assess the quality of the solution.
- Returns
cost at termination of the GNC solver. Undefined if run before running the solver.
Return the solution to the registration problem.
- Returns
Set the scale estimator used
- Parameters
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.
- Parameters
estimator –
Set the translation estimator used.
- Parameters
estimator –
Return a boolean Eigen row vector indicating whether specific measurements are inliers according to scales.
- Returns
a 1-by-(number of TIMs) boolean Eigen matrix
Return the index map for scale inliers (equivalent to the index map for TIMs).
- Returns
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 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.
Return a boolean Eigen row vector indicating whether specific measurements are inliers according to the rotation solver.
- Returns
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 the index map for translation inliers (equivalent to max clique). /TODO: This is obsolete now. Remove or update
- Returns
a 1-by-(size of max clique) Eigen matrix. Entries represent the indices of the original measurements.
Return inliers from rotation estimation
- Returns
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 boolean Eigen row vector indicating whether specific measurements are inliers according to translation measurements.
- Returns
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 the index map for translation inliers (equivalent to max clique).
- Returns
a 1-by-(size of max clique) Eigen matrix. Entries represent the indices of the original measurements.
Return inliers from rotation estimation
- Returns
a vector of indices of measurements deemed as inliers by rotation estimation
Return a boolean Eigen row vector indicating whether specific measurements are inliers according to translation measurements.
- Returns
Get TIMs built from source point cloud.
- Returns
Get TIMs built from target point cloud.
- Returns
Get src TIMs built after max clique pruning.
- Returns
Get dst TIMs built after max clique pruning.
- Returns
Get the index map of the TIMs built from source point cloud.
- Returns
Get the index map of the TIMs built from target point cloud.
- Returns
Get the index map of the TIMs used in rotation estimation.
- Returns
Get the index map of the TIMs used in rotation estimation.
- Returns
Reset the solver using the provided params
- Parameters
params – a Params struct
Return the params
- Returns
a Params struct
A struct representing params for initializing the RobustRegistrationSolver
Note: the default values needed to be changed accordingly for best performance.
Public Members
A bound on the noise of each provided measurement.
Square of the ratio between acceptable noise and noise bound. Usually set to 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.
When the solver does not estimate scale, it solves the registration problem much faster.
Which algorithm to use to estimate rotations.
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.
Maximum iterations allowed for the GNC rotation estimators.
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.
Type of TIM graph given to GNC rotation solver
Type of the inlier selection.
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.
- Deprecated:
Use inlier_selection_mode instead Set this to true to enable max clique inlier selection, false to skip it.
- Deprecated:
Use inlier_selection_mode instead Set this to false to enable heuristic only max clique finding.
Time limit on running the max clique algorithm (in seconds).
Number of threads used for the maximum clique solver
Translation Solver¶
Perform translation estimation using truncated least-squares (TLS)
Public Functions
Estimate translation between src and dst points. Assume dst = src + t.
- Parameters
src –
dst –
translation – output parameter for the translation vector
inliers – output parameter for detected outliers
Abstract virtual class for decoupling specific translation estimation method implementations with interfaces.
Subclassed by teaser::TLSTranslationSolver
Public Functions
Pure virtual method for solving translation. Different implementations may have different assumptions about input data.
- Parameters
src –
dst –
- Returns
estimated translation vector
Rotation Solver¶
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.
Public Functions
Parametrized constructor
- Parameters
params –
Estimate rotation between src & dst using GNC-TLS method
- Parameters
src –
dst –
rotation –
inliers –
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 differ from the paper on the estimation of T matrix. We only estimate rotation, instead of rotation and translation.
Public Functions
Remove default constructor
Parametrized constructor
- Parameters
params –
rotation_only –
Solve a pairwise registration problem given two sets of points. Notice that we assume no scale difference between v1 & v2.
- Parameters
src –
dst –
- Returns
a RegistrationSolution struct.
Abstract virtual class for decoupling specific rotation estimation method implementations with interfaces.
Subclassed by teaser::GNCRotationSolver
Public Functions
Pure virtual method for solving rotation. Different implementations may have different assumptions about input data.
- Parameters
src –
dst –
- Returns
estimated rotation matrix (R)
Scale Solver¶
Perform scale estimation using truncated least-squares (TLS)
Public Functions
Use TLS (adaptive voting) to solve for scale. Assume dst = s * R * src
- Parameters
src –
dst –
- Returns
a double indicating the estimated scale
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.
Public Functions
Assume dst = src + noise. The scale output will always be set to 1.
- Parameters
src – [in] a vector of points
dst – [in] a vector of points
scale – [out] a constant of 1
inliers – [out] a row vector of booleans indicating whether a measurement is an inlier
Abstract virtual class for decoupling specific scale estimation methods with interfaces.
Subclassed by teaser::ScaleInliersSelector, teaser::TLSScaleSolver
Public Functions
Pure virtual method for solving scale. Different implementations may have different assumptions about input data.
- Parameters
src –
dst –
- Returns
estimated scale (s)
TLS Estimator¶
Performs scalar truncated least squares estimation
Public Functions
Use truncated least squares method to estimate true x given measurements X TODO: call measurements Z or Y to avoid confusion with x TODO: specify which type/size is x and X in the comments
Copyright 2020, Massachusetts Institute of Technology, Cambridge, MA 02139 All Rights Reserved Authors: Jingnan Shi, et al. (see THANKS for the full author list) See LICENSE for the license information
- Parameters
X – Available measurements
ranges – Maximum admissible errors for measurements X
estimate – (output) pointer to a double holding the estimate
inliers – (output) pointer to a Eigen row vector of inliers
A slightly different implementation of TLS estimate. Use loop tiling to achieve potentially faster performance.
- Parameters
X – Available measurements
ranges – Maximum admissible errors for measurements X
s – scale for tiling
estimate – (output) pointer to a double holding the estimate
inliers – (output) pointer to a Eigen row vector of inliers
Certifier API¶
teaser/certification.h defines the core certification API which provides users with the ability to certify a rotation solution provided by a GNC-TLS rotation solver.
Certification Result¶
Public Members
Rotation Certifier¶
A rotation registration certifier using Douglas–Rachford Splitting (DRS)
Please refer to: [1] H. Yang, J. Shi, and L. Carlone, “TEASER: Fast and Certifiable Point Cloud Registration,” arXiv:2001.07715 [cs, math], Jan. 2020.
Public Types
For most cases, the default solvers in Eigen should be used. For extremely large matrices, it may make sense to use Spectra instead.
Solver for eigendecomposition solver / spectral decomposition.
Values:
Use solvers in the Eigen library.
Use solvers in the Spectra library.
Public Functions
Constructor for DRSCertifier that takes in a parameter struct
- Parameters
params – [in] struct holding all parameters
Constructor for DRSCertifier
- Parameters
noise_bound – [in] bound on the noise
cbar2 – [in] maximal allowed residual^2 to noise bound^2 ratio, usually set to 1
Main certification function
Copyright (c) 2020, Massachusetts Institute of Technology, Cambridge, MA 02139 All Rights Reserved Authors: Jingnan Shi, et al. (see THANKS for the full author list) See LICENSE for the license information
- Parameters
R_solution – [in] a feasible rotation solution
src – [in] vectors under rotation
dst – [in] vectors after rotation
theta – [in] binary (1 vs. 0) vector indicating inliers vs. outliers
- Returns
relative sub-optimality gap
Main certification function
- Parameters
R_solution – [in] a feasible rotation solution
src – [in] vectors under rotation
dst – [in] vectors after rotation
theta – [in] binary (1 vs. -1) vector indicating inliers vs. outliers
- Returns
relative sub-optimality gap
Compute sub-optimality gap
- Parameters
M –
mu –
N –
- Returns
Get the Omega_1 matrix given a quaternion
- Parameters
q – an Eigen quaternion
omega1 – 4-by-4 omega_1 matrix
Get a 4-by-4 block diagonal matrix with each block represents omega_1
- Parameters
q –
theta –
D_omega –
Get Q cost matrix (see Proposition 10 in [1])
- Parameters
v1 – vectors under rotation
v2 – vectors after rotation
Given an arbitrary matrix W, project W to the correct dual structure (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 Frobenious norm
Generate an initial guess (see Appendix U of [1]).
The initial guess satisfies:
KKT complementary slackness
diagonal blocks of (Q - Lambda_guess) is PSD (except the first diagonal block)
- Parameters
R – [in] rotation matrix
theta – [in] a binary (1 & -1) vector indicating inliers vs. outliers
src – [in]
dst – [in]
lambda_guess – [out]
Calculate the inverse of the linear projection matrix A mentioned in Theorem 35 of our TEASER paper [1].
- Parameters
theta_prepended – [in] a binary (1 & -1) vector indicating inliers vs. outliers, with 1 prepended
A_inv – [out] inverse of A
Parameter struct for DRSCertifier
Public Members
Noise bound for the vectors used for certification
Square of the ratio between acceptable noise and noise bound. Usually set to 1.
Suboptimality gap
This is not a percentage. Multiply by 100 to get a percentage.
Maximum iterations allowed
Gamma value (refer to [1] for details)
Solver for eigendecomposition / spectral decomposition
Abstract virtual class representing certification of registration results
Subclassed by teaser::DRSCertifier
Public Functions
Abstract function for certifying rotation estimation results
- Parameters
rotation_solution – [in] a solution to the rotatoin registration problem
src – [in] Assume dst = R * src
dst – [in] Assume dst = R * src
theta – [in] a binary vector indicating inliers vs. outliers
- Returns
relative sub-optimality gap