Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 00041 /////////////////////////////////////////////////////////////////////////////////////////// 00042 template <typename PointSource, typename PointTarget, typename Scalar> void 00043 pcl::Registration<PointSource, PointTarget, Scalar>::setInputCloud ( 00044 const typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSourceConstPtr &cloud) 00045 { 00046 setInputSource (cloud); 00047 } 00048 00049 /////////////////////////////////////////////////////////////////////////////////////////// 00050 template <typename PointSource, typename PointTarget, typename Scalar> typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSourceConstPtr const 00051 pcl::Registration<PointSource, PointTarget, Scalar>::getInputCloud () 00052 { 00053 return (getInputSource ()); 00054 } 00055 00056 /////////////////////////////////////////////////////////////////////////////////////////// 00057 template <typename PointSource, typename PointTarget, typename Scalar> inline void 00058 pcl::Registration<PointSource, PointTarget, Scalar>::setInputTarget (const PointCloudTargetConstPtr &cloud) 00059 { 00060 if (cloud->points.empty ()) 00061 { 00062 PCL_ERROR ("[pcl::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); 00063 return; 00064 } 00065 target_ = cloud; 00066 target_cloud_updated_ = true; 00067 } 00068 00069 /////////////////////////////////////////////////////////////////////////////////////////// 00070 template <typename PointSource, typename PointTarget, typename Scalar> bool 00071 pcl::Registration<PointSource, PointTarget, Scalar>::initCompute () 00072 { 00073 if (!target_) 00074 { 00075 PCL_ERROR ("[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ()); 00076 return (false); 00077 } 00078 00079 // Only update target kd-tree if a new target cloud was set 00080 if (target_cloud_updated_ && !force_no_recompute_) 00081 { 00082 tree_->setInputCloud (target_); 00083 target_cloud_updated_ = false; 00084 } 00085 00086 00087 // Update the correspondence estimation 00088 if (correspondence_estimation_) 00089 { 00090 correspondence_estimation_->setSearchMethodTarget (tree_, force_no_recompute_); 00091 correspondence_estimation_->setSearchMethodSource (tree_reciprocal_, force_no_recompute_reciprocal_); 00092 } 00093 00094 // Note: we /cannot/ update the search method on all correspondence rejectors, because we know 00095 // nothing about them. If they should be cached, they must be cached individually. 00096 00097 return (PCLBase<PointSource>::initCompute ()); 00098 } 00099 00100 /////////////////////////////////////////////////////////////////////////////////////////// 00101 template <typename PointSource, typename PointTarget, typename Scalar> bool 00102 pcl::Registration<PointSource, PointTarget, Scalar>::initComputeReciprocal () 00103 { 00104 if (!input_) 00105 { 00106 PCL_ERROR ("[pcl::registration::%s::compute] No input source dataset was given!\n", getClassName ().c_str ()); 00107 return (false); 00108 } 00109 00110 if (source_cloud_updated_ && !force_no_recompute_reciprocal_) 00111 { 00112 tree_reciprocal_->setInputCloud (input_); 00113 source_cloud_updated_ = false; 00114 } 00115 return (true); 00116 } 00117 00118 ////////////////////////////////////////////////////////////////////////////////////////////// 00119 template <typename PointSource, typename PointTarget, typename Scalar> inline double 00120 pcl::Registration<PointSource, PointTarget, Scalar>::getFitnessScore ( 00121 const std::vector<float> &distances_a, 00122 const std::vector<float> &distances_b) 00123 { 00124 unsigned int nr_elem = static_cast<unsigned int> (std::min (distances_a.size (), distances_b.size ())); 00125 Eigen::VectorXf map_a = Eigen::VectorXf::Map (&distances_a[0], nr_elem); 00126 Eigen::VectorXf map_b = Eigen::VectorXf::Map (&distances_b[0], nr_elem); 00127 return (static_cast<double> ((map_a - map_b).sum ()) / static_cast<double> (nr_elem)); 00128 } 00129 00130 ////////////////////////////////////////////////////////////////////////////////////////////// 00131 template <typename PointSource, typename PointTarget, typename Scalar> inline double 00132 pcl::Registration<PointSource, PointTarget, Scalar>::getFitnessScore (double max_range) 00133 { 00134 00135 double fitness_score = 0.0; 00136 00137 // Transform the input dataset using the final transformation 00138 PointCloudSource input_transformed; 00139 //transformPointCloud (*input_, input_transformed, final_transformation_); 00140 input_transformed.resize (input_->size ()); 00141 for (size_t i = 0; i < input_->size (); ++i) 00142 { 00143 const PointSource &src = input_->points[i]; 00144 PointTarget &tgt = input_transformed.points[i]; 00145 tgt.x = static_cast<float> (final_transformation_ (0, 0) * src.x + final_transformation_ (0, 1) * src.y + final_transformation_ (0, 2) * src.z + final_transformation_ (0, 3)); 00146 tgt.y = static_cast<float> (final_transformation_ (1, 0) * src.x + final_transformation_ (1, 1) * src.y + final_transformation_ (1, 2) * src.z + final_transformation_ (1, 3)); 00147 tgt.z = static_cast<float> (final_transformation_ (2, 0) * src.x + final_transformation_ (2, 1) * src.y + final_transformation_ (2, 2) * src.z + final_transformation_ (2, 3)); 00148 } 00149 00150 std::vector<int> nn_indices (1); 00151 std::vector<float> nn_dists (1); 00152 00153 // For each point in the source dataset 00154 int nr = 0; 00155 for (size_t i = 0; i < input_transformed.points.size (); ++i) 00156 { 00157 Eigen::Vector4f p1 = Eigen::Vector4f (input_transformed.points[i].x, 00158 input_transformed.points[i].y, 00159 input_transformed.points[i].z, 0); 00160 // Find its nearest neighbor in the target 00161 tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists); 00162 00163 // Deal with occlusions (incomplete targets) 00164 if (nn_dists[0] > max_range) 00165 continue; 00166 00167 Eigen::Vector4f p2 = Eigen::Vector4f (target_->points[nn_indices[0]].x, 00168 target_->points[nn_indices[0]].y, 00169 target_->points[nn_indices[0]].z, 0); 00170 // Calculate the fitness score 00171 fitness_score += fabs ((p1-p2).squaredNorm ()); 00172 nr++; 00173 } 00174 00175 if (nr > 0) 00176 return (fitness_score / nr); 00177 else 00178 return (std::numeric_limits<double>::max ()); 00179 00180 } 00181 00182 ////////////////////////////////////////////////////////////////////////////////////////////// 00183 template <typename PointSource, typename PointTarget, typename Scalar> inline void 00184 pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output) 00185 { 00186 align (output, Matrix4::Identity ()); 00187 } 00188 00189 ////////////////////////////////////////////////////////////////////////////////////////////// 00190 template <typename PointSource, typename PointTarget, typename Scalar> inline void 00191 pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output, const Matrix4& guess) 00192 { 00193 if (!initCompute ()) 00194 return; 00195 00196 // Resize the output dataset 00197 if (output.points.size () != indices_->size ()) 00198 output.points.resize (indices_->size ()); 00199 // Copy the header 00200 output.header = input_->header; 00201 // Check if the output will be computed for all points or only a subset 00202 if (indices_->size () != input_->points.size ()) 00203 { 00204 output.width = static_cast<uint32_t> (indices_->size ()); 00205 output.height = 1; 00206 } 00207 else 00208 { 00209 output.width = static_cast<uint32_t> (input_->width); 00210 output.height = input_->height; 00211 } 00212 output.is_dense = input_->is_dense; 00213 00214 // Copy the point data to output 00215 for (size_t i = 0; i < indices_->size (); ++i) 00216 output.points[i] = input_->points[(*indices_)[i]]; 00217 00218 // Set the internal point representation of choice unless otherwise noted 00219 if (point_representation_ && !force_no_recompute_) 00220 tree_->setPointRepresentation (point_representation_); 00221 00222 // Perform the actual transformation computation 00223 converged_ = false; 00224 final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity (); 00225 00226 // Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid 00227 // transformation 00228 for (size_t i = 0; i < indices_->size (); ++i) 00229 output.points[i].data[3] = 1.0; 00230 00231 computeTransformation (output, guess); 00232 00233 deinitCompute (); 00234 } 00235