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) 2012-, Open Perception, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id$ 00037 * 00038 */ 00039 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ 00040 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ 00041 00042 /////////////////////////////////////////////////////////////////////////////////////////// 00043 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool 00044 pcl::registration::CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::initCompute () 00045 { 00046 if (!source_normals_ || !target_normals_) 00047 { 00048 PCL_WARN ("[pcl::registration::%s::initCompute] Datasets containing normals for source/target have not been given!\n", getClassName ().c_str ()); 00049 return (false); 00050 } 00051 00052 return (CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute ()); 00053 } 00054 00055 /////////////////////////////////////////////////////////////////////////////////////////// 00056 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void 00057 pcl::registration::CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::determineCorrespondences ( 00058 pcl::Correspondences &correspondences, double max_distance) 00059 { 00060 if (!initCompute ()) 00061 return; 00062 00063 typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget; 00064 correspondences.resize (indices_->size ()); 00065 00066 std::vector<int> nn_indices (k_); 00067 std::vector<float> nn_dists (k_); 00068 00069 float min_dist = std::numeric_limits<float>::max (); 00070 int min_index = 0; 00071 00072 pcl::Correspondence corr; 00073 unsigned int nr_valid_correspondences = 0; 00074 00075 // Check if the template types are the same. If true, avoid a copy. 00076 // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! 00077 if (isSamePointType<PointSource, PointTarget> ()) 00078 { 00079 PointTarget pt; 00080 // Iterate over the input set of source indices 00081 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) 00082 { 00083 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); 00084 00085 // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal 00086 min_dist = std::numeric_limits<float>::max (); 00087 00088 // Find the best correspondence 00089 for (size_t j = 0; j < nn_indices.size (); j++) 00090 { 00091 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x + 00092 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + 00093 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ; 00094 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle); 00095 00096 if (dist < min_dist) 00097 { 00098 min_dist = dist; 00099 min_index = static_cast<int> (j); 00100 } 00101 } 00102 if (min_dist > max_distance) 00103 continue; 00104 00105 corr.index_query = *idx_i; 00106 corr.index_match = nn_indices[min_index]; 00107 corr.distance = nn_dists[min_index];//min_dist; 00108 correspondences[nr_valid_correspondences++] = corr; 00109 } 00110 } 00111 else 00112 { 00113 PointTarget pt; 00114 00115 // Iterate over the input set of source indices 00116 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) 00117 { 00118 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); 00119 00120 // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal 00121 min_dist = std::numeric_limits<float>::max (); 00122 00123 // Find the best correspondence 00124 for (size_t j = 0; j < nn_indices.size (); j++) 00125 { 00126 PointSource pt_src; 00127 // Copy the source data to a target PointTarget format so we can search in the tree 00128 pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> ( 00129 input_->points[*idx_i], 00130 pt_src)); 00131 00132 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x + 00133 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + 00134 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ; 00135 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle); 00136 00137 if (dist < min_dist) 00138 { 00139 min_dist = dist; 00140 min_index = static_cast<int> (j); 00141 } 00142 } 00143 if (min_dist > max_distance) 00144 continue; 00145 00146 corr.index_query = *idx_i; 00147 corr.index_match = nn_indices[min_index]; 00148 corr.distance = nn_dists[min_index];//min_dist; 00149 correspondences[nr_valid_correspondences++] = corr; 00150 } 00151 } 00152 correspondences.resize (nr_valid_correspondences); 00153 deinitCompute (); 00154 } 00155 00156 /////////////////////////////////////////////////////////////////////////////////////////// 00157 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void 00158 pcl::registration::CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::determineReciprocalCorrespondences ( 00159 pcl::Correspondences &correspondences, double max_distance) 00160 { 00161 if (!initCompute ()) 00162 return; 00163 00164 typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget; 00165 00166 // Set the internal point representation of choice 00167 if(!initComputeReciprocal()) 00168 return; 00169 00170 correspondences.resize (indices_->size ()); 00171 00172 std::vector<int> nn_indices (k_); 00173 std::vector<float> nn_dists (k_); 00174 std::vector<int> index_reciprocal (1); 00175 std::vector<float> distance_reciprocal (1); 00176 00177 float min_dist = std::numeric_limits<float>::max (); 00178 int min_index = 0; 00179 00180 pcl::Correspondence corr; 00181 unsigned int nr_valid_correspondences = 0; 00182 int target_idx = 0; 00183 00184 // Check if the template types are the same. If true, avoid a copy. 00185 // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! 00186 if (isSamePointType<PointSource, PointTarget> ()) 00187 { 00188 PointTarget pt; 00189 // Iterate over the input set of source indices 00190 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) 00191 { 00192 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); 00193 00194 // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal 00195 min_dist = std::numeric_limits<float>::max (); 00196 00197 // Find the best correspondence 00198 for (size_t j = 0; j < nn_indices.size (); j++) 00199 { 00200 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x + 00201 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + 00202 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ; 00203 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle); 00204 00205 if (dist < min_dist) 00206 { 00207 min_dist = dist; 00208 min_index = static_cast<int> (j); 00209 } 00210 } 00211 if (min_dist > max_distance) 00212 continue; 00213 00214 // Check if the correspondence is reciprocal 00215 target_idx = nn_indices[min_index]; 00216 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal); 00217 00218 if (*idx_i != index_reciprocal[0]) 00219 continue; 00220 00221 corr.index_query = *idx_i; 00222 corr.index_match = nn_indices[min_index]; 00223 corr.distance = nn_dists[min_index];//min_dist; 00224 correspondences[nr_valid_correspondences++] = corr; 00225 } 00226 } 00227 else 00228 { 00229 PointTarget pt; 00230 00231 // Iterate over the input set of source indices 00232 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) 00233 { 00234 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); 00235 00236 // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal 00237 min_dist = std::numeric_limits<float>::max (); 00238 00239 // Find the best correspondence 00240 for (size_t j = 0; j < nn_indices.size (); j++) 00241 { 00242 PointSource pt_src; 00243 // Copy the source data to a target PointTarget format so we can search in the tree 00244 pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> ( 00245 input_->points[*idx_i], 00246 pt_src)); 00247 00248 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x + 00249 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + 00250 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ; 00251 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle); 00252 00253 if (dist < min_dist) 00254 { 00255 min_dist = dist; 00256 min_index = static_cast<int> (j); 00257 } 00258 } 00259 if (min_dist > max_distance) 00260 continue; 00261 00262 // Check if the correspondence is reciprocal 00263 target_idx = nn_indices[min_index]; 00264 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal); 00265 00266 if (*idx_i != index_reciprocal[0]) 00267 continue; 00268 00269 corr.index_query = *idx_i; 00270 corr.index_match = nn_indices[min_index]; 00271 corr.distance = nn_dists[min_index];//min_dist; 00272 correspondences[nr_valid_correspondences++] = corr; 00273 } 00274 } 00275 correspondences.resize (nr_valid_correspondences); 00276 deinitCompute (); 00277 } 00278 00279 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_