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 * 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 Willow Garage, Inc. 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 */ 00037 00038 #ifndef PCL_RECOGNITION_COLOR_MODALITY 00039 #define PCL_RECOGNITION_COLOR_MODALITY 00040 00041 #include <pcl/recognition/quantizable_modality.h> 00042 #include <pcl/recognition/distance_map.h> 00043 00044 #include <pcl/pcl_base.h> 00045 #include <pcl/point_cloud.h> 00046 #include <pcl/point_types.h> 00047 #include <pcl/recognition/point_types.h> 00048 00049 00050 namespace pcl 00051 { 00052 00053 // -------------------------------------------------------------------------- 00054 00055 template <typename PointInT> 00056 class ColorModality 00057 : public QuantizableModality, public PCLBase<PointInT> 00058 { 00059 protected: 00060 using PCLBase<PointInT>::input_; 00061 00062 struct Candidate 00063 { 00064 float distance; 00065 00066 unsigned char bin_index; 00067 00068 size_t x; 00069 size_t y; 00070 00071 bool 00072 operator< (const Candidate & rhs) 00073 { 00074 return (distance > rhs.distance); 00075 } 00076 }; 00077 00078 public: 00079 typedef typename pcl::PointCloud<PointInT> PointCloudIn; 00080 00081 ColorModality (); 00082 00083 virtual ~ColorModality (); 00084 00085 inline QuantizedMap & 00086 getQuantizedMap () 00087 { 00088 return (filtered_quantized_colors_); 00089 } 00090 00091 inline QuantizedMap & 00092 getSpreadedQuantizedMap () 00093 { 00094 return (spreaded_filtered_quantized_colors_); 00095 } 00096 00097 void 00098 extractFeatures (const MaskMap & mask, size_t nr_features, size_t modalityIndex, 00099 std::vector<QuantizedMultiModFeature> & features) const; 00100 00101 /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) 00102 * \param cloud the const boost shared pointer to a PointCloud message 00103 */ 00104 virtual void 00105 setInputCloud (const typename PointCloudIn::ConstPtr & cloud) 00106 { 00107 input_ = cloud; 00108 } 00109 00110 virtual void 00111 processInputData (); 00112 00113 protected: 00114 00115 void 00116 quantizeColors (); 00117 00118 void 00119 filterQuantizedColors (); 00120 00121 static inline int 00122 quantizeColorOnRGBExtrema (const float r, 00123 const float g, 00124 const float b); 00125 00126 void 00127 computeDistanceMap (const MaskMap & input, DistanceMap & output) const; 00128 00129 private: 00130 float feature_distance_threshold_; 00131 00132 pcl::QuantizedMap quantized_colors_; 00133 pcl::QuantizedMap filtered_quantized_colors_; 00134 pcl::QuantizedMap spreaded_filtered_quantized_colors_; 00135 00136 }; 00137 00138 } 00139 00140 ////////////////////////////////////////////////////////////////////////////////////////////// 00141 template <typename PointInT> 00142 pcl::ColorModality<PointInT>::ColorModality () 00143 : feature_distance_threshold_ (1.0f), quantized_colors_ (), filtered_quantized_colors_ (), spreaded_filtered_quantized_colors_ () 00144 { 00145 } 00146 00147 ////////////////////////////////////////////////////////////////////////////////////////////// 00148 template <typename PointInT> 00149 pcl::ColorModality<PointInT>::~ColorModality () 00150 { 00151 } 00152 00153 ////////////////////////////////////////////////////////////////////////////////////////////// 00154 template <typename PointInT> 00155 void 00156 pcl::ColorModality<PointInT>::processInputData () 00157 { 00158 // quantize gradients 00159 quantizeColors (); 00160 00161 // filter quantized gradients to get only dominants one + thresholding 00162 filterQuantizedColors (); 00163 00164 // spread filtered quantized gradients 00165 //spreadFilteredQunatizedColorGradients (); 00166 const int spreading_size = 8; 00167 pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_colors_, 00168 spreaded_filtered_quantized_colors_, spreading_size); 00169 } 00170 00171 ////////////////////////////////////////////////////////////////////////////////////////////// 00172 template <typename PointInT> 00173 void pcl::ColorModality<PointInT>::extractFeatures (const MaskMap & mask, 00174 const size_t nr_features, 00175 const size_t modality_index, 00176 std::vector<QuantizedMultiModFeature> & features) const 00177 { 00178 const size_t width = mask.getWidth (); 00179 const size_t height = mask.getHeight (); 00180 00181 MaskMap mask_maps[8]; 00182 for (size_t map_index = 0; map_index < 8; ++map_index) 00183 mask_maps[map_index].resize (width, height); 00184 00185 unsigned char map[255]; 00186 memset(map, 0, 255); 00187 00188 map[0x1<<0] = 0; 00189 map[0x1<<1] = 1; 00190 map[0x1<<2] = 2; 00191 map[0x1<<3] = 3; 00192 map[0x1<<4] = 4; 00193 map[0x1<<5] = 5; 00194 map[0x1<<6] = 6; 00195 map[0x1<<7] = 7; 00196 00197 QuantizedMap distance_map_indices (width, height); 00198 //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height); 00199 00200 for (size_t row_index = 0; row_index < height; ++row_index) 00201 { 00202 for (size_t col_index = 0; col_index < width; ++col_index) 00203 { 00204 if (mask (col_index, row_index) != 0) 00205 { 00206 //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index); 00207 const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index); 00208 00209 if (quantized_value == 0) 00210 continue; 00211 const int dist_map_index = map[quantized_value]; 00212 00213 distance_map_indices (col_index, row_index) = dist_map_index; 00214 //distance_maps[dist_map_index].at<unsigned char>(row_index, col_index) = 255; 00215 mask_maps[dist_map_index] (col_index, row_index) = 255; 00216 } 00217 } 00218 } 00219 00220 DistanceMap distance_maps[8]; 00221 for (int map_index = 0; map_index < 8; ++map_index) 00222 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]); 00223 00224 std::list<Candidate> list1; 00225 std::list<Candidate> list2; 00226 00227 float weights[8] = {0,0,0,0,0,0,0,0}; 00228 00229 const size_t off = 4; 00230 for (size_t row_index = off; row_index < height-off; ++row_index) 00231 { 00232 for (size_t col_index = off; col_index < width-off; ++col_index) 00233 { 00234 if (mask (col_index, row_index) != 0) 00235 { 00236 //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index); 00237 const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index); 00238 00239 //const float nx = surface_normals_ (col_index, row_index).normal_x; 00240 //const float ny = surface_normals_ (col_index, row_index).normal_y; 00241 //const float nz = surface_normals_ (col_index, row_index).normal_z; 00242 00243 if (quantized_value != 0) 00244 { 00245 const int distance_map_index = map[quantized_value]; 00246 00247 //const float distance = distance_maps[distance_map_index].at<float> (row_index, col_index); 00248 const float distance = distance_maps[distance_map_index] (col_index, row_index); 00249 00250 if (distance >= feature_distance_threshold_) 00251 { 00252 Candidate candidate; 00253 00254 candidate.distance = distance; 00255 candidate.x = col_index; 00256 candidate.y = row_index; 00257 candidate.bin_index = distance_map_index; 00258 00259 list1.push_back (candidate); 00260 00261 ++weights[distance_map_index]; 00262 } 00263 } 00264 } 00265 } 00266 } 00267 00268 for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter) 00269 iter->distance *= 1.0f / weights[iter->bin_index]; 00270 00271 list1.sort (); 00272 00273 if (list1.size () <= nr_features) 00274 { 00275 features.reserve (list1.size ()); 00276 for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter) 00277 { 00278 QuantizedMultiModFeature feature; 00279 00280 feature.x = static_cast<int> (iter->x); 00281 feature.y = static_cast<int> (iter->y); 00282 feature.modality_index = modality_index; 00283 feature.quantized_value = filtered_quantized_colors_ (iter->x, iter->y); 00284 00285 features.push_back (feature); 00286 } 00287 00288 return; 00289 } 00290 00291 int distance = static_cast<int> (list1.size () / nr_features + 1); // ??? @todo:!:!:!:!:!:! 00292 while (list2.size () != nr_features) 00293 { 00294 const int sqr_distance = distance*distance; 00295 for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1) 00296 { 00297 bool candidate_accepted = true; 00298 00299 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) 00300 { 00301 const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x); 00302 const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y); 00303 const int tmp_distance = dx*dx + dy*dy; 00304 00305 if (tmp_distance < sqr_distance) 00306 { 00307 candidate_accepted = false; 00308 break; 00309 } 00310 } 00311 00312 if (candidate_accepted) 00313 list2.push_back (*iter1); 00314 00315 if (list2.size () == nr_features) break; 00316 } 00317 --distance; 00318 } 00319 00320 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) 00321 { 00322 QuantizedMultiModFeature feature; 00323 00324 feature.x = static_cast<int> (iter2->x); 00325 feature.y = static_cast<int> (iter2->y); 00326 feature.modality_index = modality_index; 00327 feature.quantized_value = filtered_quantized_colors_ (iter2->x, iter2->y); 00328 00329 features.push_back (feature); 00330 } 00331 } 00332 00333 ////////////////////////////////////////////////////////////////////////////////////////////// 00334 template <typename PointInT> 00335 void 00336 pcl::ColorModality<PointInT>::quantizeColors () 00337 { 00338 const size_t width = input_->width; 00339 const size_t height = input_->height; 00340 00341 quantized_colors_.resize (width, height); 00342 00343 for (size_t row_index = 0; row_index < height; ++row_index) 00344 { 00345 for (size_t col_index = 0; col_index < width; ++col_index) 00346 { 00347 const float r = static_cast<float> ((*input_) (col_index, row_index).r); 00348 const float g = static_cast<float> ((*input_) (col_index, row_index).g); 00349 const float b = static_cast<float> ((*input_) (col_index, row_index).b); 00350 00351 quantized_colors_ (col_index, row_index) = quantizeColorOnRGBExtrema (r, g, b); 00352 } 00353 } 00354 } 00355 00356 ////////////////////////////////////////////////////////////////////////////////////////////// 00357 template <typename PointInT> 00358 void 00359 pcl::ColorModality<PointInT>::filterQuantizedColors () 00360 { 00361 const size_t width = input_->width; 00362 const size_t height = input_->height; 00363 00364 filtered_quantized_colors_.resize (width, height); 00365 00366 // filter data 00367 for (size_t row_index = 1; row_index < height-1; ++row_index) 00368 { 00369 for (size_t col_index = 1; col_index < width-1; ++col_index) 00370 { 00371 unsigned char histogram[8] = {0,0,0,0,0,0,0,0}; 00372 00373 { 00374 const unsigned char * data_ptr = quantized_colors_.getData () + (row_index-1)*width+col_index-1; 00375 assert (0 <= data_ptr[0] && data_ptr[0] < 9 && 00376 0 <= data_ptr[1] && data_ptr[1] < 9 && 00377 0 <= data_ptr[2] && data_ptr[2] < 9); 00378 ++histogram[data_ptr[0]]; 00379 ++histogram[data_ptr[1]]; 00380 ++histogram[data_ptr[2]]; 00381 } 00382 { 00383 const unsigned char * data_ptr = quantized_colors_.getData () + row_index*width+col_index-1; 00384 assert (0 <= data_ptr[0] && data_ptr[0] < 9 && 00385 0 <= data_ptr[1] && data_ptr[1] < 9 && 00386 0 <= data_ptr[2] && data_ptr[2] < 9); 00387 ++histogram[data_ptr[0]]; 00388 ++histogram[data_ptr[1]]; 00389 ++histogram[data_ptr[2]]; 00390 } 00391 { 00392 const unsigned char * data_ptr = quantized_colors_.getData () + (row_index+1)*width+col_index-1; 00393 assert (0 <= data_ptr[0] && data_ptr[0] < 9 && 00394 0 <= data_ptr[1] && data_ptr[1] < 9 && 00395 0 <= data_ptr[2] && data_ptr[2] < 9); 00396 ++histogram[data_ptr[0]]; 00397 ++histogram[data_ptr[1]]; 00398 ++histogram[data_ptr[2]]; 00399 } 00400 00401 unsigned char max_hist_value = 0; 00402 int max_hist_index = -1; 00403 00404 // for (int i = 0; i < 8; ++i) 00405 // { 00406 // if (max_hist_value < histogram[i+1]) 00407 // { 00408 // max_hist_index = i; 00409 // max_hist_value = histogram[i+1] 00410 // } 00411 // } 00412 // Unrolled for performance optimization: 00413 if (max_hist_value < histogram[0]) {max_hist_index = 0; max_hist_value = histogram[0];} 00414 if (max_hist_value < histogram[1]) {max_hist_index = 1; max_hist_value = histogram[1];} 00415 if (max_hist_value < histogram[2]) {max_hist_index = 2; max_hist_value = histogram[2];} 00416 if (max_hist_value < histogram[3]) {max_hist_index = 3; max_hist_value = histogram[3];} 00417 if (max_hist_value < histogram[4]) {max_hist_index = 4; max_hist_value = histogram[4];} 00418 if (max_hist_value < histogram[5]) {max_hist_index = 5; max_hist_value = histogram[5];} 00419 if (max_hist_value < histogram[6]) {max_hist_index = 6; max_hist_value = histogram[6];} 00420 if (max_hist_value < histogram[7]) {max_hist_index = 7; max_hist_value = histogram[7];} 00421 00422 //if (max_hist_index != -1 && max_hist_value >= 5) 00423 filtered_quantized_colors_ (col_index, row_index) = 0x1 << max_hist_index; 00424 //else 00425 // filtered_quantized_color_gradients_ (col_index, row_index) = 0; 00426 00427 } 00428 } 00429 } 00430 00431 ////////////////////////////////////////////////////////////////////////////////////////////// 00432 template <typename PointInT> 00433 int 00434 pcl::ColorModality<PointInT>::quantizeColorOnRGBExtrema (const float r, 00435 const float g, 00436 const float b) 00437 { 00438 const float r_inv = 255.0f-r; 00439 const float g_inv = 255.0f-g; 00440 const float b_inv = 255.0f-b; 00441 00442 const float dist_0 = (r*r + g*g + b*b)*2.0f; 00443 const float dist_1 = r*r + g*g + b_inv*b_inv; 00444 const float dist_2 = r*r + g_inv*g_inv+ b*b; 00445 const float dist_3 = r*r + g_inv*g_inv + b_inv*b_inv; 00446 const float dist_4 = r_inv*r_inv + g*g + b*b; 00447 const float dist_5 = r_inv*r_inv + g*g + b_inv*b_inv; 00448 const float dist_6 = r_inv*r_inv + g_inv*g_inv+ b*b; 00449 const float dist_7 = (r_inv*r_inv + g_inv*g_inv + b_inv*b_inv)*1.5f; 00450 00451 const float min_dist = std::min (std::min (std::min (dist_0, dist_1), std::min (dist_2, dist_3)), std::min (std::min (dist_4, dist_5), std::min (dist_6, dist_7))); 00452 00453 if (min_dist == dist_0) 00454 { 00455 return 0; 00456 } 00457 if (min_dist == dist_1) 00458 { 00459 return 1; 00460 } 00461 if (min_dist == dist_2) 00462 { 00463 return 2; 00464 } 00465 if (min_dist == dist_3) 00466 { 00467 return 3; 00468 } 00469 if (min_dist == dist_4) 00470 { 00471 return 4; 00472 } 00473 if (min_dist == dist_5) 00474 { 00475 return 5; 00476 } 00477 if (min_dist == dist_6) 00478 { 00479 return 6; 00480 } 00481 return 7; 00482 } 00483 00484 ////////////////////////////////////////////////////////////////////////////////////////////// 00485 template <typename PointInT> void 00486 pcl::ColorModality<PointInT>::computeDistanceMap (const MaskMap & input, 00487 DistanceMap & output) const 00488 { 00489 const size_t width = input.getWidth (); 00490 const size_t height = input.getHeight (); 00491 00492 output.resize (width, height); 00493 00494 // compute distance map 00495 //float *distance_map = new float[input_->points.size ()]; 00496 const unsigned char * mask_map = input.getData (); 00497 float * distance_map = output.getData (); 00498 for (size_t index = 0; index < width*height; ++index) 00499 { 00500 if (mask_map[index] == 0) 00501 distance_map[index] = 0.0f; 00502 else 00503 distance_map[index] = static_cast<float> (width + height); 00504 } 00505 00506 // first pass 00507 float * previous_row = distance_map; 00508 float * current_row = previous_row + width; 00509 for (size_t ri = 1; ri < height; ++ri) 00510 { 00511 for (size_t ci = 1; ci < width; ++ci) 00512 { 00513 const float up_left = previous_row [ci - 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci-1] + 1.4f; 00514 const float up = previous_row [ci] + 1.0f; //distance_map[(ri-1)*input_->width + ci] + 1.0f; 00515 const float up_right = previous_row [ci + 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci+1] + 1.4f; 00516 const float left = current_row [ci - 1] + 1.0f; //distance_map[ri*input_->width + ci-1] + 1.0f; 00517 const float center = current_row [ci]; //distance_map[ri*input_->width + ci]; 00518 00519 const float min_value = std::min (std::min (up_left, up), std::min (left, up_right)); 00520 00521 if (min_value < center) 00522 current_row[ci] = min_value; //distance_map[ri * input_->width + ci] = min_value; 00523 } 00524 previous_row = current_row; 00525 current_row += width; 00526 } 00527 00528 // second pass 00529 float * next_row = distance_map + width * (height - 1); 00530 current_row = next_row - width; 00531 for (int ri = static_cast<int> (height)-2; ri >= 0; --ri) 00532 { 00533 for (int ci = static_cast<int> (width)-2; ci >= 0; --ci) 00534 { 00535 const float lower_left = next_row [ci - 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci-1] + 1.4f; 00536 const float lower = next_row [ci] + 1.0f; //distance_map[(ri+1)*input_->width + ci] + 1.0f; 00537 const float lower_right = next_row [ci + 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci+1] + 1.4f; 00538 const float right = current_row [ci + 1] + 1.0f; //distance_map[ri*input_->width + ci+1] + 1.0f; 00539 const float center = current_row [ci]; //distance_map[ri*input_->width + ci]; 00540 00541 const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right)); 00542 00543 if (min_value < center) 00544 current_row[ci] = min_value; //distance_map[ri*input_->width + ci] = min_value; 00545 } 00546 next_row = current_row; 00547 current_row -= width; 00548 } 00549 } 00550 00551 00552 #endif