Point Cloud Library (PCL)  1.14.1-dev
joint_icp.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #pragma once
40 
41 // PCL includes
42 #include <pcl/registration/icp.h>
43 namespace pcl {
44 /** \brief @b JointIterativeClosestPoint extends ICP to multiple frames which
45  * share the same transform. This is particularly useful when solving for
46  * camera extrinsics using multiple observations. When given a single pair of
47  * clouds, this reduces to vanilla ICP.
48  *
49  * \author Stephen Miller
50  * \ingroup registration
51  */
52 template <typename PointSource, typename PointTarget, typename Scalar = float>
54 : public IterativeClosestPoint<PointSource, PointTarget, Scalar> {
55 public:
56  using PointCloudSource = typename IterativeClosestPoint<PointSource,
57  PointTarget,
58  Scalar>::PointCloudSource;
59  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
60  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
61 
62  using PointCloudTarget = typename IterativeClosestPoint<PointSource,
63  PointTarget,
64  Scalar>::PointCloudTarget;
65  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
66  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
67 
69  using KdTreePtr = typename KdTree::Ptr;
70 
72  using KdTreeReciprocalPtr = typename KdTree::Ptr;
73 
76 
77  using Ptr = shared_ptr<JointIterativeClosestPoint<PointSource, PointTarget, Scalar>>;
78  using ConstPtr =
79  shared_ptr<const JointIterativeClosestPoint<PointSource, PointTarget, Scalar>>;
80 
85 
115 
118 
124 
125  using Matrix4 =
127 
128  /** \brief Empty constructor. */
130  {
132  reg_name_ = "JointIterativeClosestPoint";
133  };
134 
135  /** \brief Empty destructor */
136  ~JointIterativeClosestPoint() override = default;
137 
138  /** \brief Provide a pointer to the input source
139  * (e.g., the point cloud that we want to align to the target)
140  */
141  void
142  setInputSource(const PointCloudSourceConstPtr& /*cloud*/) override
143  {
144  PCL_WARN("[pcl::%s::setInputSource] Warning; JointIterativeClosestPoint expects "
145  "multiple clouds. Please use addInputSource.\n",
146  getClassName().c_str());
147  return;
148  }
149 
150  /** \brief Add a source cloud to the joint solver
151  *
152  * \param[in] cloud source cloud
153  */
154  inline void
156  {
157  // Set the parent InputSource, just to get all cached values (e.g. the existence of
158  // normals).
159  if (sources_.empty())
161  sources_.push_back(cloud);
162  }
163 
164  /** \brief Provide a pointer to the input target
165  * (e.g., the point cloud that we want to align to the target)
166  */
167  void
168  setInputTarget(const PointCloudTargetConstPtr& /*cloud*/) override
169  {
170  PCL_WARN("[pcl::%s::setInputTarget] Warning; JointIterativeClosestPoint expects "
171  "multiple clouds. Please use addInputTarget.\n",
172  getClassName().c_str());
173  return;
174  }
175 
176  /** \brief Add a target cloud to the joint solver
177  *
178  * \param[in] cloud target cloud
179  */
180  inline void
182  {
183  // Set the parent InputTarget, just to get all cached values (e.g. the existence of
184  // normals).
185  if (targets_.empty())
187  targets_.push_back(cloud);
188  }
189 
190  /** \brief Add a manual correspondence estimator
191  * If you choose to do this, you must add one for each
192  * input source / target pair. They do not need to have trees
193  * or input clouds set ahead of time.
194  *
195  * \param[in] ce Correspondence estimation
196  */
197  inline void
199  {
200  correspondence_estimations_.push_back(ce);
201  }
202 
203  /** \brief Reset my list of input sources
204  */
205  inline void
207  {
208  sources_.clear();
209  }
210 
211  /** \brief Reset my list of input targets
212  */
213  inline void
215  {
216  targets_.clear();
217  }
218 
219  /** \brief Reset my list of correspondence estimation methods.
220  */
221  inline void
223  {
225  }
226 
227 protected:
228  /** \brief Rigid transformation computation method with initial guess.
229  * \param output the transformed input point cloud dataset using the rigid
230  * transformation found \param guess the initial guess of the transformation to
231  * compute
232  */
233  void
234  computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
235 
236  /** \brief Looks at the Estimators and Rejectors and determines whether their
237  * blob-setter methods need to be called */
238  void
239  determineRequiredBlobData() override;
240 
241  std::vector<PointCloudSourceConstPtr> sources_;
242  std::vector<PointCloudTargetConstPtr> targets_;
243  std::vector<CorrespondenceEstimationPtr> correspondence_estimations_;
244 };
245 
246 } // namespace pcl
247 
248 #include <pcl/registration/impl/joint_icp.hpp>
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
Definition: icp.h:98
typename Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: icp.h:143
bool use_reciprocal_correspondence_
The correspondence type used for correspondence estimation.
Definition: icp.h:307
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
Definition: icp.h:232
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
Definition: icp.h:199
JointIterativeClosestPoint extends ICP to multiple frames which share the same transform.
Definition: joint_icp.h:54
std::vector< PointCloudTargetConstPtr > targets_
Definition: joint_icp.h:242
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: joint_icp.h:59
void clearCorrespondenceEstimations()
Reset my list of correspondence estimation methods.
Definition: joint_icp.h:222
shared_ptr< const JointIterativeClosestPoint< PointSource, PointTarget, Scalar > > ConstPtr
Definition: joint_icp.h:79
void addInputSource(const PointCloudSourceConstPtr &cloud)
Add a source cloud to the joint solver.
Definition: joint_icp.h:155
void determineRequiredBlobData() override
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be cal...
Definition: joint_icp.hpp:295
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudTarget PointCloudTarget
Definition: joint_icp.h:64
typename KdTree::Ptr KdTreeReciprocalPtr
Definition: joint_icp.h:72
~JointIterativeClosestPoint() override=default
Empty destructor.
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
Definition: joint_icp.hpp:49
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: joint_icp.h:66
PointIndices::Ptr PointIndicesPtr
Definition: joint_icp.h:74
void clearInputSources()
Reset my list of input sources.
Definition: joint_icp.h:206
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition: joint_icp.h:65
std::vector< PointCloudSourceConstPtr > sources_
Definition: joint_icp.h:241
typename CorrespondenceEstimation::Ptr CorrespondenceEstimationPtr
Definition: joint_icp.h:83
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition: joint_icp.h:58
void addCorrespondenceEstimation(CorrespondenceEstimationPtr ce)
Add a manual correspondence estimator If you choose to do this, you must add one for each input sourc...
Definition: joint_icp.h:198
PointIndices::ConstPtr PointIndicesConstPtr
Definition: joint_icp.h:75
JointIterativeClosestPoint()
Empty constructor.
Definition: joint_icp.h:129
void setInputTarget(const PointCloudTargetConstPtr &) override
Provide a pointer to the input target (e.g., the point cloud that we want to align to the target)
Definition: joint_icp.h:168
typename CorrespondenceEstimation::ConstPtr CorrespondenceEstimationConstPtr
Definition: joint_icp.h:84
shared_ptr< JointIterativeClosestPoint< PointSource, PointTarget, Scalar > > Ptr
Definition: joint_icp.h:77
typename KdTree::Ptr KdTreePtr
Definition: joint_icp.h:69
std::vector< CorrespondenceEstimationPtr > correspondence_estimations_
Definition: joint_icp.h:243
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: joint_icp.h:60
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: joint_icp.h:126
void clearInputTargets()
Reset my list of input targets.
Definition: joint_icp.h:214
void setInputSource(const PointCloudSourceConstPtr &) override
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
Definition: joint_icp.h:142
void addInputTarget(const PointCloudTargetConstPtr &cloud)
Add a target cloud to the joint solver.
Definition: joint_icp.h:181
std::string reg_name_
The registration method name.
Definition: registration.h:548
const std::string & getClassName() const
Abstract class get name method.
Definition: registration.h:485
CorrespondenceEstimationPtr correspondence_estimation_
A CorrespondenceEstimation object, used to estimate correspondences between the source and the target...
Definition: registration.h:629
Matrix4 previous_transformation_
The previous transformation matrix estimated by the registration method (used internally).
Definition: registration.h:580
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
Definition: registration.h:625
unsigned int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
Definition: registration.h:618
double euclidean_fitness_epsilon_
The maximum allowed Euclidean error between two consecutive steps in the ICP loop,...
Definition: registration.h:597
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
Definition: registration.h:585
std::vector< CorrespondenceRejectorPtr > correspondence_rejectors_
The list of correspondence rejectors to use.
Definition: registration.h:632
Abstract CorrespondenceEstimationBase class.
shared_ptr< const CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > ConstPtr
shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > Ptr
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:13
shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:14