Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
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>
43namespace 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 */
52template <typename PointSource, typename PointTarget, typename Scalar = float>
54: public IterativeClosestPoint<PointSource, PointTarget, Scalar> {
55public:
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
73
76
77 using Ptr = shared_ptr<JointIterativeClosestPoint<PointSource, PointTarget, Scalar>>;
78 using ConstPtr =
79 shared_ptr<const JointIterativeClosestPoint<PointSource, PointTarget, Scalar>>;
80
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>::
96 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::final_transformation_;
97 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformation_;
98 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
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>::
105 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::update_visualizer_;
106 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
108 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::correspondences_;
109 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
111 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
113 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
115
116 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
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 =
127
128 /** \brief Empty constructor. */
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
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
226
227protected:
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:97
typename Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition icp.h:142
bool use_reciprocal_correspondence_
The correspondence type used for correspondence estimation.
Definition icp.h:305
bool need_source_blob_
Checks for whether estimators and rejectors need various data.
Definition icp.h:313
pcl::registration::DefaultConvergenceCriteria< Scalar >::Ptr convergence_criteria_
Definition icp.h:141
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:240
bool source_has_normals_
Internal check whether source dataset has normals or not.
Definition icp.h:308
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:207
bool target_has_normals_
Internal check whether target dataset has normals or not.
Definition icp.h:310
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...
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
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
std::function< UpdateVisualizerCallbackSignature > update_visualizer_
Callback function to update intermediate source point cloud position during it's registration to the ...
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
std::string reg_name_
The registration method name.
Matrix4 transformation_
The transformation matrix estimated by the registration method.
CorrespondenceEstimationPtr correspondence_estimation_
A CorrespondenceEstimation object, used to estimate correspondences between the source and the target...
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
Matrix4 previous_transformation_
The previous transformation matrix estimated by the registration method (used internally).
bool converged_
Holds internal convergence state, given user parameters.
CorrespondencesPtr correspondences_
The set of correspondences determined at this ICP step.
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
unsigned int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
double euclidean_fitness_epsilon_
The maximum allowed Euclidean error between two consecutive steps in the ICP loop,...
double inlier_threshold_
The inlier distance threshold for the internal RANSAC outlier rejection loop.
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
std::vector< CorrespondenceRejectorPtr > correspondence_rejectors_
The list of correspondence rejectors to use.
PointCloudTargetConstPtr target_
The input point cloud dataset target.
const std::string & getClassName() const
Abstract class get name method.
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
shared_ptr< const ::pcl::PointIndices > ConstPtr