Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * 00035 * Implementation of the ISM algorithm described in "Hough Transforms and 3D SURF for robust three dimensional classication" 00036 * by Jan Knopp, Mukta Prasad, Geert Willems, Radu Timofte, and Luc Van Gool 00037 * 00038 * Authors: Roman Shapovalov, Alexander Velizhev, Sergey Ushakov 00039 */ 00040 00041 #ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_ 00042 #define PCL_IMPLICIT_SHAPE_MODEL_HPP_ 00043 00044 #include "../implicit_shape_model.h" 00045 00046 ////////////////////////////////////////////////////////////////////////////////////////////// 00047 template <typename PointT> 00048 pcl::features::ISMVoteList<PointT>::ISMVoteList () : 00049 votes_ (new pcl::PointCloud<pcl::InterestPoint> ()), 00050 tree_is_valid_ (false), 00051 votes_origins_ (new pcl::PointCloud<PointT> ()), 00052 votes_class_ (0), 00053 tree_ (), 00054 k_ind_ (0), 00055 k_sqr_dist_ (0) 00056 { 00057 } 00058 00059 ////////////////////////////////////////////////////////////////////////////////////////////// 00060 template <typename PointT> 00061 pcl::features::ISMVoteList<PointT>::~ISMVoteList () 00062 { 00063 votes_class_.clear (); 00064 votes_origins_.reset (); 00065 votes_.reset (); 00066 k_ind_.clear (); 00067 k_sqr_dist_.clear (); 00068 tree_.reset (); 00069 } 00070 00071 ////////////////////////////////////////////////////////////////////////////////////////////// 00072 template <typename PointT> void 00073 pcl::features::ISMVoteList<PointT>::addVote ( 00074 pcl::InterestPoint& vote, const PointT &vote_origin, int votes_class) 00075 { 00076 tree_is_valid_ = false; 00077 votes_->points.insert (votes_->points.end (), vote);// TODO: adjust height and width 00078 00079 votes_origins_->points.push_back (vote_origin); 00080 votes_class_.push_back (votes_class); 00081 } 00082 00083 ////////////////////////////////////////////////////////////////////////////////////////////// 00084 template <typename PointT> typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr 00085 pcl::features::ISMVoteList<PointT>::getColoredCloud (typename pcl::PointCloud<PointT>::Ptr cloud) 00086 { 00087 pcl::PointXYZRGB point; 00088 pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGB>)->makeShared (); 00089 colored_cloud->height = 0; 00090 colored_cloud->width = 1; 00091 00092 if (cloud != 0) 00093 { 00094 colored_cloud->height += static_cast<uint32_t> (cloud->points.size ()); 00095 point.r = 255; 00096 point.g = 255; 00097 point.b = 255; 00098 for (size_t i_point = 0; i_point < cloud->points.size (); i_point++) 00099 { 00100 point.x = cloud->points[i_point].x; 00101 point.y = cloud->points[i_point].y; 00102 point.z = cloud->points[i_point].z; 00103 colored_cloud->points.push_back (point); 00104 } 00105 } 00106 00107 point.r = 0; 00108 point.g = 0; 00109 point.b = 255; 00110 for (size_t i_vote = 0; i_vote < votes_->points.size (); i_vote++) 00111 { 00112 point.x = votes_->points[i_vote].x; 00113 point.y = votes_->points[i_vote].y; 00114 point.z = votes_->points[i_vote].z; 00115 colored_cloud->points.push_back (point); 00116 } 00117 colored_cloud->height += static_cast<uint32_t> (votes_->points.size ()); 00118 00119 return (colored_cloud); 00120 } 00121 00122 ////////////////////////////////////////////////////////////////////////////////////////////// 00123 template <typename PointT> void 00124 pcl::features::ISMVoteList<PointT>::findStrongestPeaks ( 00125 std::vector<pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > &out_peaks, 00126 int in_class_id, 00127 double in_non_maxima_radius, 00128 double in_sigma) 00129 { 00130 validateTree (); 00131 00132 const size_t n_vote_classes = votes_class_.size (); 00133 if (n_vote_classes == 0) 00134 return; 00135 for (size_t i = 0; i < n_vote_classes ; i++) 00136 assert ( votes_class_[i] == in_class_id ); 00137 00138 // heuristic: start from NUM_INIT_PTS different locations selected uniformly 00139 // on the votes. Intuitively, it is likely to get a good location in dense regions. 00140 const int NUM_INIT_PTS = 100; 00141 double SIGMA_DIST = in_sigma;// rule of thumb: 10% of the object radius 00142 const double FINAL_EPS = SIGMA_DIST / 100;// another heuristic 00143 00144 std::vector<Eigen::Vector3f> peaks (NUM_INIT_PTS); 00145 std::vector<double> peak_densities (NUM_INIT_PTS); 00146 double max_density = -1.0; 00147 for (int i = 0; i < NUM_INIT_PTS; i++) 00148 { 00149 Eigen::Vector3f old_center; 00150 Eigen::Vector3f curr_center; 00151 curr_center (0) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].x; 00152 curr_center (1) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].y; 00153 curr_center (2) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].z; 00154 00155 do 00156 { 00157 old_center = curr_center; 00158 curr_center = shiftMean (old_center, SIGMA_DIST); 00159 } while ((old_center - curr_center).norm () > FINAL_EPS); 00160 00161 pcl::PointXYZ point; 00162 point.x = curr_center (0); 00163 point.y = curr_center (1); 00164 point.z = curr_center (2); 00165 double curr_density = getDensityAtPoint (point, SIGMA_DIST); 00166 assert (curr_density >= 0.0); 00167 00168 peaks[i] = curr_center; 00169 peak_densities[i] = curr_density; 00170 00171 if ( max_density < curr_density ) 00172 max_density = curr_density; 00173 } 00174 00175 //extract peaks 00176 std::vector<bool> peak_flag (NUM_INIT_PTS, true); 00177 for (int i_peak = 0; i_peak < NUM_INIT_PTS; i_peak++) 00178 { 00179 // find best peak with taking into consideration peak flags 00180 double best_density = -1.0; 00181 Eigen::Vector3f strongest_peak; 00182 int best_peak_ind (-1); 00183 int peak_counter (0); 00184 for (int i = 0; i < NUM_INIT_PTS; i++) 00185 { 00186 if ( !peak_flag[i] ) 00187 continue; 00188 00189 if ( peak_densities[i] > best_density) 00190 { 00191 best_density = peak_densities[i]; 00192 strongest_peak = peaks[i]; 00193 best_peak_ind = i; 00194 } 00195 ++peak_counter; 00196 } 00197 00198 if( peak_counter == 0 ) 00199 break;// no peaks 00200 00201 pcl::ISMPeak peak; 00202 peak.x = strongest_peak(0); 00203 peak.y = strongest_peak(1); 00204 peak.z = strongest_peak(2); 00205 peak.density = best_density; 00206 peak.class_id = in_class_id; 00207 out_peaks.push_back ( peak ); 00208 00209 // mark best peaks and all its neighbors 00210 peak_flag[best_peak_ind] = false; 00211 for (int i = 0; i < NUM_INIT_PTS; i++) 00212 { 00213 // compute distance between best peak and all unmarked peaks 00214 if ( !peak_flag[i] ) 00215 continue; 00216 00217 double dist = (strongest_peak - peaks[i]).norm (); 00218 if ( dist < in_non_maxima_radius ) 00219 peak_flag[i] = false; 00220 } 00221 } 00222 } 00223 00224 ////////////////////////////////////////////////////////////////////////////////////////////// 00225 template <typename PointT> void 00226 pcl::features::ISMVoteList<PointT>::validateTree () 00227 { 00228 if (!tree_is_valid_) 00229 { 00230 if (tree_ == 0) 00231 tree_ = boost::shared_ptr<pcl::KdTreeFLANN<pcl::InterestPoint> > (new pcl::KdTreeFLANN<pcl::InterestPoint> ()); 00232 tree_->setInputCloud (votes_); 00233 k_ind_.resize ( votes_->points.size (), -1 ); 00234 k_sqr_dist_.resize ( votes_->points.size (), 0.0f ); 00235 tree_is_valid_ = true; 00236 } 00237 } 00238 00239 ////////////////////////////////////////////////////////////////////////////////////////////// 00240 template <typename PointT> Eigen::Vector3f 00241 pcl::features::ISMVoteList<PointT>::shiftMean (const Eigen::Vector3f& snap_pt, const double in_sigma_dist) 00242 { 00243 validateTree (); 00244 00245 Eigen::Vector3f wgh_sum (0.0, 0.0, 0.0); 00246 double denom = 0.0; 00247 00248 pcl::InterestPoint pt; 00249 pt.x = snap_pt[0]; 00250 pt.y = snap_pt[1]; 00251 pt.z = snap_pt[2]; 00252 size_t n_pts = tree_->radiusSearch (pt, 3*in_sigma_dist, k_ind_, k_sqr_dist_); 00253 00254 for (size_t j = 0; j < n_pts; j++) 00255 { 00256 double kernel = votes_->points[k_ind_[j]].strength * exp (-k_sqr_dist_[j] / (in_sigma_dist * in_sigma_dist)); 00257 Eigen::Vector3f vote_vec (votes_->points[k_ind_[j]].x, votes_->points[k_ind_[j]].y, votes_->points[k_ind_[j]].z); 00258 wgh_sum += vote_vec * static_cast<float> (kernel); 00259 denom += kernel; 00260 } 00261 assert (denom > 0.0); // at least one point is close. In fact, this case should be handled too 00262 00263 return (wgh_sum / static_cast<float> (denom)); 00264 } 00265 00266 ////////////////////////////////////////////////////////////////////////////////////////////// 00267 template <typename PointT> double 00268 pcl::features::ISMVoteList<PointT>::getDensityAtPoint ( 00269 const PointT &point, double sigma_dist) 00270 { 00271 validateTree (); 00272 00273 const size_t n_vote_classes = votes_class_.size (); 00274 if (n_vote_classes == 0) 00275 return (0.0); 00276 00277 double sum_vote = 0.0; 00278 00279 pcl::InterestPoint pt; 00280 pt.x = point.x; 00281 pt.y = point.y; 00282 pt.z = point.z; 00283 size_t num_of_pts = tree_->radiusSearch (pt, 3 * sigma_dist, k_ind_, k_sqr_dist_); 00284 00285 for (size_t j = 0; j < num_of_pts; j++) 00286 sum_vote += votes_->points[k_ind_[j]].strength * exp (-k_sqr_dist_[j] / (sigma_dist * sigma_dist)); 00287 00288 return (sum_vote); 00289 } 00290 00291 ////////////////////////////////////////////////////////////////////////////////////////////// 00292 template <typename PointT> unsigned int 00293 pcl::features::ISMVoteList<PointT>::getNumberOfVotes () 00294 { 00295 return (static_cast<unsigned int> (votes_->points.size ())); 00296 } 00297 00298 ////////////////////////////////////////////////////////////////////////////////////////////// 00299 pcl::features::ISMModel::ISMModel () : 00300 statistical_weights_ (0), 00301 learned_weights_ (0), 00302 classes_ (0), 00303 sigmas_ (0), 00304 directions_to_center_ (), 00305 clusters_centers_ (), 00306 clusters_ (0), 00307 number_of_classes_ (0), 00308 number_of_visual_words_ (0), 00309 number_of_clusters_ (0), 00310 descriptors_dimension_ (0) 00311 { 00312 } 00313 00314 ////////////////////////////////////////////////////////////////////////////////////////////// 00315 pcl::features::ISMModel::ISMModel (ISMModel const & copy) 00316 { 00317 reset (); 00318 00319 this->number_of_classes_ = copy.number_of_classes_; 00320 this->number_of_visual_words_ = copy.number_of_visual_words_; 00321 this->number_of_clusters_ = copy.number_of_clusters_; 00322 this->descriptors_dimension_ = copy.descriptors_dimension_; 00323 00324 std::vector<float> vec; 00325 vec.resize (this->number_of_clusters_, 0.0f); 00326 this->statistical_weights_.resize (this->number_of_classes_, vec); 00327 for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++) 00328 for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++) 00329 this->statistical_weights_[i_class][i_cluster] = copy.statistical_weights_[i_class][i_cluster]; 00330 00331 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f); 00332 for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++) 00333 this->learned_weights_[i_visual_word] = copy.learned_weights_[i_visual_word]; 00334 00335 this->classes_.resize (this->number_of_visual_words_, 0); 00336 for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++) 00337 this->classes_[i_visual_word] = copy.classes_[i_visual_word]; 00338 00339 this->sigmas_.resize (this->number_of_classes_, 0.0f); 00340 for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++) 00341 this->sigmas_[i_class] = copy.sigmas_[i_class]; 00342 00343 this->directions_to_center_.resize (this->number_of_visual_words_, 3); 00344 for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++) 00345 for (unsigned int i_dim = 0; i_dim < 3; i_dim++) 00346 this->directions_to_center_ (i_visual_word, i_dim) = copy.directions_to_center_ (i_visual_word, i_dim); 00347 00348 this->clusters_centers_.resize (this->number_of_clusters_, 3); 00349 for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++) 00350 for (unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++) 00351 this->clusters_centers_ (i_cluster, i_dim) = copy.clusters_centers_ (i_cluster, i_dim); 00352 } 00353 00354 ////////////////////////////////////////////////////////////////////////////////////////////// 00355 pcl::features::ISMModel::~ISMModel () 00356 { 00357 reset (); 00358 } 00359 00360 ////////////////////////////////////////////////////////////////////////////////////////////// 00361 bool 00362 pcl::features::ISMModel::saveModelToFile (std::string& file_name) 00363 { 00364 std::ofstream output_file (file_name.c_str (), std::ios::trunc); 00365 if (!output_file) 00366 { 00367 output_file.close (); 00368 return (false); 00369 } 00370 00371 output_file << number_of_classes_ << " "; 00372 output_file << number_of_visual_words_ << " "; 00373 output_file << number_of_clusters_ << " "; 00374 output_file << descriptors_dimension_ << " "; 00375 00376 //write statistical weights 00377 for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++) 00378 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) 00379 output_file << statistical_weights_[i_class][i_cluster] << " "; 00380 00381 //write learned weights 00382 for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++) 00383 output_file << learned_weights_[i_visual_word] << " "; 00384 00385 //write classes 00386 for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++) 00387 output_file << classes_[i_visual_word] << " "; 00388 00389 //write sigmas 00390 for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++) 00391 output_file << sigmas_[i_class] << " "; 00392 00393 //write directions to centers 00394 for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++) 00395 for (unsigned int i_dim = 0; i_dim < 3; i_dim++) 00396 output_file << directions_to_center_ (i_visual_word, i_dim) << " "; 00397 00398 //write clusters centers 00399 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) 00400 for (unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++) 00401 output_file << clusters_centers_ (i_cluster, i_dim) << " "; 00402 00403 //write clusters 00404 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) 00405 { 00406 output_file << static_cast<unsigned int> (clusters_[i_cluster].size ()) << " "; 00407 for (unsigned int i_visual_word = 0; i_visual_word < static_cast<unsigned int> (clusters_[i_cluster].size ()); i_visual_word++) 00408 output_file << clusters_[i_cluster][i_visual_word] << " "; 00409 } 00410 00411 output_file.close (); 00412 return (true); 00413 } 00414 00415 ////////////////////////////////////////////////////////////////////////////////////////////// 00416 bool 00417 pcl::features::ISMModel::loadModelFromfile (std::string& file_name) 00418 { 00419 reset (); 00420 std::ifstream input_file (file_name.c_str ()); 00421 if (!input_file) 00422 { 00423 input_file.close (); 00424 return (false); 00425 } 00426 00427 char line[256]; 00428 00429 input_file.getline (line, 256, ' '); 00430 number_of_classes_ = static_cast<unsigned int> (strtol (line, 0, 10)); 00431 input_file.getline (line, 256, ' '); number_of_visual_words_ = atoi (line); 00432 input_file.getline (line, 256, ' '); number_of_clusters_ = atoi (line); 00433 input_file.getline (line, 256, ' '); descriptors_dimension_ = atoi (line); 00434 00435 //read statistical weights 00436 std::vector<float> vec; 00437 vec.resize (number_of_clusters_, 0.0f); 00438 statistical_weights_.resize (number_of_classes_, vec); 00439 for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++) 00440 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) 00441 input_file >> statistical_weights_[i_class][i_cluster]; 00442 00443 //read learned weights 00444 learned_weights_.resize (number_of_visual_words_, 0.0f); 00445 for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++) 00446 input_file >> learned_weights_[i_visual_word]; 00447 00448 //read classes 00449 classes_.resize (number_of_visual_words_, 0); 00450 for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++) 00451 input_file >> classes_[i_visual_word]; 00452 00453 //read sigmas 00454 sigmas_.resize (number_of_classes_, 0.0f); 00455 for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++) 00456 input_file >> sigmas_[i_class]; 00457 00458 //read directions to centers 00459 directions_to_center_.resize (number_of_visual_words_, 3); 00460 for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++) 00461 for (unsigned int i_dim = 0; i_dim < 3; i_dim++) 00462 input_file >> directions_to_center_ (i_visual_word, i_dim); 00463 00464 //read clusters centers 00465 clusters_centers_.resize (number_of_clusters_, descriptors_dimension_); 00466 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) 00467 for (unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++) 00468 input_file >> clusters_centers_ (i_cluster, i_dim); 00469 00470 //read clusters 00471 std::vector<unsigned int> vect; 00472 clusters_.resize (number_of_clusters_, vect); 00473 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) 00474 { 00475 unsigned int size_of_current_cluster = 0; 00476 input_file >> size_of_current_cluster; 00477 clusters_[i_cluster].resize (size_of_current_cluster, 0); 00478 for (unsigned int i_visual_word = 0; i_visual_word < size_of_current_cluster; i_visual_word++) 00479 input_file >> clusters_[i_cluster][i_visual_word]; 00480 } 00481 00482 input_file.close (); 00483 return (true); 00484 } 00485 00486 ////////////////////////////////////////////////////////////////////////////////////////////// 00487 void 00488 pcl::features::ISMModel::reset () 00489 { 00490 statistical_weights_.clear (); 00491 learned_weights_.clear (); 00492 classes_.clear (); 00493 sigmas_.clear (); 00494 directions_to_center_.resize (0, 0); 00495 clusters_centers_.resize (0, 0); 00496 clusters_.clear (); 00497 number_of_classes_ = 0; 00498 number_of_visual_words_ = 0; 00499 number_of_clusters_ = 0; 00500 descriptors_dimension_ = 0; 00501 } 00502 00503 ////////////////////////////////////////////////////////////////////////////////////////////// 00504 pcl::features::ISMModel& 00505 pcl::features::ISMModel::operator = (const pcl::features::ISMModel& other) 00506 { 00507 if (this != &other) 00508 { 00509 this->reset (); 00510 00511 this->number_of_classes_ = other.number_of_classes_; 00512 this->number_of_visual_words_ = other.number_of_visual_words_; 00513 this->number_of_clusters_ = other.number_of_clusters_; 00514 this->descriptors_dimension_ = other.descriptors_dimension_; 00515 00516 std::vector<float> vec; 00517 vec.resize (number_of_clusters_, 0.0f); 00518 this->statistical_weights_.resize (this->number_of_classes_, vec); 00519 for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++) 00520 for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++) 00521 this->statistical_weights_[i_class][i_cluster] = other.statistical_weights_[i_class][i_cluster]; 00522 00523 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f); 00524 for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++) 00525 this->learned_weights_[i_visual_word] = other.learned_weights_[i_visual_word]; 00526 00527 this->classes_.resize (this->number_of_visual_words_, 0); 00528 for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++) 00529 this->classes_[i_visual_word] = other.classes_[i_visual_word]; 00530 00531 this->sigmas_.resize (this->number_of_classes_, 0.0f); 00532 for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++) 00533 this->sigmas_[i_class] = other.sigmas_[i_class]; 00534 00535 this->directions_to_center_.resize (this->number_of_visual_words_, 3); 00536 for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++) 00537 for (unsigned int i_dim = 0; i_dim < 3; i_dim++) 00538 this->directions_to_center_ (i_visual_word, i_dim) = other.directions_to_center_ (i_visual_word, i_dim); 00539 00540 this->clusters_centers_.resize (this->number_of_clusters_, 3); 00541 for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++) 00542 for (unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++) 00543 this->clusters_centers_ (i_cluster, i_dim) = other.clusters_centers_ (i_cluster, i_dim); 00544 } 00545 return (*this); 00546 } 00547 00548 ////////////////////////////////////////////////////////////////////////////////////////////// 00549 template <int FeatureSize, typename PointT, typename NormalT> 00550 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::ImplicitShapeModelEstimation () : 00551 training_clouds_ (0), 00552 training_classes_ (0), 00553 training_normals_ (0), 00554 training_sigmas_ (0), 00555 sampling_size_ (0.1f), 00556 feature_estimator_ (), 00557 number_of_clusters_ (184), 00558 n_vot_ON_ (true) 00559 { 00560 } 00561 00562 ////////////////////////////////////////////////////////////////////////////////////////////// 00563 template <int FeatureSize, typename PointT, typename NormalT> 00564 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::~ImplicitShapeModelEstimation () 00565 { 00566 training_clouds_.clear (); 00567 training_classes_.clear (); 00568 training_normals_.clear (); 00569 training_sigmas_.clear (); 00570 feature_estimator_.reset (); 00571 } 00572 00573 ////////////////////////////////////////////////////////////////////////////////////////////// 00574 template <int FeatureSize, typename PointT, typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr> 00575 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::getTrainingClouds () 00576 { 00577 return (training_clouds_); 00578 } 00579 00580 ////////////////////////////////////////////////////////////////////////////////////////////// 00581 template <int FeatureSize, typename PointT, typename NormalT> void 00582 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::setTrainingClouds ( 00583 const std::vector< typename pcl::PointCloud<PointT>::Ptr >& training_clouds) 00584 { 00585 training_clouds_.clear (); 00586 std::vector<typename pcl::PointCloud<PointT>::Ptr > clouds ( training_clouds.begin (), training_clouds.end () ); 00587 training_clouds_.swap (clouds); 00588 } 00589 00590 ////////////////////////////////////////////////////////////////////////////////////////////// 00591 template <int FeatureSize, typename PointT, typename NormalT> std::vector<unsigned int> 00592 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::getTrainingClasses () 00593 { 00594 return (training_classes_); 00595 } 00596 00597 ////////////////////////////////////////////////////////////////////////////////////////////// 00598 template <int FeatureSize, typename PointT, typename NormalT> void 00599 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::setTrainingClasses (const std::vector<unsigned int>& training_classes) 00600 { 00601 training_classes_.clear (); 00602 std::vector<unsigned int> classes ( training_classes.begin (), training_classes.end () ); 00603 training_classes_.swap (classes); 00604 } 00605 00606 ////////////////////////////////////////////////////////////////////////////////////////////// 00607 template <int FeatureSize, typename PointT, typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr> 00608 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::getTrainingNormals () 00609 { 00610 return (training_normals_); 00611 } 00612 00613 ////////////////////////////////////////////////////////////////////////////////////////////// 00614 template <int FeatureSize, typename PointT, typename NormalT> void 00615 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::setTrainingNormals ( 00616 const std::vector< typename pcl::PointCloud<NormalT>::Ptr >& training_normals) 00617 { 00618 training_normals_.clear (); 00619 std::vector<typename pcl::PointCloud<NormalT>::Ptr > normals ( training_normals.begin (), training_normals.end () ); 00620 training_normals_.swap (normals); 00621 } 00622 00623 ////////////////////////////////////////////////////////////////////////////////////////////// 00624 template <int FeatureSize, typename PointT, typename NormalT> float 00625 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::getSamplingSize () 00626 { 00627 return (sampling_size_); 00628 } 00629 00630 ////////////////////////////////////////////////////////////////////////////////////////////// 00631 template <int FeatureSize, typename PointT, typename NormalT> void 00632 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::setSamplingSize (float sampling_size) 00633 { 00634 if (sampling_size >= std::numeric_limits<float>::epsilon ()) 00635 sampling_size_ = sampling_size; 00636 } 00637 00638 ////////////////////////////////////////////////////////////////////////////////////////////// 00639 template <int FeatureSize, typename PointT, typename NormalT> boost::shared_ptr<pcl::Feature<PointT, pcl::Histogram<FeatureSize> > > 00640 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::getFeatureEstimator () 00641 { 00642 return (feature_estimator_); 00643 } 00644 00645 ////////////////////////////////////////////////////////////////////////////////////////////// 00646 template <int FeatureSize, typename PointT, typename NormalT> void 00647 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::setFeatureEstimator ( 00648 boost::shared_ptr<pcl::Feature<PointT, pcl::Histogram<FeatureSize> > > feature) 00649 { 00650 feature_estimator_ = feature; 00651 } 00652 00653 ////////////////////////////////////////////////////////////////////////////////////////////// 00654 template <int FeatureSize, typename PointT, typename NormalT> unsigned int 00655 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::getNumberOfClusters () 00656 { 00657 return (number_of_clusters_); 00658 } 00659 00660 ////////////////////////////////////////////////////////////////////////////////////////////// 00661 template <int FeatureSize, typename PointT, typename NormalT> void 00662 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::setNumberOfClusters (unsigned int num_of_clusters) 00663 { 00664 if (num_of_clusters > 0) 00665 number_of_clusters_ = num_of_clusters; 00666 } 00667 00668 ////////////////////////////////////////////////////////////////////////////////////////////// 00669 template <int FeatureSize, typename PointT, typename NormalT> std::vector<float> 00670 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::getSigmaDists () 00671 { 00672 return (training_sigmas_); 00673 } 00674 00675 ////////////////////////////////////////////////////////////////////////////////////////////// 00676 template <int FeatureSize, typename PointT, typename NormalT> void 00677 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::setSigmaDists (const std::vector<float>& training_sigmas) 00678 { 00679 training_sigmas_.clear (); 00680 std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () ); 00681 training_sigmas_.swap (sigmas); 00682 } 00683 00684 ////////////////////////////////////////////////////////////////////////////////////////////// 00685 template <int FeatureSize, typename PointT, typename NormalT> bool 00686 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::getNVotState () 00687 { 00688 return (n_vot_ON_); 00689 } 00690 00691 ////////////////////////////////////////////////////////////////////////////////////////////// 00692 template <int FeatureSize, typename PointT, typename NormalT> void 00693 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::setNVotState (bool state) 00694 { 00695 n_vot_ON_ = state; 00696 } 00697 00698 ////////////////////////////////////////////////////////////////////////////////////////////// 00699 template <int FeatureSize, typename PointT, typename NormalT> bool 00700 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::trainISM (ISMModelPtr& trained_model) 00701 { 00702 bool success = true; 00703 00704 if (trained_model == 0) 00705 return (false); 00706 trained_model->reset (); 00707 00708 std::vector<pcl::Histogram<FeatureSize> > histograms; 00709 std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations; 00710 success = extractDescriptors (histograms, locations); 00711 if (!success) 00712 return (false); 00713 00714 Eigen::MatrixXi labels; 00715 success = clusterDescriptors(histograms, labels, trained_model->clusters_centers_); 00716 if (!success) 00717 return (false); 00718 00719 std::vector<unsigned int> vec; 00720 trained_model->clusters_.resize (number_of_clusters_, vec); 00721 for (int i_label = 0; i_label < locations.size (); i_label++) 00722 trained_model->clusters_[labels (i_label)].push_back (i_label); 00723 00724 calculateSigmas (trained_model->sigmas_); 00725 00726 calculateWeights( 00727 locations, 00728 labels, 00729 trained_model->sigmas_, 00730 trained_model->clusters_, 00731 trained_model->statistical_weights_, 00732 trained_model->learned_weights_); 00733 00734 trained_model->number_of_classes_ = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1; 00735 trained_model->number_of_visual_words_ = static_cast<unsigned int> (histograms.size ()); 00736 trained_model->number_of_clusters_ = number_of_clusters_; 00737 trained_model->descriptors_dimension_ = FeatureSize; 00738 00739 trained_model->directions_to_center_.resize (locations.size (), 3); 00740 trained_model->classes_.resize (locations.size ()); 00741 for (int i_dir = 0; i_dir < locations.size (); i_dir++) 00742 { 00743 trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x; 00744 trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y; 00745 trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z; 00746 trained_model->classes_[i_dir] = training_classes_[locations[i_dir].model_num_]; 00747 } 00748 00749 return (true); 00750 } 00751 00752 ////////////////////////////////////////////////////////////////////////////////////////////// 00753 template <int FeatureSize, typename PointT, typename NormalT> boost::shared_ptr<pcl::features::ISMVoteList<PointT> > 00754 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::findObjects ( 00755 ISMModelPtr model, 00756 typename pcl::PointCloud<PointT>::Ptr in_cloud, 00757 typename pcl::PointCloud<Normal>::Ptr in_normals, 00758 int in_class_of_interest) 00759 { 00760 boost::shared_ptr<pcl::features::ISMVoteList<PointT> > out_votes (new pcl::features::ISMVoteList<PointT> ()); 00761 00762 if (in_cloud->points.size () == 0) 00763 return (out_votes); 00764 00765 typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud (new pcl::PointCloud<PointT> ()); 00766 typename pcl::PointCloud<NormalT>::Ptr sampled_normal_cloud (new pcl::PointCloud<NormalT> ()); 00767 simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud); 00768 if (sampled_point_cloud->points.size () == 0) 00769 return (out_votes); 00770 00771 typename pcl::PointCloud<pcl::Histogram<FeatureSize> >::Ptr feature_cloud (new pcl::PointCloud<pcl::Histogram<FeatureSize> > ()); 00772 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud); 00773 00774 //find nearest cluster 00775 const unsigned int n_key_points = static_cast<unsigned int> (sampled_point_cloud->size ()); 00776 std::vector<int> min_dist_inds (n_key_points, -1); 00777 for (unsigned int i_point = 0; i_point < n_key_points; i_point++) 00778 { 00779 Eigen::VectorXf curr_descriptor (FeatureSize); 00780 for (int i_dim = 0; i_dim < FeatureSize; i_dim++) 00781 curr_descriptor (i_dim) = feature_cloud->points[i_point].histogram[i_dim]; 00782 00783 float descriptor_sum = curr_descriptor.sum (); 00784 if (descriptor_sum < std::numeric_limits<float>::epsilon ()) 00785 continue; 00786 00787 unsigned int min_dist_idx = 0; 00788 Eigen::VectorXf clusters_center (FeatureSize); 00789 for (int i_dim = 0; i_dim < FeatureSize; i_dim++) 00790 clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim); 00791 00792 float best_dist = computeDistance (curr_descriptor, clusters_center); 00793 for (unsigned int i_clust_cent = 0; i_clust_cent < number_of_clusters_; i_clust_cent++) 00794 { 00795 for (int i_dim = 0; i_dim < FeatureSize; i_dim++) 00796 clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim); 00797 float curr_dist = computeDistance (clusters_center, curr_descriptor); 00798 if (curr_dist < best_dist) 00799 { 00800 min_dist_idx = i_clust_cent; 00801 best_dist = curr_dist; 00802 } 00803 } 00804 min_dist_inds[i_point] = min_dist_idx; 00805 }//next keypoint 00806 00807 for (size_t i_point = 0; i_point < n_key_points; i_point++) 00808 { 00809 int min_dist_idx = min_dist_inds[i_point]; 00810 if (min_dist_idx == -1) 00811 continue; 00812 00813 const unsigned int n_words = static_cast<unsigned int> (model->clusters_[min_dist_idx].size ()); 00814 //compute coord system transform 00815 Eigen::Matrix3f transform = alignYCoordWithNormal (sampled_normal_cloud->points[i_point]); 00816 for (unsigned int i_word = 0; i_word < n_words; i_word++) 00817 { 00818 unsigned int index = model->clusters_[min_dist_idx][i_word]; 00819 unsigned int i_class = model->classes_[index]; 00820 if (i_class != in_class_of_interest) 00821 continue;//skip this class 00822 00823 //rotate dir to center as needed 00824 Eigen::Vector3f direction ( 00825 model->directions_to_center_(index, 0), 00826 model->directions_to_center_(index, 1), 00827 model->directions_to_center_(index, 2)); 00828 applyTransform (direction, transform.transpose ()); 00829 00830 pcl::InterestPoint vote; 00831 Eigen::Vector3f vote_pos = sampled_point_cloud->points[i_point].getVector3fMap () + direction; 00832 vote.x = vote_pos[0]; 00833 vote.y = vote_pos[1]; 00834 vote.z = vote_pos[2]; 00835 float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx]; 00836 float learned_weight = model->learned_weights_[index]; 00837 float power = statistical_weight * learned_weight; 00838 vote.strength = power; 00839 if (vote.strength > std::numeric_limits<float>::epsilon ()) 00840 out_votes->addVote (vote, sampled_point_cloud->points[i_point], i_class); 00841 } 00842 }//next point 00843 00844 return (out_votes); 00845 } 00846 00847 ////////////////////////////////////////////////////////////////////////////////////////////// 00848 template <int FeatureSize, typename PointT, typename NormalT> bool 00849 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::extractDescriptors ( 00850 std::vector< pcl::Histogram<FeatureSize> >& histograms, 00851 std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations) 00852 { 00853 histograms.clear (); 00854 locations.clear (); 00855 00856 int n_key_points = 0; 00857 00858 if (training_clouds_.size () == 0 || training_classes_.size () == 0 || feature_estimator_ == 0) 00859 return (false); 00860 00861 for (size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++) 00862 { 00863 //compute the center of the training object 00864 Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f); 00865 const size_t num_of_points = training_clouds_[i_cloud]->points.size (); 00866 typename pcl::PointCloud<PointT>::iterator point_j; 00867 for (point_j = training_clouds_[i_cloud]->begin (); point_j != training_clouds_[i_cloud]->end (); point_j++) 00868 models_center += point_j->getVector3fMap (); 00869 models_center /= static_cast<float> (num_of_points); 00870 00871 //downsample the cloud 00872 typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud (new pcl::PointCloud<PointT> ()); 00873 typename pcl::PointCloud<NormalT>::Ptr sampled_normal_cloud (new pcl::PointCloud<NormalT> ()); 00874 simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud); 00875 if (sampled_point_cloud->points.size () == 0) 00876 continue; 00877 00878 shiftCloud (training_clouds_[i_cloud], models_center); 00879 shiftCloud (sampled_point_cloud, models_center); 00880 00881 n_key_points += static_cast<int> (sampled_point_cloud->size ()); 00882 00883 typename pcl::PointCloud<pcl::Histogram<FeatureSize> >::Ptr feature_cloud (new pcl::PointCloud<pcl::Histogram<FeatureSize> > ()); 00884 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud); 00885 00886 int point_index = 0; 00887 typename pcl::PointCloud<PointT>::iterator point_i; 00888 for (point_i = sampled_point_cloud->points.begin (); point_i != sampled_point_cloud->points.end (); point_i++, point_index++) 00889 { 00890 float descriptor_sum = Eigen::VectorXf::Map (feature_cloud->points[point_index].histogram, FeatureSize).sum (); 00891 if (descriptor_sum < std::numeric_limits<float>::epsilon ()) 00892 continue; 00893 00894 histograms.insert ( histograms.end (), feature_cloud->begin () + point_index, feature_cloud->begin () + point_index + 1 ); 00895 00896 int dist = static_cast<int> (std::distance (sampled_point_cloud->points.begin (), point_i)); 00897 Eigen::Matrix3f new_basis = alignYCoordWithNormal (sampled_normal_cloud->points[dist]); 00898 Eigen::Vector3f zero; 00899 zero (0) = 0.0; 00900 zero (1) = 0.0; 00901 zero (2) = 0.0; 00902 Eigen::Vector3f new_dir = zero - point_i->getVector3fMap (); 00903 applyTransform (new_dir, new_basis); 00904 00905 PointT point (new_dir[0], new_dir[1], new_dir[2]); 00906 LocationInfo info (static_cast<unsigned int> (i_cloud), point, *point_i, sampled_normal_cloud->points[dist]); 00907 locations.insert(locations.end (), info); 00908 } 00909 }//next training cloud 00910 00911 return (true); 00912 } 00913 00914 ////////////////////////////////////////////////////////////////////////////////////////////// 00915 template <int FeatureSize, typename PointT, typename NormalT> bool 00916 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::clusterDescriptors ( 00917 std::vector< pcl::Histogram<FeatureSize> >& histograms, 00918 Eigen::MatrixXi& labels, 00919 Eigen::MatrixXf& clusters_centers) 00920 { 00921 Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize); 00922 00923 for (unsigned int i_feature = 0; i_feature < histograms.size (); i_feature++) 00924 for (int i_dim = 0; i_dim < FeatureSize; i_dim++) 00925 points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim]; 00926 00927 labels.resize (histograms.size(), 1); 00928 computeKMeansClustering ( 00929 points_to_cluster, 00930 number_of_clusters_, 00931 labels, 00932 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),//1000 00933 5, 00934 PP_CENTERS, 00935 clusters_centers); 00936 00937 return (true); 00938 } 00939 00940 ////////////////////////////////////////////////////////////////////////////////////////////// 00941 template <int FeatureSize, typename PointT, typename NormalT> void 00942 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::calculateSigmas (std::vector<float>& sigmas) 00943 { 00944 if (training_sigmas_.size () != 0) 00945 { 00946 sigmas.resize (training_sigmas_.size (), 0.0f); 00947 for (unsigned int i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++) 00948 sigmas[i_sigma] = training_sigmas_[i_sigma]; 00949 return; 00950 } 00951 00952 sigmas.clear (); 00953 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1; 00954 sigmas.resize (number_of_classes, 0.0f); 00955 00956 std::vector<float> vec; 00957 std::vector<std::vector<float> > objects_sigmas; 00958 objects_sigmas.resize (number_of_classes, vec); 00959 00960 unsigned int number_of_objects = static_cast<unsigned int> (training_clouds_.size ()); 00961 for (unsigned int i_object = 0; i_object < number_of_objects; i_object++) 00962 { 00963 float max_distance = 0.0f; 00964 unsigned int number_of_points = static_cast<unsigned int> (training_clouds_[i_object]->points.size ()); 00965 for (unsigned int i_point = 0; i_point < number_of_points - 1; i_point++) 00966 for (unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++) 00967 { 00968 float curr_distance = 0.0f; 00969 curr_distance += training_clouds_[i_object]->points[i_point].x * training_clouds_[i_object]->points[j_point].x; 00970 curr_distance += training_clouds_[i_object]->points[i_point].y * training_clouds_[i_object]->points[j_point].y; 00971 curr_distance += training_clouds_[i_object]->points[i_point].z * training_clouds_[i_object]->points[j_point].z; 00972 if (curr_distance > max_distance) 00973 max_distance = curr_distance; 00974 } 00975 max_distance = static_cast<float> (sqrt (max_distance)); 00976 unsigned int i_class = training_classes_[i_object]; 00977 objects_sigmas[i_class].push_back (max_distance); 00978 } 00979 00980 for (unsigned int i_class = 0; i_class < number_of_classes; i_class++) 00981 { 00982 float sig = 0.0f; 00983 unsigned int number_of_objects_in_class = static_cast<unsigned int> (objects_sigmas[i_class].size ()); 00984 for (unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++) 00985 sig += objects_sigmas[i_class][i_object]; 00986 sig /= (static_cast<float> (number_of_objects_in_class) * 10.0f); 00987 sigmas[i_class] = sig; 00988 } 00989 } 00990 00991 ////////////////////////////////////////////////////////////////////////////////////////////// 00992 template <int FeatureSize, typename PointT, typename NormalT> void 00993 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::calculateWeights ( 00994 const std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations, 00995 const Eigen::MatrixXi &labels, 00996 std::vector<float>& sigmas, 00997 std::vector<std::vector<unsigned int> >& clusters, 00998 std::vector<std::vector<float> >& statistical_weights, 00999 std::vector<float>& learned_weights) 01000 { 01001 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1; 01002 //Temporary variable 01003 std::vector<float> vec; 01004 vec.resize (number_of_clusters_, 0.0f); 01005 statistical_weights.clear (); 01006 learned_weights.clear (); 01007 statistical_weights.resize (number_of_classes, vec); 01008 learned_weights.resize (locations.size (), 0.0f); 01009 01010 //Temporary variable 01011 std::vector<int> vect; 01012 vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0); 01013 01014 //Number of features from which c_i was learned 01015 std::vector<int> n_ftr; 01016 01017 //Total number of votes from visual word v_j 01018 std::vector<int> n_vot; 01019 01020 //Number of visual words that vote for class c_i 01021 std::vector<int> n_vw; 01022 01023 //Number of votes for class c_i from v_j 01024 std::vector<std::vector<int> > n_vot_2; 01025 01026 n_vot_2.resize (number_of_clusters_, vect); 01027 n_vot.resize (number_of_clusters_, 0); 01028 n_ftr.resize (number_of_classes, 0); 01029 for (int i_location = 0; i_location < locations.size (); i_location++) 01030 { 01031 int i_class = training_classes_[locations[i_location].model_num_]; 01032 int i_cluster = labels (i_location); 01033 n_vot_2[i_cluster][i_class] += 1; 01034 n_vot[i_cluster] += 1; 01035 n_ftr[i_class] += 1; 01036 } 01037 01038 n_vw.resize (number_of_classes, 0); 01039 for (unsigned int i_class = 0; i_class < number_of_classes; i_class++) 01040 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) 01041 if (n_vot_2[i_cluster][i_class] > 0) 01042 n_vw[i_class] += 1; 01043 01044 //computing learned weights 01045 learned_weights.resize (locations.size (), 0.0); 01046 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) 01047 { 01048 unsigned int number_of_words_in_cluster = static_cast<unsigned int> (clusters[i_cluster].size ()); 01049 for (unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++) 01050 { 01051 unsigned int i_index = clusters[i_cluster][i_visual_word]; 01052 int i_class = training_classes_[locations[i_index].model_num_]; 01053 float square_sigma_dist = sigmas[i_class] * sigmas[i_class]; 01054 if (square_sigma_dist < std::numeric_limits<float>::epsilon ()) 01055 { 01056 std::vector<float> calculated_sigmas; 01057 calculateSigmas (calculated_sigmas); 01058 square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class]; 01059 if (square_sigma_dist < std::numeric_limits<float>::epsilon ()) 01060 continue; 01061 } 01062 Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_); 01063 Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap (); 01064 applyTransform (direction, transform); 01065 Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction; 01066 01067 //collect gaussian weighted distances 01068 std::vector<float> gauss_dists; 01069 for (unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++) 01070 { 01071 unsigned int j_index = clusters[i_cluster][j_visual_word]; 01072 int j_class = training_classes_[locations[j_index].model_num_]; 01073 if (i_class != j_class) 01074 continue; 01075 //predict center 01076 Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_); 01077 Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap (); 01078 applyTransform (direction_2, transform_2); 01079 Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2; 01080 float residual = (predicted_center - actual_center).norm (); 01081 float value = -residual * residual / square_sigma_dist; 01082 gauss_dists.push_back (static_cast<float> (exp (value))); 01083 }//next word 01084 //find median gaussian weighted distance 01085 size_t mid_elem = (gauss_dists.size () - 1) / 2; 01086 std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ()); 01087 learned_weights[i_index] = *(gauss_dists.begin () + mid_elem); 01088 }//next word 01089 }//next cluster 01090 01091 //computing statistical weights 01092 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) 01093 { 01094 for (unsigned int i_class = 0; i_class < number_of_classes; i_class++) 01095 { 01096 if (n_vot_2[i_cluster][i_class] == 0) 01097 continue;//no votes per class of interest in this cluster 01098 if (n_vw[i_class] == 0) 01099 continue;//there were no objects of this class in the training dataset 01100 if (n_vot[i_cluster] == 0) 01101 continue;//this cluster has never been used 01102 if (n_ftr[i_class] == 0) 01103 continue;//there were no objects of this class in the training dataset 01104 float part_1 = static_cast<float> (n_vw[i_class]); 01105 float part_2 = static_cast<float> (n_vot[i_cluster]); 01106 float part_3 = static_cast<float> (n_vot_2[i_cluster][i_class]) / static_cast<float> (n_ftr[i_class]); 01107 float part_4 = 0.0f; 01108 01109 if (!n_vot_ON_) 01110 part_2 = 1.0f; 01111 01112 for (unsigned int j_class = 0; j_class < number_of_classes; j_class++) 01113 if (n_ftr[j_class] != 0) 01114 part_4 += static_cast<float> (n_vot_2[i_cluster][j_class]) / static_cast<float> (n_ftr[j_class]); 01115 01116 statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4; 01117 } 01118 }//next cluster 01119 } 01120 01121 ////////////////////////////////////////////////////////////////////////////////////////////// 01122 template <int FeatureSize, typename PointT, typename NormalT> void 01123 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::simplifyCloud ( 01124 typename pcl::PointCloud<PointT>::ConstPtr in_point_cloud, 01125 typename pcl::PointCloud<NormalT>::ConstPtr in_normal_cloud, 01126 typename pcl::PointCloud<PointT>::Ptr out_sampled_point_cloud, 01127 typename pcl::PointCloud<NormalT>::Ptr out_sampled_normal_cloud) 01128 { 01129 //create voxel grid 01130 pcl::VoxelGrid<PointT> grid; 01131 grid.setLeafSize (sampling_size_, sampling_size_, sampling_size_); 01132 grid.setSaveLeafLayout (true); 01133 grid.setInputCloud (in_point_cloud); 01134 01135 pcl::PointCloud<PointT> temp_cloud; 01136 grid.filter (temp_cloud); 01137 01138 //extract indices of points from source cloud which are closest to grid points 01139 const float max_value = std::numeric_limits<float>::max (); 01140 01141 const size_t num_source_points = in_point_cloud->points.size (); 01142 const size_t num_sample_points = temp_cloud.points.size (); 01143 01144 std::vector<float> dist_to_grid_center (num_sample_points, max_value); 01145 std::vector<int> sampling_indices (num_sample_points, -1); 01146 01147 for (size_t i_point = 0; i_point < num_source_points; i_point++) 01148 { 01149 int index = grid.getCentroidIndex (in_point_cloud->points[i_point]); 01150 if (index == -1) 01151 continue; 01152 01153 PointT pt_1 = in_point_cloud->points[i_point]; 01154 PointT pt_2 = temp_cloud.points[index]; 01155 01156 float distance = (pt_1.x - pt_2.x) * (pt_1.x - pt_2.x) + (pt_1.y - pt_2.y) * (pt_1.y - pt_2.y) + (pt_1.z - pt_2.z) * (pt_1.z - pt_2.z); 01157 if (distance < dist_to_grid_center[index]) 01158 { 01159 dist_to_grid_center[index] = distance; 01160 sampling_indices[index] = static_cast<int> (i_point); 01161 } 01162 } 01163 01164 //extract source points 01165 pcl::PointIndices::Ptr final_inliers_indices (new pcl::PointIndices ()); 01166 pcl::ExtractIndices<PointT> extract_points; 01167 pcl::ExtractIndices<NormalT> extract_normals; 01168 01169 final_inliers_indices->indices.reserve (num_sample_points); 01170 for (size_t i_point = 0; i_point < num_sample_points; i_point++) 01171 { 01172 if (sampling_indices[i_point] != -1) 01173 final_inliers_indices->indices.push_back ( sampling_indices[i_point] ); 01174 } 01175 01176 extract_points.setInputCloud (in_point_cloud); 01177 extract_points.setIndices (final_inliers_indices); 01178 extract_points.filter (*out_sampled_point_cloud); 01179 01180 extract_normals.setInputCloud (in_normal_cloud); 01181 extract_normals.setIndices (final_inliers_indices); 01182 extract_normals.filter (*out_sampled_normal_cloud); 01183 } 01184 01185 ////////////////////////////////////////////////////////////////////////////////////////////// 01186 template <int FeatureSize, typename PointT, typename NormalT> void 01187 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::shiftCloud ( 01188 typename pcl::PointCloud<PointT>::Ptr in_cloud, 01189 Eigen::Vector3f shift_point) 01190 { 01191 typename pcl::PointCloud<PointT>::iterator point_it; 01192 for (point_it = in_cloud->points.begin (); point_it != in_cloud->points.end (); point_it++) 01193 { 01194 point_it->x -= shift_point.x (); 01195 point_it->y -= shift_point.y (); 01196 point_it->z -= shift_point.z (); 01197 } 01198 } 01199 01200 ////////////////////////////////////////////////////////////////////////////////////////////// 01201 template <int FeatureSize, typename PointT, typename NormalT> Eigen::Matrix3f 01202 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::alignYCoordWithNormal (const NormalT& in_normal) 01203 { 01204 Eigen::Matrix3f result; 01205 Eigen::Matrix3f rotation_matrix_X; 01206 Eigen::Matrix3f rotation_matrix_Z; 01207 01208 float A = 0.0f; 01209 float B = 0.0f; 01210 float sign = -1.0f; 01211 01212 float denom_X = static_cast<float> (sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y)); 01213 A = in_normal.normal_y / denom_X; 01214 B = sign * in_normal.normal_z / denom_X; 01215 rotation_matrix_X << 1.0f, 0.0f, 0.0f, 01216 0.0f, A, -B, 01217 0.0f, B, A; 01218 01219 float denom_Z = static_cast<float> (sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y)); 01220 A = in_normal.normal_y / denom_Z; 01221 B = sign * in_normal.normal_x / denom_Z; 01222 rotation_matrix_Z << A, -B, 0.0f, 01223 B, A, 0.0f, 01224 0.0f, 0.0f, 1.0f; 01225 01226 result = rotation_matrix_X * rotation_matrix_Z; 01227 01228 return (result); 01229 } 01230 01231 ////////////////////////////////////////////////////////////////////////////////////////////// 01232 template <int FeatureSize, typename PointT, typename NormalT> void 01233 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::applyTransform (Eigen::Vector3f& io_vec, const Eigen::Matrix3f& in_transform) 01234 { 01235 io_vec = in_transform * io_vec; 01236 } 01237 01238 ////////////////////////////////////////////////////////////////////////////////////////////// 01239 template <int FeatureSize, typename PointT, typename NormalT> void 01240 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::estimateFeatures ( 01241 typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud, 01242 typename pcl::PointCloud<NormalT>::Ptr normal_cloud, 01243 typename pcl::PointCloud<pcl::Histogram<FeatureSize> >::Ptr feature_cloud) 01244 { 01245 typename pcl::search::Search<PointT>::Ptr tree = boost::shared_ptr<pcl::search::Search<PointT> > (new pcl::search::KdTree<PointT>); 01246 // tree->setInputCloud (point_cloud); 01247 01248 feature_estimator_->setInputCloud (sampled_point_cloud->makeShared ()); 01249 // feature_estimator_->setSearchSurface (point_cloud->makeShared ()); 01250 feature_estimator_->setSearchMethod (tree); 01251 01252 // typename pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> >::Ptr feat_est_norm = 01253 // boost::dynamic_pointer_cast<pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_); 01254 // feat_est_norm->setInputNormals (normal_cloud); 01255 01256 typename pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> >::Ptr feat_est_norm = 01257 boost::dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_); 01258 feat_est_norm->setInputNormals (normal_cloud); 01259 01260 feature_estimator_->compute (*feature_cloud); 01261 } 01262 01263 ////////////////////////////////////////////////////////////////////////////////////////////// 01264 template <int FeatureSize, typename PointT, typename NormalT> double 01265 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::computeKMeansClustering ( 01266 const Eigen::MatrixXf& points_to_cluster, 01267 int number_of_clusters, 01268 Eigen::MatrixXi& io_labels, 01269 TermCriteria criteria, 01270 int attempts, 01271 int flags, 01272 Eigen::MatrixXf& cluster_centers) 01273 { 01274 const int spp_trials = 3; 01275 size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols (); 01276 int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1; 01277 01278 attempts = std::max (attempts, 1); 01279 srand (static_cast<unsigned int> (time (0))); 01280 01281 Eigen::MatrixXi labels (number_of_points, 1); 01282 01283 if (flags & USE_INITIAL_LABELS) 01284 labels = io_labels; 01285 else 01286 labels.setZero (); 01287 01288 Eigen::MatrixXf centers (number_of_clusters, feature_dimension); 01289 Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension); 01290 std::vector<int> counters (number_of_clusters); 01291 std::vector<Eigen::Vector2f> boxes (feature_dimension); 01292 Eigen::Vector2f* box = &boxes[0]; 01293 01294 double best_compactness = std::numeric_limits<double>::max (); 01295 double compactness = 0.0; 01296 01297 if (criteria.type_ & TermCriteria::EPS) 01298 criteria.epsilon_ = std::max (criteria.epsilon_, 0.0f); 01299 else 01300 criteria.epsilon_ = std::numeric_limits<float>::epsilon (); 01301 01302 criteria.epsilon_ *= criteria.epsilon_; 01303 01304 if (criteria.type_ & TermCriteria::COUNT) 01305 criteria.max_count_ = std::min (std::max (criteria.max_count_, 2), 100); 01306 else 01307 criteria.max_count_ = 100; 01308 01309 if (number_of_clusters == 1) 01310 { 01311 attempts = 1; 01312 criteria.max_count_ = 2; 01313 } 01314 01315 for (int i_dim = 0; i_dim < feature_dimension; i_dim++) 01316 box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim)); 01317 01318 for (unsigned int i_point = 0; i_point < number_of_points; i_point++) 01319 for (int i_dim = 0; i_dim < feature_dimension; i_dim++) 01320 { 01321 float v = points_to_cluster (i_point, i_dim); 01322 box[i_dim] (0) = std::min (box[i_dim] (0), v); 01323 box[i_dim] (1) = std::max (box[i_dim] (1), v); 01324 } 01325 01326 for (int i_attempt = 0; i_attempt < attempts; i_attempt++) 01327 { 01328 float max_center_shift = std::numeric_limits<float>::max (); 01329 for (int iter = 0; iter < criteria.max_count_ && max_center_shift > criteria.epsilon_; iter++) 01330 { 01331 Eigen::MatrixXf temp (centers.rows (), centers.cols ()); 01332 temp = centers; 01333 centers = old_centers; 01334 old_centers = temp; 01335 01336 if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) ) 01337 { 01338 if (flags & PP_CENTERS) 01339 generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials); 01340 else 01341 { 01342 for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++) 01343 { 01344 Eigen::VectorXf center (feature_dimension); 01345 generateRandomCenter (boxes, center); 01346 for (int i_dim = 0; i_dim < feature_dimension; i_dim++) 01347 centers (i_cl_center, i_dim) = center (i_dim); 01348 }//generate center for next cluster 01349 }//end if-else random or PP centers 01350 } 01351 else 01352 { 01353 centers.setZero (); 01354 for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++) 01355 counters[i_cluster] = 0; 01356 for (unsigned int i_point = 0; i_point < number_of_points; i_point++) 01357 { 01358 int i_label = labels (i_point, 0); 01359 for (int i_dim = 0; i_dim < feature_dimension; i_dim++) 01360 centers (i_label, i_dim) += points_to_cluster (i_point, i_dim); 01361 counters[i_label]++; 01362 } 01363 if (iter > 0) 01364 max_center_shift = 0.0f; 01365 for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++) 01366 { 01367 if (counters[i_cl_center] != 0) 01368 { 01369 float scale = 1.0f / static_cast<float> (counters[i_cl_center]); 01370 for (int i_dim = 0; i_dim < feature_dimension; i_dim++) 01371 centers (i_cl_center, i_dim) *= scale; 01372 } 01373 else 01374 { 01375 Eigen::VectorXf center (feature_dimension); 01376 generateRandomCenter (boxes, center); 01377 for(int i_dim = 0; i_dim < feature_dimension; i_dim++) 01378 centers (i_cl_center, i_dim) = center (i_dim); 01379 } 01380 01381 if (iter > 0) 01382 { 01383 float dist = 0.0f; 01384 for (int i_dim = 0; i_dim < feature_dimension; i_dim++) 01385 { 01386 float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim); 01387 dist += diff * diff; 01388 } 01389 max_center_shift = std::max (max_center_shift, dist); 01390 } 01391 } 01392 } 01393 compactness = 0.0f; 01394 for (unsigned int i_point = 0; i_point < number_of_points; i_point++) 01395 { 01396 Eigen::VectorXf sample (feature_dimension); 01397 sample = points_to_cluster.row (i_point); 01398 01399 int k_best = 0; 01400 float min_dist = std::numeric_limits<float>::max (); 01401 01402 for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++) 01403 { 01404 Eigen::VectorXf center (feature_dimension); 01405 center = centers.row (i_cluster); 01406 float dist = computeDistance (sample, center); 01407 if (min_dist > dist) 01408 { 01409 min_dist = dist; 01410 k_best = i_cluster; 01411 } 01412 } 01413 compactness += min_dist; 01414 labels (i_point, 0) = k_best; 01415 } 01416 }//next iteration 01417 01418 if (compactness < best_compactness) 01419 { 01420 best_compactness = compactness; 01421 cluster_centers = centers; 01422 io_labels = labels; 01423 } 01424 }//next attempt 01425 01426 return (best_compactness); 01427 } 01428 01429 ////////////////////////////////////////////////////////////////////////////////////////////// 01430 template <int FeatureSize, typename PointT, typename NormalT> void 01431 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::generateCentersPP ( 01432 const Eigen::MatrixXf& data, 01433 Eigen::MatrixXf& out_centers, 01434 int number_of_clusters, 01435 int trials) 01436 { 01437 size_t dimension = data.cols (); 01438 unsigned int number_of_points = static_cast<unsigned int> (data.rows ()); 01439 std::vector<int> centers_vec (number_of_clusters); 01440 int* centers = ¢ers_vec[0]; 01441 std::vector<double> dist (number_of_points); 01442 std::vector<double> tdist (number_of_points); 01443 std::vector<double> tdist2 (number_of_points); 01444 double sum0 = 0.0; 01445 01446 unsigned int random_unsigned = rand (); 01447 centers[0] = random_unsigned % number_of_points; 01448 01449 for (unsigned int i_point = 0; i_point < number_of_points; i_point++) 01450 { 01451 Eigen::VectorXf first (dimension); 01452 Eigen::VectorXf second (dimension); 01453 first = data.row (i_point); 01454 second = data.row (centers[0]); 01455 dist[i_point] = computeDistance (first, second); 01456 sum0 += dist[i_point]; 01457 } 01458 01459 for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++) 01460 { 01461 double best_sum = std::numeric_limits<double>::max (); 01462 int best_center = -1; 01463 for (int i_trials = 0; i_trials < trials; i_trials++) 01464 { 01465 unsigned int random_integer = rand () - 1; 01466 double random_double = static_cast<double> (random_integer) / static_cast<double> (std::numeric_limits<unsigned int>::max ()); 01467 double p = random_double * sum0; 01468 01469 unsigned int i_point; 01470 for (i_point = 0; i_point < number_of_points - 1; i_point++) 01471 if ( (p -= dist[i_point]) <= 0.0) 01472 break; 01473 01474 int ci = i_point; 01475 01476 double s = 0.0; 01477 for (unsigned int i_point = 0; i_point < number_of_points; i_point++) 01478 { 01479 Eigen::VectorXf first (dimension); 01480 Eigen::VectorXf second (dimension); 01481 first = data.row (i_point); 01482 second = data.row (ci); 01483 tdist2[i_point] = std::min (static_cast<double> (computeDistance (first, second)), dist[i_point]); 01484 s += tdist2[i_point]; 01485 } 01486 01487 if (s <= best_sum) 01488 { 01489 best_sum = s; 01490 best_center = ci; 01491 std::swap (tdist, tdist2); 01492 } 01493 } 01494 01495 centers[i_cluster] = best_center; 01496 sum0 = best_sum; 01497 std::swap (dist, tdist); 01498 } 01499 01500 for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++) 01501 for (unsigned int i_dim = 0; i_dim < dimension; i_dim++) 01502 out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim); 01503 } 01504 01505 ////////////////////////////////////////////////////////////////////////////////////////////// 01506 template <int FeatureSize, typename PointT, typename NormalT> void 01507 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::generateRandomCenter (const std::vector<Eigen::Vector2f>& boxes, 01508 Eigen::VectorXf& center) 01509 { 01510 size_t dimension = boxes.size (); 01511 float margin = 1.0f / static_cast<float> (dimension); 01512 01513 for (unsigned int i_dim = 0; i_dim < dimension; i_dim++) 01514 { 01515 unsigned int random_integer = rand () - 1; 01516 float random_float = static_cast<float> (random_integer) / static_cast<float> (std::numeric_limits<unsigned int>::max ()); 01517 center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0); 01518 } 01519 } 01520 01521 ////////////////////////////////////////////////////////////////////////////////////////////// 01522 template <int FeatureSize, typename PointT, typename NormalT> float 01523 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::computeDistance (Eigen::VectorXf& vec_1, Eigen::VectorXf& vec_2) 01524 { 01525 size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols (); 01526 float distance = 0.0f; 01527 for(unsigned int i_dim = 0; i_dim < dimension; i_dim++) 01528 { 01529 float diff = vec_1 (i_dim) - vec_2 (i_dim); 01530 distance += diff * diff; 01531 } 01532 01533 return (distance); 01534 } 01535 01536 #endif //#ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_