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