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-2012, 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_NORMAL_SHOOTING_H_ 00041 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_ 00042 00043 /////////////////////////////////////////////////////////////////////////////////////////// 00044 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool 00045 pcl::registration::CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::initCompute () 00046 { 00047 if (!source_normals_) 00048 { 00049 PCL_WARN ("[pcl::registration::%s::initCompute] Datasets containing normals for source have not been given!\n", getClassName ().c_str ()); 00050 return (false); 00051 } 00052 00053 return (CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute ()); 00054 } 00055 00056 /////////////////////////////////////////////////////////////////////////////////////////// 00057 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void 00058 pcl::registration::CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::determineCorrespondences ( 00059 pcl::Correspondences &correspondences, double max_distance) 00060 { 00061 if (!initCompute ()) 00062 return; 00063 00064 typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget; 00065 correspondences.resize (indices_->size ()); 00066 00067 std::vector<int> nn_indices (k_); 00068 std::vector<float> nn_dists (k_); 00069 00070 double min_dist = std::numeric_limits<double>::max (); 00071 int min_index = 0; 00072 00073 pcl::Correspondence corr; 00074 unsigned int nr_valid_correspondences = 0; 00075 00076 // Check if the template types are the same. If true, avoid a copy. 00077 // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! 00078 if (isSamePointType<PointSource, PointTarget> ()) 00079 { 00080 PointTarget pt; 00081 // Iterate over the input set of source indices 00082 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) 00083 { 00084 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); 00085 00086 // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal 00087 min_dist = std::numeric_limits<double>::max (); 00088 00089 // Find the best correspondence 00090 for (size_t j = 0; j < nn_indices.size (); j++) 00091 { 00092 // computing the distance between a point and a line in 3d. 00093 // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html 00094 pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x; 00095 pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y; 00096 pt.z = target_->points[nn_indices[j]].z - input_->points[*idx_i].z; 00097 00098 const NormalT &normal = source_normals_->points[*idx_i]; 00099 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z); 00100 Eigen::Vector3d V (pt.x, pt.y, pt.z); 00101 Eigen::Vector3d C = N.cross (V); 00102 00103 // Check if we have a better correspondence 00104 double dist = C.dot (C); 00105 if (dist < min_dist) 00106 { 00107 min_dist = dist; 00108 min_index = static_cast<int> (j); 00109 } 00110 } 00111 if (min_dist > max_distance) 00112 continue; 00113 00114 corr.index_query = *idx_i; 00115 corr.index_match = nn_indices[min_index]; 00116 corr.distance = nn_dists[min_index];//min_dist; 00117 correspondences[nr_valid_correspondences++] = corr; 00118 } 00119 } 00120 else 00121 { 00122 PointTarget pt; 00123 00124 // Iterate over the input set of source indices 00125 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) 00126 { 00127 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); 00128 00129 // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal 00130 min_dist = std::numeric_limits<double>::max (); 00131 00132 // Find the best correspondence 00133 for (size_t j = 0; j < nn_indices.size (); j++) 00134 { 00135 PointSource pt_src; 00136 // Copy the source data to a target PointTarget format so we can search in the tree 00137 pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> ( 00138 input_->points[*idx_i], 00139 pt_src)); 00140 00141 // computing the distance between a point and a line in 3d. 00142 // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html 00143 pt.x = target_->points[nn_indices[j]].x - pt_src.x; 00144 pt.y = target_->points[nn_indices[j]].y - pt_src.y; 00145 pt.z = target_->points[nn_indices[j]].z - pt_src.z; 00146 00147 const NormalT &normal = source_normals_->points[*idx_i]; 00148 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z); 00149 Eigen::Vector3d V (pt.x, pt.y, pt.z); 00150 Eigen::Vector3d C = N.cross (V); 00151 00152 // Check if we have a better correspondence 00153 double dist = C.dot (C); 00154 if (dist < min_dist) 00155 { 00156 min_dist = dist; 00157 min_index = static_cast<int> (j); 00158 } 00159 } 00160 if (min_dist > max_distance) 00161 continue; 00162 00163 corr.index_query = *idx_i; 00164 corr.index_match = nn_indices[min_index]; 00165 corr.distance = nn_dists[min_index];//min_dist; 00166 correspondences[nr_valid_correspondences++] = corr; 00167 } 00168 } 00169 correspondences.resize (nr_valid_correspondences); 00170 deinitCompute (); 00171 } 00172 00173 /////////////////////////////////////////////////////////////////////////////////////////// 00174 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void 00175 pcl::registration::CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::determineReciprocalCorrespondences ( 00176 pcl::Correspondences &correspondences, double max_distance) 00177 { 00178 if (!initCompute ()) 00179 return; 00180 00181 typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget; 00182 00183 // setup tree for reciprocal search 00184 // Set the internal point representation of choice 00185 if (!initComputeReciprocal ()) 00186 return; 00187 00188 correspondences.resize (indices_->size ()); 00189 00190 std::vector<int> nn_indices (k_); 00191 std::vector<float> nn_dists (k_); 00192 std::vector<int> index_reciprocal (1); 00193 std::vector<float> distance_reciprocal (1); 00194 00195 double min_dist = std::numeric_limits<double>::max (); 00196 int min_index = 0; 00197 00198 pcl::Correspondence corr; 00199 unsigned int nr_valid_correspondences = 0; 00200 int target_idx = 0; 00201 00202 // Check if the template types are the same. If true, avoid a copy. 00203 // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! 00204 if (isSamePointType<PointSource, PointTarget> ()) 00205 { 00206 PointTarget pt; 00207 // Iterate over the input set of source indices 00208 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) 00209 { 00210 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); 00211 00212 // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal 00213 min_dist = std::numeric_limits<double>::max (); 00214 00215 // Find the best correspondence 00216 for (size_t j = 0; j < nn_indices.size (); j++) 00217 { 00218 // computing the distance between a point and a line in 3d. 00219 // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html 00220 pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x; 00221 pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y; 00222 pt.z = target_->points[nn_indices[j]].z - input_->points[*idx_i].z; 00223 00224 const NormalT &normal = source_normals_->points[*idx_i]; 00225 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z); 00226 Eigen::Vector3d V (pt.x, pt.y, pt.z); 00227 Eigen::Vector3d C = N.cross (V); 00228 00229 // Check if we have a better correspondence 00230 double dist = C.dot (C); 00231 if (dist < min_dist) 00232 { 00233 min_dist = dist; 00234 min_index = static_cast<int> (j); 00235 } 00236 } 00237 if (min_dist > max_distance) 00238 continue; 00239 00240 // Check if the correspondence is reciprocal 00241 target_idx = nn_indices[min_index]; 00242 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal); 00243 00244 if (*idx_i != index_reciprocal[0]) 00245 continue; 00246 00247 // Correspondence IS reciprocal, save it and continue 00248 corr.index_query = *idx_i; 00249 corr.index_match = nn_indices[min_index]; 00250 corr.distance = nn_dists[min_index];//min_dist; 00251 correspondences[nr_valid_correspondences++] = corr; 00252 } 00253 } 00254 else 00255 { 00256 PointTarget pt; 00257 00258 // Iterate over the input set of source indices 00259 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) 00260 { 00261 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); 00262 00263 // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal 00264 min_dist = std::numeric_limits<double>::max (); 00265 00266 // Find the best correspondence 00267 for (size_t j = 0; j < nn_indices.size (); j++) 00268 { 00269 PointSource pt_src; 00270 // Copy the source data to a target PointTarget format so we can search in the tree 00271 pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> ( 00272 input_->points[*idx_i], 00273 pt_src)); 00274 00275 // computing the distance between a point and a line in 3d. 00276 // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html 00277 pt.x = target_->points[nn_indices[j]].x - pt_src.x; 00278 pt.y = target_->points[nn_indices[j]].y - pt_src.y; 00279 pt.z = target_->points[nn_indices[j]].z - pt_src.z; 00280 00281 const NormalT &normal = source_normals_->points[*idx_i]; 00282 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z); 00283 Eigen::Vector3d V (pt.x, pt.y, pt.z); 00284 Eigen::Vector3d C = N.cross (V); 00285 00286 // Check if we have a better correspondence 00287 double dist = C.dot (C); 00288 if (dist < min_dist) 00289 { 00290 min_dist = dist; 00291 min_index = static_cast<int> (j); 00292 } 00293 } 00294 if (min_dist > max_distance) 00295 continue; 00296 00297 // Check if the correspondence is reciprocal 00298 target_idx = nn_indices[min_index]; 00299 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal); 00300 00301 if (*idx_i != index_reciprocal[0]) 00302 continue; 00303 00304 // Correspondence IS reciprocal, save it and continue 00305 corr.index_query = *idx_i; 00306 corr.index_match = nn_indices[min_index]; 00307 corr.distance = nn_dists[min_index];//min_dist; 00308 correspondences[nr_valid_correspondences++] = corr; 00309 } 00310 } 00311 correspondences.resize (nr_valid_correspondences); 00312 deinitCompute (); 00313 } 00314 00315 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_