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>
53 class JointIterativeClosestPoint
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 
68   using KdTree = pcl::search::KdTree<PointTarget>;
69   using KdTreePtr = typename KdTree::Ptr;
70 
71   using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
72   using KdTreeReciprocalPtr = typename KdTree::Ptr;
73 
74   using PointIndicesPtr = PointIndices::Ptr;
75   using PointIndicesConstPtr = PointIndices::ConstPtr;
76 
77   using Ptr = shared_ptr<JointIterativeClosestPoint<PointSource, PointTarget, Scalar>>;
78   using ConstPtr =
79       shared_ptr<const JointIterativeClosestPoint<PointSource, PointTarget, Scalar>>;
80 
81   using CorrespondenceEstimation =
82       pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>;
83   using CorrespondenceEstimationPtr = typename CorrespondenceEstimation::Ptr;
84   using CorrespondenceEstimationConstPtr = typename CorrespondenceEstimation::ConstPtr;
85 
86   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_;
87   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::getClassName;
88   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputSource;
89   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::input_;
90   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::indices_;
91   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::target_;
92   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::nr_iterations_;
93   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::max_iterations_;
94   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
95       previous_transformation_;
96   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::final_transformation_;
97   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformation_;
98   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
99       transformation_epsilon_;
100   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::converged_;
101   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
102   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::inlier_threshold_;
103   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
104       min_number_correspondences_;
105   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::update_visualizer_;
106   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
107       euclidean_fitness_epsilon_;
108   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::correspondences_;
109   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
110       transformation_estimation_;
111   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
112       correspondence_estimation_;
113   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
114       correspondence_rejectors_;
115 
116   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
117       use_reciprocal_correspondence_;
118 
119   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::convergence_criteria_;
120   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::source_has_normals_;
121   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::target_has_normals_;
122   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::need_source_blob_;
123   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::need_target_blob_;
124 
125   using Matrix4 =
126       typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4;
127 
128   /** \brief Empty constructor. */
JointIterativeClosestPoint()129   JointIterativeClosestPoint()
130   {
131     IterativeClosestPoint<PointSource, PointTarget, Scalar>();
132     reg_name_ = "JointIterativeClosestPoint";
133   };
134 
135   /** \brief Empty destructor */
~JointIterativeClosestPoint()136   ~JointIterativeClosestPoint() {}
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
setInputSource(const PointCloudSourceConstPtr &)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
addInputSource(const PointCloudSourceConstPtr & cloud)155   addInputSource(const PointCloudSourceConstPtr& cloud)
156   {
157     // Set the parent InputSource, just to get all cached values (e.g. the existence of
158     // normals).
159     if (sources_.empty())
160       IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputSource(cloud);
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
setInputTarget(const PointCloudTargetConstPtr &)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
addInputTarget(const PointCloudTargetConstPtr & cloud)181   addInputTarget(const PointCloudTargetConstPtr& cloud)
182   {
183     // Set the parent InputTarget, just to get all cached values (e.g. the existence of
184     // normals).
185     if (targets_.empty())
186       IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
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
addCorrespondenceEstimation(CorrespondenceEstimationPtr ce)198   addCorrespondenceEstimation(CorrespondenceEstimationPtr ce)
199   {
200     correspondence_estimations_.push_back(ce);
201   }
202 
203   /** \brief Reset my list of input sources
204    */
205   inline void
clearInputSources()206   clearInputSources()
207   {
208     sources_.clear();
209   }
210 
211   /** \brief Reset my list of input targets
212    */
213   inline void
clearInputTargets()214   clearInputTargets()
215   {
216     targets_.clear();
217   }
218 
219   /** \brief Reset my list of correspondence estimation methods.
220    */
221   inline void
clearCorrespondenceEstimations()222   clearCorrespondenceEstimations()
223   {
224     correspondence_estimations_.clear();
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>
249