Point Cloud Library (PCL)  1.10.1
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 {
45  /** \brief @b JointIterativeClosestPoint extends ICP to multiple frames which
46  * share the same transform. This is particularly useful when solving for
47  * camera extrinsics using multiple observations. When given a single pair of
48  * clouds, this reduces to vanilla ICP.
49  *
50  * \author Stephen Miller
51  * \ingroup registration
52  */
53  template <typename PointSource, typename PointTarget, typename Scalar = float>
54  class JointIterativeClosestPoint : public IterativeClosestPoint<PointSource, PointTarget, Scalar>
55  {
56  public:
58  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
59  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
60 
62  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
63  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
64 
66  using KdTreePtr = typename KdTree::Ptr;
67 
69  using KdTreeReciprocalPtr = typename KdTree::Ptr;
70 
71 
74 
77 
81 
82 
105 
107 
113 
114 
116 
117  /** \brief Empty constructor. */
119  {
121  reg_name_ = "JointIterativeClosestPoint";
122  };
123 
124  /** \brief Empty destructor */
126 
127 
128  /** \brief Provide a pointer to the input source
129  * (e.g., the point cloud that we want to align to the target)
130  */
131  void
132  setInputSource (const PointCloudSourceConstPtr& /*cloud*/) override
133  {
134  PCL_WARN ("[pcl::%s::setInputSource] Warning; JointIterativeClosestPoint expects multiple clouds. Please use addInputSource.",
135  getClassName ().c_str ());
136  return;
137  }
138 
139  /** \brief Add a source cloud to the joint solver
140  *
141  * \param[in] cloud source cloud
142  */
143  inline void
145  {
146  // Set the parent InputSource, just to get all cached values (e.g. the existence of normals).
147  if (sources_.empty ())
149  sources_.push_back (cloud);
150  }
151 
152  /** \brief Provide a pointer to the input target
153  * (e.g., the point cloud that we want to align to the target)
154  */
155  void
156  setInputTarget (const PointCloudTargetConstPtr& /*cloud*/) override
157  {
158  PCL_WARN ("[pcl::%s::setInputTarget] Warning; JointIterativeClosestPoint expects multiple clouds. Please use addInputTarget.",
159  getClassName ().c_str ());
160  return;
161  }
162 
163  /** \brief Add a target cloud to the joint solver
164  *
165  * \param[in] cloud target cloud
166  */
167  inline void
169  {
170  // Set the parent InputTarget, just to get all cached values (e.g. the existence of normals).
171  if (targets_.empty ())
173  targets_.push_back (cloud);
174  }
175 
176  /** \brief Add a manual correspondence estimator
177  * If you choose to do this, you must add one for each
178  * input source / target pair. They do not need to have trees
179  * or input clouds set ahead of time.
180  *
181  * \param[in] ce Correspondence estimation
182  */
183  inline void
185  {
186  correspondence_estimations_.push_back (ce);
187  }
188 
189  /** \brief Reset my list of input sources
190  */
191  inline void
193  { sources_.clear (); }
194 
195  /** \brief Reset my list of input targets
196  */
197  inline void
199  { targets_.clear (); }
200 
201  /** \brief Reset my list of correspondence estimation methods.
202  */
203  inline void
205  { correspondence_estimations_.clear (); }
206 
207 
208  protected:
209 
210  /** \brief Rigid transformation computation method with initial guess.
211  * \param output the transformed input point cloud dataset using the rigid transformation found
212  * \param guess the initial guess of the transformation to compute
213  */
214  void
215  computeTransformation (PointCloudSource &output, const Matrix4 &guess) override;
216 
217  /** \brief Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be called */
218  void
219  determineRequiredBlobData () override;
220 
221  std::vector<PointCloudSourceConstPtr> sources_;
222  std::vector<PointCloudTargetConstPtr> targets_;
223  std::vector<CorrespondenceEstimationPtr> correspondence_estimations_;
224  };
225 
226 }
227 
228 #include <pcl/registration/impl/joint_icp.hpp>
pcl::IterativeClosestPoint::Ptr
shared_ptr< IterativeClosestPoint< PointSource, PointTarget, Scalar > > Ptr
Definition: icp.h:109
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::JointIterativeClosestPoint::setInputSource
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:132
pcl::JointIterativeClosestPoint::~JointIterativeClosestPoint
~JointIterativeClosestPoint()
Empty destructor.
Definition: joint_icp.h:125
pcl::JointIterativeClosestPoint::targets_
std::vector< PointCloudTargetConstPtr > targets_
Definition: joint_icp.h:222
pcl::JointIterativeClosestPoint::clearCorrespondenceEstimations
void clearCorrespondenceEstimations()
Reset my list of correspondence estimation methods.
Definition: joint_icp.h:204
pcl::IterativeClosestPoint::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition: icp.h:107
pcl::IterativeClosestPoint::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: icp.h:99
pcl::IterativeClosestPoint
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
Definition: icp.h:95
pcl::Registration::getClassName
const std::string & getClassName() const
Abstract class get name method.
Definition: registration.h:429
pcl::IterativeClosestPoint::PointCloudTarget
typename Registration< PointSource, PointTarget, Scalar >::PointCloudTarget PointCloudTarget
Definition: icp.h:102
pcl::registration::CorrespondenceEstimationBase
Abstract CorrespondenceEstimationBase class.
Definition: correspondence_estimation.h:62
pcl::JointIterativeClosestPoint::JointIterativeClosestPoint
JointIterativeClosestPoint()
Empty constructor.
Definition: joint_icp.h:118
pcl::IterativeClosestPoint::PointCloudSource
typename Registration< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition: icp.h:98
pcl::search::KdTree::Ptr
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:78
pcl::JointIterativeClosestPoint::KdTreeReciprocalPtr
typename KdTree::Ptr KdTreeReciprocalPtr
Definition: joint_icp.h:69
pcl::PointIndices::ConstPtr
shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:23
pcl::registration::CorrespondenceEstimationBase::Ptr
shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > Ptr
Definition: correspondence_estimation.h:65
pcl::registration::CorrespondenceEstimationBase::ConstPtr
shared_ptr< const CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > ConstPtr
Definition: correspondence_estimation.h:66
pcl::JointIterativeClosestPoint::computeTransformation
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
Definition: joint_icp.hpp:49
pcl::JointIterativeClosestPoint::sources_
std::vector< PointCloudSourceConstPtr > sources_
Definition: joint_icp.h:221
pcl::JointIterativeClosestPoint::setInputTarget
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:156
pcl::JointIterativeClosestPoint::addInputSource
void addInputSource(const PointCloudSourceConstPtr &cloud)
Add a source cloud to the joint solver.
Definition: joint_icp.h:144
pcl::JointIterativeClosestPoint::determineRequiredBlobData
void determineRequiredBlobData() override
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be cal...
Definition: joint_icp.hpp:283
pcl::JointIterativeClosestPoint::addInputTarget
void addInputTarget(const PointCloudTargetConstPtr &cloud)
Add a target cloud to the joint solver.
Definition: joint_icp.h:168
pcl::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:61
pcl::JointIterativeClosestPoint::clearInputTargets
void clearInputTargets()
Reset my list of input targets.
Definition: joint_icp.h:198
pcl::JointIterativeClosestPoint::addCorrespondenceEstimation
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:184
pcl::JointIterativeClosestPoint::PointCloudSource
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition: joint_icp.h:57
pcl::PointIndices::Ptr
shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:22
pcl::IterativeClosestPoint::PointIndicesPtr
PointIndices::Ptr PointIndicesPtr
Definition: icp.h:106
pcl::JointIterativeClosestPoint
JointIterativeClosestPoint extends ICP to multiple frames which share the same transform.
Definition: joint_icp.h:54
pcl::IterativeClosestPoint::PointCloudTargetPtr
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition: icp.h:103
pcl::IterativeClosestPoint::Matrix4
typename Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: icp.h:136
pcl::JointIterativeClosestPoint::KdTreePtr
typename KdTree::Ptr KdTreePtr
Definition: joint_icp.h:66
pcl::JointIterativeClosestPoint::correspondence_estimations_
std::vector< CorrespondenceEstimationPtr > correspondence_estimations_
Definition: joint_icp.h:223
pcl::IterativeClosestPoint::ConstPtr
shared_ptr< const IterativeClosestPoint< PointSource, PointTarget, Scalar > > ConstPtr
Definition: icp.h:110
pcl::JointIterativeClosestPoint::PointCloudTargetConstPtr
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: joint_icp.h:63
pcl::JointIterativeClosestPoint::CorrespondenceEstimationConstPtr
typename CorrespondenceEstimation::ConstPtr CorrespondenceEstimationConstPtr
Definition: joint_icp.h:80
pcl::JointIterativeClosestPoint::clearInputSources
void clearInputSources()
Reset my list of input sources.
Definition: joint_icp.h:192
pcl::JointIterativeClosestPoint::CorrespondenceEstimationPtr
typename CorrespondenceEstimation::Ptr CorrespondenceEstimationPtr
Definition: joint_icp.h:79
pcl::JointIterativeClosestPoint::PointCloudSourceConstPtr
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: joint_icp.h:59
pcl::Registration::reg_name_
std::string reg_name_
The registration method name.
Definition: registration.h:489
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:108
pcl::JointIterativeClosestPoint::Matrix4
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: joint_icp.h:115