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