Open3D (C++ API)  0.12.0
Registration.h
Go to the documentation of this file.
1 // ----------------------------------------------------------------------------
2 // - Open3D: www.open3d.org -
3 // ----------------------------------------------------------------------------
4 // The MIT License (MIT)
5 //
6 // Copyright (c) 2018 www.open3d.org
7 //
8 // Permission is hereby granted, free of charge, to any person obtaining a copy
9 // of this software and associated documentation files (the "Software"), to deal
10 // in the Software without restriction, including without limitation the rights
11 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12 // copies of the Software, and to permit persons to whom the Software is
13 // furnished to do so, subject to the following conditions:
14 //
15 // The above copyright notice and this permission notice shall be included in
16 // all copies or substantial portions of the Software.
17 //
18 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
23 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
24 // IN THE SOFTWARE.
25 // ----------------------------------------------------------------------------
26 
27 #pragma once
28 
29 #include <Eigen/Core>
30 #include <tuple>
31 #include <vector>
32 
35 #include "open3d/utility/Eigen.h"
36 
37 namespace open3d {
38 
39 namespace geometry {
40 class PointCloud;
41 }
42 
43 namespace pipelines {
44 namespace registration {
45 class Feature;
46 
55 public:
63  ICPConvergenceCriteria(double relative_fitness = 1e-6,
64  double relative_rmse = 1e-6,
65  int max_iteration = 30)
66  : relative_fitness_(relative_fitness),
67  relative_rmse_(relative_rmse),
68  max_iteration_(max_iteration) {}
70 
71 public:
80 };
81 
92 public:
99  RANSACConvergenceCriteria(int max_iteration = 100000,
100  double confidence = 0.999)
101  : max_iteration_(max_iteration), confidence_(confidence) {}
102 
104 
105 public:
109  double confidence_;
110 };
111 
116 public:
121  const Eigen::Matrix4d &transformation = Eigen::Matrix4d::Identity())
122  : transformation_(transformation), inlier_rmse_(0.0), fitness_(0.0) {}
124  bool IsBetterRANSACThan(const RegistrationResult &other) const {
125  return fitness_ > other.fitness_ || (fitness_ == other.fitness_ &&
126  inlier_rmse_ < other.inlier_rmse_);
127  }
128 
129 public:
131  Eigen::Matrix4d_u transformation_;
135  double inlier_rmse_;
140  double fitness_;
141 };
142 
152  const geometry::PointCloud &source,
153  const geometry::PointCloud &target,
154  double max_correspondence_distance,
155  const Eigen::Matrix4d &transformation = Eigen::Matrix4d::Identity());
156 
168  const geometry::PointCloud &source,
169  const geometry::PointCloud &target,
170  double max_correspondence_distance,
171  const Eigen::Matrix4d &init = Eigen::Matrix4d::Identity(),
172  const TransformationEstimation &estimation =
174  const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria());
175 
189  const geometry::PointCloud &source,
190  const geometry::PointCloud &target,
191  const CorrespondenceSet &corres,
192  double max_correspondence_distance,
193  const TransformationEstimation &estimation =
195  int ransac_n = 3,
196  const std::vector<std::reference_wrapper<const CorrespondenceChecker>>
197  &checkers = {},
198  const RANSACConvergenceCriteria &criteria =
200 
215  const geometry::PointCloud &source,
216  const geometry::PointCloud &target,
217  const Feature &source_feature,
218  const Feature &target_feature,
219  bool mutual_filter,
220  double max_correspondence_distance,
221  const TransformationEstimation &estimation =
223  int ransac_n = 3,
224  const std::vector<std::reference_wrapper<const CorrespondenceChecker>>
225  &checkers = {},
226  const RANSACConvergenceCriteria &criteria =
228 
235  const geometry::PointCloud &source,
236  const geometry::PointCloud &target,
237  double max_correspondence_distance,
238  const Eigen::Matrix4d &transformation);
239 
240 } // namespace registration
241 } // namespace pipelines
242 } // namespace open3d
Class that defines the convergence criteria of RANSAC.
Definition: Registration.h:91
int max_iteration_
Maximum iteration before iteration stops.
Definition: Registration.h:107
Class that defines the convergence criteria of ICP.
Definition: Registration.h:54
RegistrationResult RegistrationRANSACBasedOnCorrespondence(const geometry::PointCloud &source, const geometry::PointCloud &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)
Function for global RANSAC registration based on a given set of correspondences.
Definition: Registration.cpp:189
bool IsBetterRANSACThan(const RegistrationResult &other) const
Definition: Registration.h:124
RegistrationResult(const Eigen::Matrix4d &transformation=Eigen::Matrix4d::Identity())
Parameterized Constructor.
Definition: Registration.h:120
A point cloud consists of point coordinates, and optionally point colors and point normals...
Definition: PointCloud.h:54
std::vector< Eigen::Vector2i > CorrespondenceSet
Definition: TransformationEstimation.h:46
RegistrationResult EvaluateRegistration(const geometry::PointCloud &source, const geometry::PointCloud &target, double max_correspondence_distance, const Eigen::Matrix4d &transformation)
Function for evaluating registration between point clouds.
Definition: Registration.cpp:118
double relative_fitness_
Definition: Registration.h:74
Class to store featrues for registration.
Definition: Feature.h:47
RANSACConvergenceCriteria(int max_iteration=100000, double confidence=0.999)
Parameterized Constructor.
Definition: Registration.h:99
Definition: PinholeCameraIntrinsic.cpp:35
int max_iteration_
Maximum iteration before iteration stops.
Definition: Registration.h:79
double inlier_rmse_
RMSE of all inlier correspondences. Lower is better.
Definition: Registration.h:135
RegistrationResult RegistrationRANSACBasedOnFeatureMatching(const geometry::PointCloud &source, const geometry::PointCloud &target, const Feature &source_feature, const 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)
Function for global RANSAC registration based on feature matching.
Definition: Registration.cpp:275
~RegistrationResult()
Definition: Registration.h:123
Eigen::Matrix4d_u transformation_
The estimated transformation matrix.
Definition: Registration.h:131
ICPConvergenceCriteria(double relative_fitness=1e-6, double relative_rmse=1e-6, int max_iteration=30)
Parameterized Constructor.
Definition: Registration.h:63
Eigen::Matrix6d GetInformationMatrixFromPointClouds(const geometry::PointCloud &source, const geometry::PointCloud &target, double max_correspondence_distance, const Eigen::Matrix4d &transformation)
Definition: Registration.cpp:352
RegistrationResult RegistrationICP(const geometry::PointCloud &source, const geometry::PointCloud &target, double max_correspondence_distance, const Eigen::Matrix4d &init, const TransformationEstimation &estimation, const ICPConvergenceCriteria &criteria)
Functions for ICP registration.
Definition: Registration.cpp:134
~RANSACConvergenceCriteria()
Definition: Registration.h:103
Definition: TransformationEstimation.h:60
double confidence_
Desired probability of success.
Definition: Registration.h:109
double relative_rmse_
Definition: Registration.h:77
~ICPConvergenceCriteria()
Definition: Registration.h:69
double fitness_
Definition: Registration.h:140
CorrespondenceSet correspondence_set_
Correspondence set between source and target point cloud.
Definition: Registration.h:133