21 #include "pipelines/registration/Registration.h"
22 #include "pipelines/registration/RobustKernel.h"
23 #include "pipelines/registration/TransformationEstimation.h"
28 namespace registration {
30 template <
class TransformationEstimationBase = TransformationEstimation>
33 using TransformationEstimationBase::TransformationEstimationBase;
37 TransformationEstimationBase,
void);
42 PYBIND11_OVERLOAD_PURE(
double, TransformationEstimationBase, source,
49 PYBIND11_OVERLOAD_PURE(Eigen::Matrix4d, TransformationEstimationBase,
50 source, target, corres);
54 template <
class CorrespondenceCheckerBase = CorrespondenceChecker>
57 using CorrespondenceCheckerBase::CorrespondenceCheckerBase;
61 const Eigen::Matrix4d &transformation)
const override {
62 PYBIND11_OVERLOAD_PURE(
bool, CorrespondenceCheckerBase, source, target,
63 corres, transformation);
69 py::class_<ICPConvergenceCriteria> convergence_criteria(
70 m,
"ICPConvergenceCriteria",
71 "Class that defines the convergence criteria of ICP. ICP "
73 "stops if the relative change of fitness and rmse hit "
74 "``relative_fitness`` and ``relative_rmse`` individually, "
76 "iteration number exceeds ``max_iteration``.");
77 py::detail::bind_copy_functions<ICPConvergenceCriteria>(
78 convergence_criteria);
80 .def(py::init([](
double fitness,
double rmse,
int itr) {
83 "relative_fitness"_a = 1e-6,
"relative_rmse"_a = 1e-6,
84 "max_iteration"_a = 30)
88 "If relative change (difference) of fitness score is lower "
89 "than ``relative_fitness``, the iteration stops.")
92 "If relative change (difference) of inliner RMSE score is "
93 "lower than ``relative_rmse``, the iteration stops.")
94 .def_readwrite(
"max_iteration",
96 "Maximum iteration before iteration stops.")
99 "ICPConvergenceCriteria class "
100 "with relative_fitness={:e}, relative_rmse={:e}, "
101 "and max_iteration={:d}",
107 py::class_<RANSACConvergenceCriteria> ransac_criteria(
108 m,
"RANSACConvergenceCriteria",
109 "Class that defines the convergence criteria of "
110 "RANSAC. RANSAC algorithm stops if the iteration "
111 "number hits ``max_iteration``, or the fitness "
112 "measured during validation suggests that the "
113 "algorithm can be terminated early with some "
114 "``confidence``. Early termination takes place "
115 "when the number of iterations reaches ``k = "
116 "log(1 - confidence)/log(1 - fitness^{ransac_n})``, "
117 "where ``ransac_n`` is the number of points used "
118 "during a ransac iteration. Note "
119 "that the validation is the most computational "
120 "expensive operator in an iteration. Most "
121 "iterations do not do full validation. It is "
122 "crucial to control ``confidence`` so that the "
123 "computation time is acceptable.");
124 py::detail::bind_copy_functions<RANSACConvergenceCriteria>(ransac_criteria);
126 .def(py::init([](
int max_iteration,
double confidence) {
130 "max_iteration"_a = 100000,
"confidence"_a = 0.999)
131 .def_readwrite(
"max_iteration",
133 "Maximum iteration before iteration stops.")
136 "Desired probability of success. Used for estimating early "
140 "RANSACConvergenceCriteria "
141 "class with max_iteration={:d}, "
142 "and confidence={:e}",
149 te(m,
"TransformationEstimation",
150 "Base class that estimates a transformation between two point "
151 "clouds. The virtual function ComputeTransformation() must be "
152 "implemented in subclasses.");
154 "target"_a,
"corres"_a,
155 "Compute RMSE between source and target points cloud given "
157 te.def(
"compute_transformation",
159 "target"_a,
"corres"_a,
160 "Compute transformation from source to target point cloud given "
163 m,
"TransformationEstimation",
"compute_rmse",
164 {{
"source",
"Source point cloud."},
165 {
"target",
"Target point cloud."},
167 "Correspondence set between source and target point cloud."}});
169 m,
"TransformationEstimation",
"compute_transformation",
170 {{
"source",
"Source point cloud."},
171 {
"target",
"Target point cloud."},
173 "Correspondence set between source and target point cloud."}});
180 te_p2p(m,
"TransformationEstimationPointToPoint",
181 "Class to estimate a transformation for point to point "
183 py::detail::bind_copy_functions<TransformationEstimationPointToPoint>(
185 te_p2p.def(py::init([](
bool with_scaling) {
189 "with_scaling"_a =
false)
194 "TransformationEstimationPointToPoint ") +
196 ? std::string(
"with scaling.")
197 : std::string(
"without scaling."));
202 R
"(Set to ``True`` to estimate scaling, ``False`` to force
205 The homogeneous transformation is given by
207 :math:`T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}`
209 Sets :math:`c = 1` if ``with_scaling`` is ``False``.
217 te_p2l(m,
"TransformationEstimationPointToPlane",
218 "Class to estimate a transformation for point to plane "
220 py::detail::bind_default_constructor<TransformationEstimationPointToPlane>(
222 py::detail::bind_copy_functions<TransformationEstimationPointToPlane>(
224 te_p2l.def(py::init([](std::shared_ptr<RobustKernel> kernel) {
231 return std::string(
"TransformationEstimationPointToPlane");
233 .def_readwrite(
"kernel",
235 "Robust Kernel used in the Optimization");
242 te_col(m,
"TransformationEstimationForColoredICP",
243 "Class to estimate a transformation between two point "
244 "clouds using color information");
245 py::detail::bind_default_constructor<TransformationEstimationForColoredICP>(
247 py::detail::bind_copy_functions<TransformationEstimationForColoredICP>(
249 te_col.def(py::init([](
double lambda_geometric,
250 std::shared_ptr<RobustKernel> kernel) {
252 lambda_geometric, std::move(kernel));
254 "lambda_geometric"_a,
"kernel"_a)
255 .def(py::init([](
double lambda_geometric) {
259 "lambda_geometric"_a)
260 .def(py::init([](std::shared_ptr<RobustKernel> kernel) {
262 te.kernel_ = std::move(kernel);
269 "TransformationEstimationForColoredICP "
270 "with lambda_geometric:") +
277 .def_readwrite(
"kernel",
279 "Robust Kernel used in the Optimization");
287 te_gicp(m,
"TransformationEstimationForGeneralizedICP",
288 "Class to estimate a transformation for Generalized ICP.");
291 py::detail::bind_copy_functions<TransformationEstimationForGeneralizedICP>(
293 te_gicp.def(py::init([](
double epsilon,
294 std::shared_ptr<RobustKernel> kernel) {
296 epsilon, std::move(kernel));
298 "epsilon"_a,
"kernel"_a)
299 .def(py::init([](
double epsilon) {
304 .def(py::init([](std::shared_ptr<RobustKernel> kernel) {
306 te.kernel_ = std::move(kernel);
313 "TransformationEstimationForGeneralizedICP "
317 .def_readwrite(
"epsilon",
320 .def_readwrite(
"kernel",
322 "Robust Kernel used in the Optimization");
327 cc(m,
"CorrespondenceChecker",
328 "Base class that checks if two (small) point clouds can be "
329 "aligned. This class is used in feature based matching "
330 "algorithms (such as RANSAC and FastGlobalRegistration) to "
331 "prune out outlier correspondences. The virtual function "
332 "Check() must be implemented in subclasses.");
334 "corres"_a,
"transformation"_a,
335 "Function to check if two points can be aligned. The two input "
336 "point clouds must have exact the same number of points.");
338 "require_pointcloud_alignment_",
340 "Some checkers do not require point clouds to be aligned, e.g., "
341 "the edge length checker. Some checkers do, e.g., the distance "
344 m,
"CorrespondenceChecker",
"Check",
345 {{
"source",
"Source point cloud."},
346 {
"target",
"Target point cloud."},
348 "Correspondence set between source and target point cloud."},
349 {
"transformation",
"The estimated transformation (inplace)."}});
356 cc_el(m,
"CorrespondenceCheckerBasedOnEdgeLength",
357 "Check if two point clouds build the polygons with similar "
358 "edge lengths. That is, checks if the lengths of any two "
359 "arbitrary edges (line formed by two vertices) individually "
360 "drawn withinin source point cloud and within the target "
361 "point cloud with correspondences are similar. The only "
362 "parameter similarity_threshold is a number between 0 "
363 "(loose) and 1 (strict)");
364 py::detail::bind_copy_functions<CorrespondenceCheckerBasedOnEdgeLength>(
366 cc_el.def(py::init([](
double similarity_threshold) {
368 similarity_threshold);
370 "similarity_threshold"_a = 0.9)
375 "CorrespondenceCheckerBasedOnEdgeLength "
376 "with similarity_threshold={:f}",
377 c.similarity_threshold_);
380 "similarity_threshold",
382 similarity_threshold_,
383 R
"(float value between 0 (loose) and 1 (strict): For the
386 :math:`||\text{edge}_{\text{source}}|| > \text{similarity_threshold} \times ||\text{edge}_{\text{target}}||` and
388 :math:`||\text{edge}_{\text{target}}|| > \text{similarity_threshold} \times ||\text{edge}_{\text{source}}||`
390 must hold true for all edges.)");
397 cc_d(m,
"CorrespondenceCheckerBasedOnDistance",
398 "Class to check if aligned point clouds are close (less than "
399 "specified threshold).");
400 py::detail::bind_copy_functions<CorrespondenceCheckerBasedOnDistance>(cc_d);
401 cc_d.def(py::init([](
double distance_threshold) {
405 "distance_threshold"_a)
410 "CorrespondenceCheckerBasedOnDistance with "
411 "distance_threshold={:f}",
412 c.distance_threshold_);
415 "distance_threshold",
417 "Distance threshold for the check.");
424 cc_n(m,
"CorrespondenceCheckerBasedOnNormal",
425 "Class to check if two aligned point clouds have similar "
426 "normals. It considers vertex normal affinity of any "
427 "correspondences. It computes dot product of two normal "
428 "vectors. It takes radian value for the threshold.");
429 py::detail::bind_copy_functions<CorrespondenceCheckerBasedOnNormal>(cc_n);
430 cc_n.def(py::init([](
double normal_angle_threshold) {
432 normal_angle_threshold);
434 "normal_angle_threshold"_a)
439 "CorrespondenceCheckerBasedOnNormal with "
440 "normal_threshold={:f}",
441 c.normal_angle_threshold_);
443 .def_readwrite(
"normal_angle_threshold",
445 normal_angle_threshold_,
446 "Radian value for angle threshold.");
449 py::class_<FastGlobalRegistrationOption> fgr_option(
450 m,
"FastGlobalRegistrationOption",
451 "Options for FastGlobalRegistration.");
452 py::detail::bind_copy_functions<FastGlobalRegistrationOption>(fgr_option);
454 .def(py::init([](
double division_factor,
bool use_absolute_scale,
456 double maximum_correspondence_distance,
457 int iteration_number,
double tuple_scale,
458 int maximum_tuple_count,
bool tuple_test) {
460 division_factor, use_absolute_scale, decrease_mu,
461 maximum_correspondence_distance, iteration_number,
462 tuple_scale, maximum_tuple_count, tuple_test);
464 "division_factor"_a = 1.4,
"use_absolute_scale"_a =
false,
465 "decrease_mu"_a =
false,
466 "maximum_correspondence_distance"_a = 0.025,
467 "iteration_number"_a = 64,
"tuple_scale"_a = 0.95,
468 "maximum_tuple_count"_a = 1000,
"tuple_test"_a =
true)
472 "float: Division factor used for graduated non-convexity.")
474 "use_absolute_scale",
476 "bool: Measure distance in absolute scale (1) or in scale "
477 "relative to the diameter of the model (0).")
478 .def_readwrite(
"decrease_mu",
480 "bool: Set to ``True`` to decrease scale mu by "
481 "``division_factor`` for graduated non-convexity.")
482 .def_readwrite(
"maximum_correspondence_distance",
484 maximum_correspondence_distance_,
485 "float: Maximum correspondence distance.")
486 .def_readwrite(
"iteration_number",
488 "int: Maximum number of iterations.")
491 "float: Similarity measure used for tuples of feature "
493 .def_readwrite(
"maximum_tuple_count",
495 "float: Maximum tuple numbers.")
498 "bool: Set to `true` to perform geometric compatibility "
499 "tests on initial set of correspondences.")
502 "FastGlobalRegistrationOption("
503 "\ndivision_factor={},"
504 "\nuse_absolute_scale={},"
506 "\nmaximum_correspondence_distance={},"
507 "\niteration_number={},"
509 "\nmaximum_tuple_count={},"
512 c.division_factor_, c.use_absolute_scale_,
513 c.decrease_mu_, c.maximum_correspondence_distance_,
514 c.iteration_number_, c.tuple_scale_,
515 c.maximum_tuple_count_, c.tuple_test_);
519 py::class_<RegistrationResult> registration_result(
520 m,
"RegistrationResult",
521 "Class that contains the registration results.");
522 py::detail::bind_default_constructor<RegistrationResult>(
523 registration_result);
524 py::detail::bind_copy_functions<RegistrationResult>(registration_result);
526 .def_readwrite(
"transformation",
528 "``4 x 4`` float64 numpy array: The estimated "
529 "transformation matrix.")
531 "correspondence_set",
533 "``n x 2`` int numpy array: Correspondence set between "
534 "source and target point cloud.")
536 "float: RMSE of all inlier correspondences. Lower "
540 "float: The overlapping area (# of inlier correspondences "
541 "/ # of points in target). Higher is better.")
544 "RegistrationResult with "
547 ", and correspondence_set size of {:d}"
548 "\nAccess transformation to get result.",
555 static const std::unordered_map<std::string, std::string>
558 "Vector of Checker class to check if two point "
559 "clouds can be aligned. One of "
561 "CorrespondenceCheckerBasedOnEdgeLength``, "
563 "CorrespondenceCheckerBasedOnDistance``, "
565 "CorrespondenceCheckerBasedOnNormal``)"},
567 "Desired probability of success for RANSAC. Used for "
568 "estimating early termination by k = log(1 - "
569 "confidence)/log(1 - inlier_ratio^{ransac_n}."},
571 "o3d.utility.Vector2iVector that stores indices of "
572 "corresponding point or feature arrays."},
573 {
"criteria",
"Convergence criteria"},
574 {
"estimation_method",
575 "Estimation method. One of "
577 "TransformationEstimationPointToPoint``, "
579 "TransformationEstimationPointToPlane``, "
581 "TransformationEstimationForGeneralizedICP``, "
583 "TransformationEstimationForColoredICP``)"},
584 {
"init",
"Initial transformation estimation"},
585 {
"lambda_geometric",
"lambda_geometric value"},
586 {
"epsilon",
"epsilon value"},
587 {
"kernel",
"Robust Kernel used in the Optimization"},
588 {
"max_correspondence_distance",
589 "Maximum correspondence points-pair distance."},
591 "Enables mutual filter such that the correspondence of the "
592 "source point's correspondence is itself."},
593 {
"option",
"Registration option"},
594 {
"ransac_n",
"Fit ransac with ``ransac_n`` correspondences"},
595 {
"source_feature",
"Source point cloud feature."},
596 {
"source",
"The source point cloud."},
597 {
"target_feature",
"Target point cloud feature."},
598 {
"target",
"The target point cloud."},
600 "The 4x4 transformation matrix to transform ``source`` to "
605 py::call_guard<py::gil_scoped_release>(),
606 "Function for evaluating registration between point clouds",
607 "source"_a,
"target"_a,
"max_correspondence_distance"_a,
608 "transformation"_a = Eigen::Matrix4d::Identity());
613 py::call_guard<py::gil_scoped_release>(),
614 "Function for ICP registration",
"source"_a,
"target"_a,
615 "max_correspondence_distance"_a,
616 "init"_a = Eigen::Matrix4d::Identity(),
617 "estimation_method"_a = TransformationEstimationPointToPoint(
false),
618 "criteria"_a = ICPConvergenceCriteria());
623 py::call_guard<py::gil_scoped_release>(),
624 "Function for Colored ICP registration",
"source"_a,
"target"_a,
625 "max_correspondence_distance"_a,
626 "init"_a = Eigen::Matrix4d::Identity(),
627 "estimation_method"_a = TransformationEstimationForColoredICP(),
628 "criteria"_a = ICPConvergenceCriteria());
633 "Function for Generalized ICP registration",
"source"_a,
"target"_a,
634 "max_correspondence_distance"_a,
635 "init"_a = Eigen::Matrix4d::Identity(),
636 "estimation_method"_a = TransformationEstimationForGeneralizedICP(),
637 "criteria"_a = ICPConvergenceCriteria());
641 m.def(
"registration_ransac_based_on_correspondence",
643 py::call_guard<py::gil_scoped_release>(),
644 "Function for global RANSAC registration based on a set of "
646 "source"_a,
"target"_a,
"corres"_a,
"max_correspondence_distance"_a,
647 "estimation_method"_a = TransformationEstimationPointToPoint(
false),
649 "checkers"_a = std::vector<
650 std::reference_wrapper<const CorrespondenceChecker>>(),
651 "criteria"_a = RANSACConvergenceCriteria(100000, 0.999),
652 "seed"_a = py::none());
654 "registration_ransac_based_on_correspondence",
657 m.def(
"registration_ransac_based_on_feature_matching",
659 py::call_guard<py::gil_scoped_release>(),
660 "Function for global RANSAC registration based on feature matching",
661 "source"_a,
"target"_a,
"source_feature"_a,
"target_feature"_a,
662 "mutual_filter"_a,
"max_correspondence_distance"_a,
663 "estimation_method"_a = TransformationEstimationPointToPoint(
false),
665 "checkers"_a = std::vector<
666 std::reference_wrapper<const CorrespondenceChecker>>(),
667 "criteria"_a = RANSACConvergenceCriteria(100000, 0.999),
668 "seed"_a = py::none());
670 m,
"registration_ransac_based_on_feature_matching",
673 m.def(
"registration_fgr_based_on_correspondence",
675 py::call_guard<py::gil_scoped_release>(),
676 "Function for fast global registration based on a set of "
678 "source"_a,
"target"_a,
"corres"_a,
679 "option"_a = FastGlobalRegistrationOption());
683 m.def(
"registration_fgr_based_on_feature_matching",
685 py::call_guard<py::gil_scoped_release>(),
686 "Function for fast global registration based on feature matching",
687 "source"_a,
"target"_a,
"source_feature"_a,
"target_feature"_a,
688 "option"_a = FastGlobalRegistrationOption());
690 "registration_fgr_based_on_feature_matching",
693 m.def(
"get_information_matrix_from_point_clouds",
695 py::call_guard<py::gil_scoped_release>(),
696 "Function for computing information matrix from transformation "
698 "source"_a,
"target"_a,
"max_correspondence_distance"_a,
705 py::module m_submodule =
706 m.def_submodule(
"registration",
"Registration pipeline.");
filament::Texture::InternalFormat format
A 3D cloud and its associated features (color, normals, scalar fields, etc.)
Check if two aligned point clouds are close.
double distance_threshold_
Distance threashold for the check.
Check if two point clouds build the polygons with similar edge lengths.
Class to check if two aligned point clouds have similar normals.
Base class that checks if two (small) point clouds can be aligned.
bool require_pointcloud_alignment_
virtual bool Check(const ccPointCloud &source, const ccPointCloud &target, const CorrespondenceSet &corres, const Eigen::Matrix4d &transformation) const =0
Function to check if two points can be aligned.
Options for FastGlobalRegistration.
int maximum_tuple_count_
Maximum number of tuples..
int iteration_number_
Maximum number of iterations.
double tuple_scale_
Similarity measure used for tuples of feature points.
double division_factor_
Division factor used for graduated non-convexity.
Class that defines the convergence criteria of ICP.
int max_iteration_
Maximum iteration before iteration stops.
bool Check(const ccPointCloud &source, const ccPointCloud &target, const CorrespondenceSet &corres, const Eigen::Matrix4d &transformation) const override
Function to check if two points can be aligned.
Class that defines the convergence criteria of RANSAC.
int max_iteration_
Maximum iteration before iteration stops.
double confidence_
Desired probability of success.
double inlier_rmse_
RMSE of all inlier correspondences. Lower is better.
Eigen::Matrix4d_u transformation_
The estimated transformation matrix.
CorrespondenceSet correspondence_set_
Correspondence set between source and target point cloud.
void ClassMethodDocInject(py::module &pybind_module, const std::string &class_name, const std::string &function_name, const std::unordered_map< std::string, std::string > &map_parameter_body_docs)
void FunctionDocInject(py::module &pybind_module, const std::string &function_name, const std::unordered_map< std::string, std::string > &map_parameter_body_docs)
void pybind_registration_classes(py::module &m)
void pybind_global_optimization(py::module &m)
void pybind_global_optimization_methods(py::module &m)
RegistrationResult RegistrationRANSACBasedOnCorrespondence(const ccPointCloud &source, const ccPointCloud &target, const CorrespondenceSet &corres, double max_correspondence_distance, const TransformationEstimation &estimation, int ransac_n, const std::vector< std::reference_wrapper< const CorrespondenceChecker >> &checkers, const RANSACConvergenceCriteria &criteria, utility::optional< unsigned int > seed)
Function for global RANSAC registration based on a given set of correspondences.
RegistrationResult RegistrationGeneralizedICP(const ccPointCloud &source, const ccPointCloud &target, double max_correspondence_distance, const Eigen::Matrix4d &init, const TransformationEstimationForGeneralizedICP &estimation, const ICPConvergenceCriteria &criteria)
Function for Generalized ICP registration.
void pybind_robust_kernels(py::module &m)
Eigen::Matrix6d GetInformationMatrixFromPointClouds(const ccPointCloud &source, const ccPointCloud &target, double max_correspondence_distance, const Eigen::Matrix4d &transformation)
std::vector< Eigen::Vector2i > CorrespondenceSet
RegistrationResult RegistrationColoredICP(const ccPointCloud &source, const ccPointCloud &target, double max_distance, const Eigen::Matrix4d &init, const TransformationEstimationForColoredICP &estimation, const ICPConvergenceCriteria &criteria)
Function for Colored ICP registration.
RegistrationResult FastGlobalRegistrationBasedOnFeatureMatching(const ccPointCloud &source, const ccPointCloud &target, const utility::Feature &source_feature, const utility::Feature &target_feature, const FastGlobalRegistrationOption &option)
Fast Global Registration based on a given set of FPFH features.
void pybind_feature(py::module &m)
RegistrationResult RegistrationICP(const ccPointCloud &source, const ccPointCloud &target, double max_correspondence_distance, const Eigen::Matrix4d &init, const TransformationEstimation &estimation, const ICPConvergenceCriteria &criteria)
Functions for ICP registration.
static const std::unordered_map< std::string, std::string > map_shared_argument_docstrings
void pybind_registration_methods(py::module &m)
void pybind_registration(py::module &m)
void pybind_feature_methods(py::module &m)
TransformationEstimationType
RegistrationResult RegistrationRANSACBasedOnFeatureMatching(const ccPointCloud &source, const ccPointCloud &target, const utility::Feature &source_feature, const utility::Feature &target_feature, bool mutual_filter, double max_correspondence_distance, const TransformationEstimation &estimation, int ransac_n, const std::vector< std::reference_wrapper< const CorrespondenceChecker >> &checkers, const RANSACConvergenceCriteria &criteria, utility::optional< unsigned int > seed)
Function for global RANSAC registration based on feature matching.
RegistrationResult FastGlobalRegistrationBasedOnCorrespondence(const ccPointCloud &source, const ccPointCloud &target, const CorrespondenceSet &corres, const FastGlobalRegistrationOption &option)
Fast Global Registration based on a given set of correspondences.
RegistrationResult EvaluateRegistration(const ccPointCloud &source, const ccPointCloud &target, double max_correspondence_distance, const Eigen::Matrix4d &transformation)
Function for evaluating registration between point clouds.
Generic file read and write utility for python interface.
std::string to_string(const T &n)
void bind_default_constructor(Class_ &cl)