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 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ 00041 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ 00042 00043 #include <pcl/common/concatenate.h> 00044 #include <pcl/common/io.h> 00045 00046 /////////////////////////////////////////////////////////////////////////////////////////// 00047 template <typename PointSource, typename PointTarget, typename Scalar> void 00048 pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::setInputCloud (const typename pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::PointCloudSourceConstPtr &cloud) 00049 { 00050 setInputSource (cloud); 00051 } 00052 00053 /////////////////////////////////////////////////////////////////////////////////////////// 00054 template <typename PointSource, typename PointTarget, typename Scalar> typename pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::PointCloudSourceConstPtr const 00055 pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getInputCloud () 00056 { 00057 return (getInputSource ()); 00058 } 00059 00060 /////////////////////////////////////////////////////////////////////////////////////////// 00061 template <typename PointSource, typename PointTarget, typename Scalar> void 00062 pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::setInputTarget ( 00063 const PointCloudTargetConstPtr &cloud) 00064 { 00065 if (cloud->points.empty ()) 00066 { 00067 PCL_ERROR ("[pcl::registration::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); 00068 return; 00069 } 00070 target_ = cloud; 00071 00072 // Set the internal point representation of choice 00073 if (point_representation_) 00074 tree_->setPointRepresentation (point_representation_); 00075 00076 target_cloud_updated_ = true; 00077 } 00078 00079 /////////////////////////////////////////////////////////////////////////////////////////// 00080 template <typename PointSource, typename PointTarget, typename Scalar> bool 00081 pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute () 00082 { 00083 if (!target_) 00084 { 00085 PCL_ERROR ("[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ()); 00086 return (false); 00087 } 00088 00089 // Only update target kd-tree if a new target cloud was set 00090 if (target_cloud_updated_ && !force_no_recompute_) 00091 { 00092 // If the target indices have been given via setIndicesTarget 00093 if (target_indices_) 00094 tree_->setInputCloud (target_, target_indices_); 00095 else 00096 tree_->setInputCloud (target_); 00097 00098 target_cloud_updated_ = false; 00099 } 00100 00101 return (PCLBase<PointSource>::initCompute ()); 00102 } 00103 00104 /////////////////////////////////////////////////////////////////////////////////////////// 00105 template <typename PointSource, typename PointTarget, typename Scalar> bool 00106 pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeReciprocal () 00107 { 00108 // Only update source kd-tree if a new target cloud was set 00109 if (source_cloud_updated_ && !force_no_recompute_reciprocal_) 00110 { 00111 if (point_representation_) 00112 tree_reciprocal_->setPointRepresentation (point_representation_); 00113 // If the target indices have been given via setIndicesTarget 00114 if (indices_) 00115 tree_reciprocal_->setInputCloud (getInputSource(), getIndicesSource()); 00116 else 00117 tree_reciprocal_->setInputCloud (getInputSource()); 00118 00119 source_cloud_updated_ = false; 00120 } 00121 00122 return (true); 00123 } 00124 00125 /////////////////////////////////////////////////////////////////////////////////////////// 00126 template <typename PointSource, typename PointTarget, typename Scalar> void 00127 pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences ( 00128 pcl::Correspondences &correspondences, double max_distance) 00129 { 00130 if (!initCompute ()) 00131 return; 00132 00133 double max_dist_sqr = max_distance * max_distance; 00134 00135 typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget; 00136 correspondences.resize (indices_->size ()); 00137 00138 std::vector<int> index (1); 00139 std::vector<float> distance (1); 00140 pcl::Correspondence corr; 00141 unsigned int nr_valid_correspondences = 0; 00142 00143 // Check if the template types are the same. If true, avoid a copy. 00144 // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! 00145 if (isSamePointType<PointSource, PointTarget> ()) 00146 { 00147 // Iterate over the input set of source indices 00148 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) 00149 { 00150 tree_->nearestKSearch (input_->points[*idx], 1, index, distance); 00151 if (distance[0] > max_dist_sqr) 00152 continue; 00153 00154 corr.index_query = *idx; 00155 corr.index_match = index[0]; 00156 corr.distance = distance[0]; 00157 correspondences[nr_valid_correspondences++] = corr; 00158 } 00159 } 00160 else 00161 { 00162 PointTarget pt; 00163 00164 // Iterate over the input set of source indices 00165 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) 00166 { 00167 // Copy the source data to a target PointTarget format so we can search in the tree 00168 pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> ( 00169 input_->points[*idx], 00170 pt)); 00171 00172 tree_->nearestKSearch (pt, 1, index, distance); 00173 if (distance[0] > max_dist_sqr) 00174 continue; 00175 00176 corr.index_query = *idx; 00177 corr.index_match = index[0]; 00178 corr.distance = distance[0]; 00179 correspondences[nr_valid_correspondences++] = corr; 00180 } 00181 } 00182 correspondences.resize (nr_valid_correspondences); 00183 deinitCompute (); 00184 } 00185 00186 /////////////////////////////////////////////////////////////////////////////////////////// 00187 template <typename PointSource, typename PointTarget, typename Scalar> void 00188 pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineReciprocalCorrespondences ( 00189 pcl::Correspondences &correspondences, double max_distance) 00190 { 00191 if (!initCompute ()) 00192 return; 00193 00194 typedef typename pcl::traits::fieldList<PointSource>::type FieldListSource; 00195 typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget; 00196 typedef typename pcl::intersect<FieldListSource, FieldListTarget>::type FieldList; 00197 00198 // setup tree for reciprocal search 00199 // Set the internal point representation of choice 00200 if (!initComputeReciprocal()) 00201 return; 00202 double max_dist_sqr = max_distance * max_distance; 00203 00204 correspondences.resize (indices_->size()); 00205 std::vector<int> index (1); 00206 std::vector<float> distance (1); 00207 std::vector<int> index_reciprocal (1); 00208 std::vector<float> distance_reciprocal (1); 00209 pcl::Correspondence corr; 00210 unsigned int nr_valid_correspondences = 0; 00211 int target_idx = 0; 00212 00213 // Check if the template types are the same. If true, avoid a copy. 00214 // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! 00215 if (isSamePointType<PointSource, PointTarget> ()) 00216 { 00217 // Iterate over the input set of source indices 00218 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) 00219 { 00220 tree_->nearestKSearch (input_->points[*idx], 1, index, distance); 00221 if (distance[0] > max_dist_sqr) 00222 continue; 00223 00224 target_idx = index[0]; 00225 00226 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal); 00227 if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0]) 00228 continue; 00229 00230 corr.index_query = *idx; 00231 corr.index_match = index[0]; 00232 corr.distance = distance[0]; 00233 correspondences[nr_valid_correspondences++] = corr; 00234 } 00235 } 00236 else 00237 { 00238 PointTarget pt_src; 00239 PointSource pt_tgt; 00240 00241 // Iterate over the input set of source indices 00242 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) 00243 { 00244 // Copy the source data to a target PointTarget format so we can search in the tree 00245 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointSource, PointTarget> ( 00246 input_->points[*idx], 00247 pt_src)); 00248 00249 tree_->nearestKSearch (pt_src, 1, index, distance); 00250 if (distance[0] > max_dist_sqr) 00251 continue; 00252 00253 target_idx = index[0]; 00254 00255 // Copy the target data to a target PointSource format so we can search in the tree_reciprocal 00256 pcl::for_each_type<FieldList> (pcl::NdConcatenateFunctor <PointTarget, PointSource> ( 00257 target_->points[target_idx], 00258 pt_tgt)); 00259 00260 tree_reciprocal_->nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal); 00261 if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0]) 00262 continue; 00263 00264 corr.index_query = *idx; 00265 corr.index_match = index[0]; 00266 corr.distance = distance[0]; 00267 correspondences[nr_valid_correspondences++] = corr; 00268 } 00269 } 00270 correspondences.resize (nr_valid_correspondences); 00271 deinitCompute (); 00272 } 00273 00274 //#define PCL_INSTANTIATE_CorrespondenceEstimation(T,U) template class PCL_EXPORTS pcl::registration::CorrespondenceEstimation<T,U>; 00275 00276 #endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ */