From 7a26ce7921cc383ac073276015b96e0bbef4fd6e Mon Sep 17 00:00:00 2001 From: Stephen Miller Date: Sat, 2 Nov 2013 17:24:44 +0900 Subject: [PATCH 1/3] Initial push of JointICP --- registration/CMakeLists.txt | 3 + .../pcl/registration/impl/joint_icp.hpp | 259 ++++++++++++++++++ .../include/pcl/registration/joint_icp.h | 231 ++++++++++++++++ test/registration/test_registration.cpp | 64 +++++ 4 files changed, 557 insertions(+) create mode 100644 registration/include/pcl/registration/impl/joint_icp.hpp create mode 100644 registration/include/pcl/registration/joint_icp.h diff --git a/registration/CMakeLists.txt b/registration/CMakeLists.txt index bd94b5b2efa..d05862db992 100644 --- a/registration/CMakeLists.txt +++ b/registration/CMakeLists.txt @@ -35,6 +35,7 @@ if(build) include/pcl/${SUBSYS_NAME}/correspondence_types.h include/pcl/${SUBSYS_NAME}/ia_ransac.h include/pcl/${SUBSYS_NAME}/icp.h + include/pcl/${SUBSYS_NAME}/joint_icp.h include/pcl/${SUBSYS_NAME}/icp_nl.h include/pcl/${SUBSYS_NAME}/lum.h include/pcl/${SUBSYS_NAME}/elch.h @@ -90,6 +91,7 @@ if(build) include/pcl/${SUBSYS_NAME}/impl/correspondence_types.hpp include/pcl/${SUBSYS_NAME}/impl/ia_ransac.hpp include/pcl/${SUBSYS_NAME}/impl/icp.hpp + include/pcl/${SUBSYS_NAME}/impl/joint_icp.hpp include/pcl/${SUBSYS_NAME}/impl/icp_nl.hpp include/pcl/${SUBSYS_NAME}/impl/elch.hpp include/pcl/${SUBSYS_NAME}/impl/lum.hpp @@ -134,6 +136,7 @@ if(build) #src/pairwise_graph_registration.cpp src/ia_ransac.cpp src/icp.cpp + src/joint_icp.cpp src/gicp.cpp src/icp_nl.cpp src/elch.cpp diff --git a/registration/include/pcl/registration/impl/joint_icp.hpp b/registration/include/pcl/registration/impl/joint_icp.hpp new file mode 100644 index 00000000000..cfe1448cf8c --- /dev/null +++ b/registration/include/pcl/registration/impl/joint_icp.hpp @@ -0,0 +1,259 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_ +#define PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_ + +#include +#include +#include + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::JointIterativeClosestPoint::computeTransformation ( + PointCloudSource &output, const Matrix4 &guess) +{ + // Point clouds containing the correspondences of each point in + if (sources_.size () != targets_.size () || sources_.empty () || targets_.empty ()) + { + PCL_ERROR ("[pcl::%s::computeTransformation] Must set InputSources and InputTargets to the same, nonzero size!\n", + getClassName ().c_str ()); + return; + } + bool manual_correspondence_estimations_set = true; + if (correspondence_estimations_.empty ()) + { + manual_correspondence_estimations_set = false; + correspondence_estimations_.resize (sources_.size ()); + for (size_t i = 0; i < sources_.size (); i++) + { + correspondence_estimations_[i].reset (new pcl::registration::CorrespondenceEstimation); + *correspondence_estimations_[i] = *correspondence_estimation_; + KdTreeReciprocalPtr src_tree (new KdTreeReciprocal); + KdTreePtr tgt_tree (new KdTree); + correspondence_estimations_[i]->setSearchMethodTarget (tgt_tree); + correspondence_estimations_[i]->setSearchMethodSource (src_tree); + } + } + if (correspondence_estimations_.size () != sources_.size ()) + { + PCL_ERROR ("[pcl::%s::computeTransform] Must set CorrespondenceEstimations to be the same size as the joint\n", + getClassName ().c_str ()); + return; + } + std::vector inputs_transformed (sources_.size ()); + for (size_t i = 0; i < sources_.size (); i++) + { + inputs_transformed[i].reset (new PointCloudSource); + } + + nr_iterations_ = 0; + converged_ = false; + + // Initialise final transformation to the guessed one + final_transformation_ = guess; + + // Make a combined transformed input and output + std::vector input_offsets (sources_.size ()); + std::vector target_offsets (targets_.size ()); + PointCloudSourcePtr sources_combined (new PointCloudSource); + PointCloudSourcePtr inputs_transformed_combined (new PointCloudSource); + PointCloudTargetPtr targets_combined (new PointCloudTarget); + size_t input_offset = 0; + size_t target_offset = 0; + for (size_t i = 0; i < sources_.size (); i++) + { + // If the guessed transformation is non identity + if (guess != Matrix4::Identity ()) + { + // Apply guessed transformation prior to search for neighbours + pcl::transformPointCloud (*sources_[i], *inputs_transformed[i], guess); + } + else + { + *inputs_transformed[i] = *sources_[i]; + } + *sources_combined += *sources_[i]; + *inputs_transformed_combined += *inputs_transformed[i]; + *targets_combined += *targets_[i]; + input_offsets[i] = input_offset; + target_offsets[i] = target_offset; + input_offset += inputs_transformed[i]->size (); + target_offset += targets_[i]->size (); + } + + + + transformation_ = Matrix4::Identity (); + // Pass in the default target for the Correspondence Estimation/Rejection code + for (size_t i = 0; i < sources_.size (); i++) + { + correspondence_estimations_[i]->setInputTarget (targets_[i]); + } + // We should be doing something like this + // for (size_t i = 0; i < correspondence_rejectors_.size (); ++i) + // { + // correspondence_rejectors_[i]->setTargetCloud (target_); + // if (target_has_normals_) + // correspondence_rejectors_[i]->setTargetNormals (target_); + // } + + convergence_criteria_->setMaximumIterations (max_iterations_); + convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_); + convergence_criteria_->setTranslationThreshold (transformation_epsilon_); + convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_); + + // Repeat until convergence + std::vector partial_correspondences_ (sources_.size ()); + for (size_t i = 0; i < sources_.size (); i++) + { + partial_correspondences_[i].reset (new pcl::Correspondences); + } + + do + { + // Save the previously estimated transformation + previous_transformation_ = transformation_; + + // Set the source each iteration, to ensure the dirty flag is updated + correspondences_->clear (); + for (size_t i = 0; i < correspondence_estimations_.size (); i++) + { + correspondence_estimations_[i]->setInputSource (inputs_transformed[i]); + + // Estimate correspondences on each cloud pair separately + if (use_reciprocal_correspondence_) + { + correspondence_estimations_[i]->determineReciprocalCorrespondences (*partial_correspondences_[i], corr_dist_threshold_); + } + else + { + correspondence_estimations_[i]->determineCorrespondences (*partial_correspondences_[i], corr_dist_threshold_); + } + PCL_DEBUG ("[pcl::%s::computeTransformation] Found %d partial correspondences for cloud [%d]\n", + getClassName ().c_str (), + partial_correspondences_[i]->size (), i); + for (size_t j = 0; j < partial_correspondences_[i]->size (); j++) + { + pcl::Correspondence corr = partial_correspondences_[i]->at (j); + // Update the offsets to be for the combined clouds + corr.index_query += input_offsets[i]; + corr.index_match += target_offsets[i]; + correspondences_->push_back (corr); + } + } + PCL_DEBUG ("[pcl::%s::computeTransformation] Total correspondences: %d\n", getClassName ().c_str (), correspondences_->size ()); + + CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_)); + for (size_t i = 0; i < correspondence_rejectors_.size (); ++i) + { + PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", correspondence_rejectors_[i]->getClassName ().c_str ()); + // We should be doing something like this + // correspondence_rejectors_[i]->setInputSource (input_transformed); + // if (source_has_normals_) + // correspondence_rejectors_[i]->setInputNormals (input_transformed); + correspondence_rejectors_[i]->setInputCorrespondences (temp_correspondences); + correspondence_rejectors_[i]->getCorrespondences (*correspondences_); + // Modify input for the next iteration + if (i < correspondence_rejectors_.size () - 1) + *temp_correspondences = *correspondences_; + } + + size_t cnt = correspondences_->size (); + // Check whether we have enough correspondences + if (cnt < min_number_correspondences_) + { + PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ()); + convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES); + converged_ = false; + break; + } + + // Estimate the transform jointly, on a combined correspondence set + transformation_estimation_->estimateRigidTransformation (*inputs_transformed_combined, *targets_combined, *correspondences_, transformation_); + + // Tranform the combined data + pcl::transformPointCloud (*inputs_transformed_combined, *inputs_transformed_combined, transformation_); + // And all its components + for (size_t i = 0; i < sources_.size (); i++) + { + pcl::transformPointCloud (*inputs_transformed[i], *inputs_transformed[i], transformation_); + } + + // Obtain the final transformation + final_transformation_ = transformation_ * final_transformation_; + + ++nr_iterations_; + + // Update the vizualization of icp convergence + //if (update_visualizer_ != 0) + // update_visualizer_(output, source_indices_good, *target_, target_indices_good ); + + converged_ = static_cast ((*convergence_criteria_)); + } + while (!converged_); + + PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n", + final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3), + final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3), + final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3), + final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3)); + + // For fitness checks, etc, we'll use an aggregated cloud for now (should be evaluating independently for correctness, but this requires propagating a few virtual methods from Registration) + IterativeClosestPoint::setInputSource (sources_combined); + IterativeClosestPoint::setInputTarget (targets_combined); + + // If we automatically set the correspondence estimators, we should clear them now + if (!manual_correspondence_estimations_set) + { + correspondence_estimations_.clear (); + } + + + // By definition, this method will return an empty cloud (for compliance with the ICP API). + // We can figure out a better solution, if necessary. + output = PointCloudSource (); +} + + +#endif /* PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_ */ + + diff --git a/registration/include/pcl/registration/joint_icp.h b/registration/include/pcl/registration/joint_icp.h new file mode 100644 index 00000000000..0dbd8bfc2d8 --- /dev/null +++ b/registration/include/pcl/registration/joint_icp.h @@ -0,0 +1,231 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_JOINT_ICP_H_ +#define PCL_JOINT_ICP_H_ + +// PCL includes +#include +namespace pcl +{ + /** \brief @b JointIterativeClosestPoint extends ICP to multiple frames which + * share the same transform. This is particularly useful when solving for + * camera extrinsics using multiple observations. When given a single pair of + * clouds, this reduces to vanilla ICP. + * + * \author Stephen Miller + * \ingroup registration + */ + template + class JointIterativeClosestPoint : public IterativeClosestPoint + { + public: + typedef typename IterativeClosestPoint::PointCloudSource PointCloudSource; + typedef typename PointCloudSource::Ptr PointCloudSourcePtr; + typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; + + typedef typename IterativeClosestPoint::PointCloudTarget PointCloudTarget; + typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; + typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; + + typedef pcl::search::KdTree KdTree; + typedef typename pcl::search::KdTree::Ptr KdTreePtr; + + typedef pcl::search::KdTree KdTreeReciprocal; + typedef typename KdTree::Ptr KdTreeReciprocalPtr; + + + typedef PointIndices::Ptr PointIndicesPtr; + typedef PointIndices::ConstPtr PointIndicesConstPtr; + + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + + typedef typename pcl::registration::CorrespondenceEstimationBase CorrespondenceEstimation; + typedef typename CorrespondenceEstimation::Ptr CorrespondenceEstimationPtr; + typedef typename CorrespondenceEstimation::ConstPtr CorrespondenceEstimationConstPtr; + + + using IterativeClosestPoint::reg_name_; + using IterativeClosestPoint::getClassName; + using IterativeClosestPoint::setInputSource; + using IterativeClosestPoint::input_; + using IterativeClosestPoint::indices_; + using IterativeClosestPoint::target_; + using IterativeClosestPoint::nr_iterations_; + using IterativeClosestPoint::max_iterations_; + using IterativeClosestPoint::previous_transformation_; + using IterativeClosestPoint::final_transformation_; + using IterativeClosestPoint::transformation_; + using IterativeClosestPoint::transformation_epsilon_; + using IterativeClosestPoint::converged_; + using IterativeClosestPoint::corr_dist_threshold_; + using IterativeClosestPoint::inlier_threshold_; + using IterativeClosestPoint::min_number_correspondences_; + using IterativeClosestPoint::update_visualizer_; + using IterativeClosestPoint::euclidean_fitness_epsilon_; + using IterativeClosestPoint::correspondences_; + using IterativeClosestPoint::transformation_estimation_; + using IterativeClosestPoint::correspondence_estimation_; + using IterativeClosestPoint::correspondence_rejectors_; + + using IterativeClosestPoint::use_reciprocal_correspondence_; + + using IterativeClosestPoint::convergence_criteria_; + + + typedef typename IterativeClosestPoint::Matrix4 Matrix4; + + /** \brief Empty constructor. */ + JointIterativeClosestPoint () + { + IterativeClosestPoint (); + reg_name_ = "JointIterativeClosestPoint"; + }; + + /** \brief Empty destructor */ + virtual ~JointIterativeClosestPoint () {} + + + /** \brief Provide a pointer to the input source + * (e.g., the point cloud that we want to align to the target) + * + * \param[in] cloud the input point cloud source + */ + virtual void + setInputSource (const PointCloudSourceConstPtr &cloud) + { + PCL_WARN ("[pcl::%s::setInputSource] Warning; JointIterativeClosestPoint expects multiple clouds. Please use addInputSource.", + getClassName ().c_str ()); + return; + } + + /** \brief Add a source cloud to the joint solver + * + * \param[in] source cloud + */ + inline void + addInputSource (const PointCloudSourceConstPtr &cloud) + { + // Set the parent InputSource, just to get all cached values (e.g. the existence of normals). + if (sources_.empty ()) + IterativeClosestPoint::setInputSource (cloud); + sources_.push_back (cloud); + } + + /** \brief Provide a pointer to the input target + * (e.g., the point cloud that we want to align to the target) + * + * \param[in] cloud the input point cloud target + */ + virtual void + setInputTarget (const PointCloudTargetConstPtr &cloud) + { + PCL_WARN ("[pcl::%s::setInputTarget] Warning; JointIterativeClosestPoint expects multiple clouds. Please use addInputTarget.", + getClassName ().c_str ()); + return; + } + + /** \brief Add a target cloud to the joint solver + * + * \param[in] target cloud + */ + inline void + addInputTarget (const PointCloudTargetConstPtr &cloud) + { + // Set the parent InputTarget, just to get all cached values (e.g. the existence of normals). + if (targets_.empty ()) + IterativeClosestPoint::setInputTarget (cloud); + targets_.push_back (cloud); + } + + /** \brief Add a manual correspondence estimator + * If you choose to do this, you must add one for each + * input source / target pair. They do not need to have trees + * or input clouds set ahead of time. + * + * \param[in] Correspondence estimation + */ + inline void + addCorrespondenceEstimation (CorrespondenceEstimationPtr ce) + { + correspondence_estimations_.push_back (ce); + } + + /** \brief Reset my list of input sources + */ + inline void + clearInputSources () + { sources_.clear (); } + + /** \brief Reset my list of input targets + */ + inline void + clearInputTargets () + { targets_.clear (); } + + /** \brief Reset my list of correspondence estimation methods. + */ + inline void + clearCorrespondenceEstimations () + { correspondence_estimations_.clear (); } + + + protected: + + /** \brief Rigid transformation computation method with initial guess. + * \param output the transformed input point cloud dataset using the rigid transformation found + * \param guess the initial guess of the transformation to compute + */ + virtual void + computeTransformation (PointCloudSource &output, const Matrix4 &guess); + + std::vector sources_; + std::vector targets_; + std::vector correspondence_estimations_; + }; + +} + +#include + +#endif //#ifndef PCL_JOINT_ICP_H_ + + diff --git a/test/registration/test_registration.cpp b/test/registration/test_registration.cpp index 313e07db495..54b77ce6723 100644 --- a/test/registration/test_registration.cpp +++ b/test/registration/test_registration.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -192,6 +193,69 @@ TEST (PCL, IterativeClosestPoint) // EXPECT_EQ (transformation (3, 3), 1); } +//////////////////////////////////////////////////////////////////////////////////////////////////////// +void +sampleRandomTransform (Eigen::Affine3f &trans, float max_angle, float max_trans) +{ + // Sample random transform + Eigen::Vector3f axis((float)rand() / RAND_MAX, (float)rand() / RAND_MAX, (float)rand() / RAND_MAX); + axis.normalize(); + float angle = (float)rand() / RAND_MAX * max_angle; + Eigen::Vector3f translation((float)rand() / RAND_MAX, (float)rand() / RAND_MAX, (float)rand() / RAND_MAX); + translation *= max_trans; + Eigen::Affine3f rotation(Eigen::AngleAxis(angle, axis)); + trans = Eigen::Translation3f(translation) * rotation; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, JointIterativeClosestPoint) +{ + // Set up + JointIterativeClosestPoint reg; + reg.setMaximumIterations (50); + reg.setTransformationEpsilon (1e-8); + reg.setMaxCorrespondenceDistance (0.25); // Making sure the correspondence distance > the max translation + size_t ntransforms = 10; + for (size_t t = 0; t < ntransforms; t++) + { + + // Sample a fixed offset between cloud pairs + Eigen::Affine3f delta_transform; + // No rotation, since at a random offset this could make it converge to a wrong (but still reasonable) result + sampleRandomTransform (delta_transform, 0., 0.10); + // Make a few transformed versions of the data, plus noise + size_t nclouds = 5; + for (size_t i = 0; i < nclouds; i++) + { + PointCloud::ConstPtr source (cloud_source.makeShared ()); + // Sample random global transform for each pair + Eigen::Affine3f net_transform; + sampleRandomTransform (net_transform, 2*M_PI, 10.); + // And apply it to the source and target + PointCloud::Ptr source_trans (new PointCloud); + PointCloud::Ptr target_trans (new PointCloud); + pcl::transformPointCloud (*source, *source_trans, delta_transform.inverse () * net_transform); + pcl::transformPointCloud (*source, *target_trans, net_transform); + // Add these to the joint solver + reg.addInputSource (source_trans); + reg.addInputTarget (target_trans); + + } + + // Register + reg.align (cloud_reg); + Eigen::Matrix4f trans_final = reg.getFinalTransformation (); + for (int y = 0; y < 4; y++) + for (int x = 0; x < 4; x++) + EXPECT_NEAR (trans_final (y, x), delta_transform (y, x), 1E-2); + + EXPECT_TRUE (cloud_reg.empty ()); // By definition, returns an empty cloud + // Clear + reg.clearInputSources (); + reg.clearInputTargets (); + } +} + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, IterativeClosestPointNonLinear) { From 35896443faf2da459dc0731bf9373764f6778d98 Mon Sep 17 00:00:00 2001 From: Stephen Miller Date: Sat, 2 Nov 2013 17:41:01 +0900 Subject: [PATCH 2/3] Adding missing cpp file --- registration/src/joint_icp.cpp | 40 ++++++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 registration/src/joint_icp.cpp diff --git a/registration/src/joint_icp.cpp b/registration/src/joint_icp.cpp new file mode 100644 index 00000000000..7d3702b6c48 --- /dev/null +++ b/registration/src/joint_icp.cpp @@ -0,0 +1,40 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#include From a5eb2a6112961e8ac40fe059cbac0b5cec22fc71 Mon Sep 17 00:00:00 2001 From: Stephen Miller Date: Fri, 8 Nov 2013 20:17:56 +0900 Subject: [PATCH 3/3] Made sure the LICENSE text was identical to the given version --- .../pcl/registration/impl/joint_icp.hpp | 68 +++++++++---------- .../include/pcl/registration/joint_icp.h | 68 +++++++++---------- registration/src/joint_icp.cpp | 67 +++++++++--------- 3 files changed, 99 insertions(+), 104 deletions(-) diff --git a/registration/include/pcl/registration/impl/joint_icp.hpp b/registration/include/pcl/registration/impl/joint_icp.hpp index cfe1448cf8c..d874341b539 100644 --- a/registration/include/pcl/registration/impl/joint_icp.hpp +++ b/registration/include/pcl/registration/impl/joint_icp.hpp @@ -1,40 +1,38 @@ /* * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2010-2011, Willow Garage, Inc. - * Copyright (c) 2012-, Open Perception, Inc - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id$ + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. * */ diff --git a/registration/include/pcl/registration/joint_icp.h b/registration/include/pcl/registration/joint_icp.h index 0dbd8bfc2d8..9842dae500a 100644 --- a/registration/include/pcl/registration/joint_icp.h +++ b/registration/include/pcl/registration/joint_icp.h @@ -1,40 +1,38 @@ /* * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2010, Willow Garage, Inc. - * Copyright (c) 2012-, Open Perception, Inc. - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id$ + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. * */ diff --git a/registration/src/joint_icp.cpp b/registration/src/joint_icp.cpp index 7d3702b6c48..b83032f6749 100644 --- a/registration/src/joint_icp.cpp +++ b/registration/src/joint_icp.cpp @@ -1,39 +1,38 @@ /* * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2010-2011, Willow Garage, Inc. - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id$ + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. * */