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_SURFACE_NORMAL_MODALITY 00039 #define PCL_RECOGNITION_SURFACE_NORMAL_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/features/linear_least_squares_normal.h> 00048 00049 namespace pcl 00050 { 00051 00052 /** \brief Map that stores orientations. 00053 * \author Stefan Holzer 00054 */ 00055 struct PCL_EXPORTS LINEMOD_OrientationMap 00056 { 00057 public: 00058 /** \brief Constructor. */ 00059 inline LINEMOD_OrientationMap () : width_ (0), height_ (0), map_ () {} 00060 /** \brief Destructor. */ 00061 inline ~LINEMOD_OrientationMap () {} 00062 00063 /** \brief Returns the width of the modality data map. */ 00064 inline size_t 00065 getWidth () const 00066 { 00067 return width_; 00068 } 00069 00070 /** \brief Returns the height of the modality data map. */ 00071 inline size_t 00072 getHeight () const 00073 { 00074 return height_; 00075 } 00076 00077 /** \brief Resizes the map to the specific width and height and initializes 00078 * all new elements with the specified value. 00079 * \param[in] width the width of the resized map. 00080 * \param[in] height the height of the resized map. 00081 * \param[in] value the value all new elements will be initialized with. 00082 */ 00083 inline void 00084 resize (const size_t width, const size_t height, const float value) 00085 { 00086 width_ = width; 00087 height_ = height; 00088 map_.clear (); 00089 map_.resize (width*height, value); 00090 } 00091 00092 /** \brief Operator to access elements of the map. 00093 * \param[in] col_index the column index of the element to access. 00094 * \param[in] row_index the row index of the element to access. 00095 */ 00096 inline float & 00097 operator() (const size_t col_index, const size_t row_index) 00098 { 00099 return map_[row_index * width_ + col_index]; 00100 } 00101 00102 /** \brief Operator to access elements of the map. 00103 * \param[in] col_index the column index of the element to access. 00104 * \param[in] row_index the row index of the element to access. 00105 */ 00106 inline const float & 00107 operator() (const size_t col_index, const size_t row_index) const 00108 { 00109 return map_[row_index * width_ + col_index]; 00110 } 00111 00112 private: 00113 /** \brief The width of the map. */ 00114 size_t width_; 00115 /** \brief The height of the map. */ 00116 size_t height_; 00117 /** \brief Storage for the data of the map. */ 00118 std::vector<float> map_; 00119 00120 }; 00121 00122 /** \brief Look-up-table for fast surface normal quantization. 00123 * \author Stefan Holzer 00124 */ 00125 struct QuantizedNormalLookUpTable 00126 { 00127 /** \brief The range of the LUT in x-direction. */ 00128 int range_x; 00129 /** \brief The range of the LUT in y-direction. */ 00130 int range_y; 00131 /** \brief The range of the LUT in z-direction. */ 00132 int range_z; 00133 00134 /** \brief The offset in x-direction. */ 00135 int offset_x; 00136 /** \brief The offset in y-direction. */ 00137 int offset_y; 00138 /** \brief The offset in z-direction. */ 00139 int offset_z; 00140 00141 /** \brief The size of the LUT in x-direction. */ 00142 int size_x; 00143 /** \brief The size of the LUT in y-direction. */ 00144 int size_y; 00145 /** \brief The size of the LUT in z-direction. */ 00146 int size_z; 00147 00148 /** \brief The LUT data. */ 00149 unsigned char * lut; 00150 00151 /** \brief Constructor. */ 00152 QuantizedNormalLookUpTable () : 00153 range_x (-1), range_y (-1), range_z (-1), 00154 offset_x (-1), offset_y (-1), offset_z (-1), 00155 size_x (-1), size_y (-1), size_z (-1), lut (NULL) 00156 {} 00157 00158 /** \brief Destructor. */ 00159 ~QuantizedNormalLookUpTable () 00160 { 00161 if (lut != NULL) 00162 delete[] lut; 00163 } 00164 00165 /** \brief Initializes the LUT. 00166 * \param[in] range_x_arg the range of the LUT in x-direction. 00167 * \param[in] range_y_arg the range of the LUT in y-direction. 00168 * \parma[in] range_z_arg the range of the LUT in z-direction. 00169 */ 00170 void 00171 initializeLUT (const int range_x_arg, const int range_y_arg, const int range_z_arg) 00172 { 00173 range_x = range_x_arg; 00174 range_y = range_y_arg; 00175 range_z = range_z_arg; 00176 size_x = range_x_arg+1; 00177 size_y = range_y_arg+1; 00178 size_z = range_z_arg+1; 00179 offset_x = range_x_arg/2; 00180 offset_y = range_y_arg/2; 00181 offset_z = range_z_arg; 00182 00183 //if (lut != NULL) free16(lut); 00184 //lut = malloc16(size_x*size_y*size_z); 00185 00186 if (lut != NULL) 00187 delete[] lut; 00188 lut = new unsigned char[size_x*size_y*size_z]; 00189 00190 const int nr_normals = 8; 00191 pcl::PointCloud<PointXYZ>::VectorType ref_normals (nr_normals); 00192 00193 const float normal0_angle = 40.0f * 3.14f / 180.0f; 00194 ref_normals[0].x = cosf (normal0_angle); 00195 ref_normals[0].y = 0.0f; 00196 ref_normals[0].z = -sinf (normal0_angle); 00197 00198 const float inv_nr_normals = 1.0f / static_cast<float> (nr_normals); 00199 for (int normal_index = 1; normal_index < nr_normals; ++normal_index) 00200 { 00201 const float angle = 2.0f * static_cast<float> (M_PI * normal_index * inv_nr_normals); 00202 00203 ref_normals[normal_index].x = cosf (angle) * ref_normals[0].x - sinf (angle) * ref_normals[0].y; 00204 ref_normals[normal_index].y = sinf (angle) * ref_normals[0].x + cosf (angle) * ref_normals[0].y; 00205 ref_normals[normal_index].z = ref_normals[0].z; 00206 } 00207 00208 // normalize normals 00209 for (int normal_index = 0; normal_index < nr_normals; ++normal_index) 00210 { 00211 const float length = sqrtf (ref_normals[normal_index].x * ref_normals[normal_index].x + 00212 ref_normals[normal_index].y * ref_normals[normal_index].y + 00213 ref_normals[normal_index].z * ref_normals[normal_index].z); 00214 00215 const float inv_length = 1.0f / length; 00216 00217 ref_normals[normal_index].x *= inv_length; 00218 ref_normals[normal_index].y *= inv_length; 00219 ref_normals[normal_index].z *= inv_length; 00220 } 00221 00222 // set LUT 00223 for (int z_index = 0; z_index < size_z; ++z_index) 00224 { 00225 for (int y_index = 0; y_index < size_y; ++y_index) 00226 { 00227 for (int x_index = 0; x_index < size_x; ++x_index) 00228 { 00229 PointXYZ normal (static_cast<float> (x_index - range_x/2), 00230 static_cast<float> (y_index - range_y/2), 00231 static_cast<float> (z_index - range_z)); 00232 const float length = sqrtf (normal.x*normal.x + normal.y*normal.y + normal.z*normal.z); 00233 const float inv_length = 1.0f / (length + 0.00001f); 00234 00235 normal.x *= inv_length; 00236 normal.y *= inv_length; 00237 normal.z *= inv_length; 00238 00239 float max_response = -1.0f; 00240 int max_index = -1; 00241 00242 for (int normal_index = 0; normal_index < nr_normals; ++normal_index) 00243 { 00244 const float response = normal.x * ref_normals[normal_index].x + 00245 normal.y * ref_normals[normal_index].y + 00246 normal.z * ref_normals[normal_index].z; 00247 00248 const float abs_response = fabsf (response); 00249 if (max_response < abs_response) 00250 { 00251 max_response = abs_response; 00252 max_index = normal_index; 00253 } 00254 00255 lut[z_index*size_y*size_x + y_index*size_x + x_index] = static_cast<unsigned char> (0x1 << max_index); 00256 } 00257 } 00258 } 00259 } 00260 } 00261 00262 /** \brief Operator to access an element in the LUT. 00263 * \param[in] x the x-component of the normal. 00264 * \param[in] y the y-component of the normal. 00265 * \param[in] z the z-component of the normal. 00266 */ 00267 inline unsigned char 00268 operator() (const float x, const float y, const float z) const 00269 { 00270 const size_t x_index = static_cast<size_t> (x * static_cast<float> (offset_x) + static_cast<float> (offset_x)); 00271 const size_t y_index = static_cast<size_t> (y * static_cast<float> (offset_y) + static_cast<float> (offset_y)); 00272 const size_t z_index = static_cast<size_t> (z * static_cast<float> (range_z) + static_cast<float> (range_z)); 00273 00274 const size_t index = z_index*size_y*size_x + y_index*size_x + x_index; 00275 00276 return (lut[index]); 00277 } 00278 00279 /** \brief Operator to access an element in the LUT. 00280 * \param[in] index the index of the element. 00281 */ 00282 inline unsigned char 00283 operator() (const int index) const 00284 { 00285 return (lut[index]); 00286 } 00287 }; 00288 00289 00290 /** \brief Modality based on surface normals. 00291 * \author Stefan Holzer 00292 */ 00293 template <typename PointInT> 00294 class SurfaceNormalModality : public QuantizableModality, public PCLBase<PointInT> 00295 { 00296 protected: 00297 using PCLBase<PointInT>::input_; 00298 00299 /** \brief Candidate for a feature (used in feature extraction methods). */ 00300 struct Candidate 00301 { 00302 /** \brief Constructor. */ 00303 Candidate () : normal (), distance (0.0f), bin_index (0), x (0), y (0) {} 00304 00305 /** \brief Normal. */ 00306 Normal normal; 00307 /** \brief Distance to the next different quantized value. */ 00308 float distance; 00309 00310 /** \brief Quantized value. */ 00311 unsigned char bin_index; 00312 00313 /** \brief x-position of the feature. */ 00314 size_t x; 00315 /** \brief y-position of the feature. */ 00316 size_t y; 00317 00318 /** \brief Compares two candidates based on their distance to the next different quantized value. 00319 * \param[in] rhs the candidate to compare with. 00320 */ 00321 bool 00322 operator< (const Candidate & rhs) 00323 { 00324 return (distance > rhs.distance); 00325 } 00326 }; 00327 00328 public: 00329 typedef typename pcl::PointCloud<PointInT> PointCloudIn; 00330 00331 /** \brief Constructor. */ 00332 SurfaceNormalModality (); 00333 /** \brief Destructor. */ 00334 virtual ~SurfaceNormalModality (); 00335 00336 /** \brief Sets the spreading size. 00337 * \param[in] spreading_size the spreading size. 00338 */ 00339 inline void 00340 setSpreadingSize (const size_t spreading_size) 00341 { 00342 spreading_size_ = spreading_size; 00343 } 00344 00345 /** \brief Enables/disables the use of extracting a variable number of features. 00346 * \param[in] enabled specifies whether extraction of a variable number of features will be enabled/disabled. 00347 */ 00348 inline void 00349 setVariableFeatureNr (const bool enabled) 00350 { 00351 variable_feature_nr_ = enabled; 00352 } 00353 00354 /** \brief Returns the surface normals. */ 00355 inline pcl::PointCloud<pcl::Normal> & 00356 getSurfaceNormals () 00357 { 00358 return surface_normals_; 00359 } 00360 00361 /** \brief Returns the surface normals. */ 00362 inline const pcl::PointCloud<pcl::Normal> & 00363 getSurfaceNormals () const 00364 { 00365 return surface_normals_; 00366 } 00367 00368 /** \brief Returns a reference to the internal quantized map. */ 00369 inline QuantizedMap & 00370 getQuantizedMap () 00371 { 00372 return (filtered_quantized_surface_normals_); 00373 } 00374 00375 /** \brief Returns a reference to the internal spreaded quantized map. */ 00376 inline QuantizedMap & 00377 getSpreadedQuantizedMap () 00378 { 00379 return (spreaded_quantized_surface_normals_); 00380 } 00381 00382 /** \brief Returns a reference to the orientation map. */ 00383 inline LINEMOD_OrientationMap & 00384 getOrientationMap () 00385 { 00386 return (surface_normal_orientations_); 00387 } 00388 00389 /** \brief Extracts features from this modality within the specified mask. 00390 * \param[in] mask defines the areas where features are searched in. 00391 * \param[in] nr_features defines the number of features to be extracted 00392 * (might be less if not sufficient information is present in the modality). 00393 * \param[in] modality_index the index which is stored in the extracted features. 00394 * \param[out] features the destination for the extracted features. 00395 */ 00396 void 00397 extractFeatures (const MaskMap & mask, size_t nr_features, size_t modality_index, 00398 std::vector<QuantizedMultiModFeature> & features) const; 00399 00400 /** \brief Extracts all possible features from the modality within the specified mask. 00401 * \param[in] mask defines the areas where features are searched in. 00402 * \param[in] nr_features IGNORED (TODO: remove this parameter). 00403 * \param[in] modality_index the index which is stored in the extracted features. 00404 * \param[out] features the destination for the extracted features. 00405 */ 00406 void 00407 extractAllFeatures (const MaskMap & mask, size_t nr_features, size_t modality_index, 00408 std::vector<QuantizedMultiModFeature> & features) const; 00409 00410 /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) 00411 * \param[in] cloud the const boost shared pointer to a PointCloud message 00412 */ 00413 virtual void 00414 setInputCloud (const typename PointCloudIn::ConstPtr & cloud) 00415 { 00416 input_ = cloud; 00417 } 00418 00419 /** \brief Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading). */ 00420 virtual void 00421 processInputData (); 00422 00423 /** \brief Processes the input data assuming that everything up to filtering is already done/available 00424 * (so only spreading is performed). */ 00425 virtual void 00426 processInputDataFromFiltered (); 00427 00428 protected: 00429 00430 /** \brief Computes the surface normals from the input cloud. */ 00431 void 00432 computeSurfaceNormals (); 00433 00434 /** \brief Computes and quantizes the surface normals. */ 00435 void 00436 computeAndQuantizeSurfaceNormals (); 00437 00438 /** \brief Computes and quantizes the surface normals. */ 00439 void 00440 computeAndQuantizeSurfaceNormals2 (); 00441 00442 /** \brief Quantizes the surface normals. */ 00443 void 00444 quantizeSurfaceNormals (); 00445 00446 /** \brief Filters the quantized surface normals. */ 00447 void 00448 filterQuantizedSurfaceNormals (); 00449 00450 /** \brief Computes a distance map from the supplied input mask. 00451 * \param[in] input the mask for which a distance map will be computed. 00452 * \param[out] output the destination for the distance map. 00453 */ 00454 void 00455 computeDistanceMap (const MaskMap & input, DistanceMap & output) const; 00456 00457 private: 00458 00459 /** \brief Determines whether variable numbers of features are extracted or not. */ 00460 bool variable_feature_nr_; 00461 00462 /** \brief The feature distance threshold. */ 00463 float feature_distance_threshold_; 00464 /** \brief Minimum distance of a feature to a border. */ 00465 float min_distance_to_border_; 00466 00467 /** \brief Look-up-table for quantizing surface normals. */ 00468 QuantizedNormalLookUpTable normal_lookup_; 00469 00470 /** \brief The spreading size. */ 00471 size_t spreading_size_; 00472 00473 /** \brief Point cloud holding the computed surface normals. */ 00474 pcl::PointCloud<pcl::Normal> surface_normals_; 00475 /** \brief Quantized surface normals. */ 00476 pcl::QuantizedMap quantized_surface_normals_; 00477 /** \brief Filtered quantized surface normals. */ 00478 pcl::QuantizedMap filtered_quantized_surface_normals_; 00479 /** \brief Spreaded quantized surface normals. */ 00480 pcl::QuantizedMap spreaded_quantized_surface_normals_; 00481 00482 /** \brief Map containing surface normal orientations. */ 00483 pcl::LINEMOD_OrientationMap surface_normal_orientations_; 00484 00485 }; 00486 00487 } 00488 00489 ////////////////////////////////////////////////////////////////////////////////////////////// 00490 template <typename PointInT> 00491 pcl::SurfaceNormalModality<PointInT>:: 00492 SurfaceNormalModality () 00493 : variable_feature_nr_ (false) 00494 , feature_distance_threshold_ (2.0f) 00495 , min_distance_to_border_ (2.0f) 00496 , normal_lookup_ () 00497 , spreading_size_ (8) 00498 , surface_normals_ () 00499 , quantized_surface_normals_ () 00500 , filtered_quantized_surface_normals_ () 00501 , spreaded_quantized_surface_normals_ () 00502 , surface_normal_orientations_ () 00503 { 00504 } 00505 00506 ////////////////////////////////////////////////////////////////////////////////////////////// 00507 template <typename PointInT> 00508 pcl::SurfaceNormalModality<PointInT>::~SurfaceNormalModality () 00509 { 00510 } 00511 00512 ////////////////////////////////////////////////////////////////////////////////////////////// 00513 template <typename PointInT> void 00514 pcl::SurfaceNormalModality<PointInT>::processInputData () 00515 { 00516 // compute surface normals 00517 //computeSurfaceNormals (); 00518 00519 // quantize surface normals 00520 //quantizeSurfaceNormals (); 00521 00522 computeAndQuantizeSurfaceNormals2 (); 00523 00524 // filter quantized surface normals 00525 filterQuantizedSurfaceNormals (); 00526 00527 // spread quantized surface normals 00528 pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_surface_normals_, 00529 spreaded_quantized_surface_normals_, 00530 spreading_size_); 00531 } 00532 00533 ////////////////////////////////////////////////////////////////////////////////////////////// 00534 template <typename PointInT> void 00535 pcl::SurfaceNormalModality<PointInT>::processInputDataFromFiltered () 00536 { 00537 // spread quantized surface normals 00538 pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_surface_normals_, 00539 spreaded_quantized_surface_normals_, 00540 spreading_size_); 00541 } 00542 00543 ////////////////////////////////////////////////////////////////////////////////////////////// 00544 template <typename PointInT> void 00545 pcl::SurfaceNormalModality<PointInT>::computeSurfaceNormals () 00546 { 00547 // compute surface normals 00548 pcl::LinearLeastSquaresNormalEstimation<PointInT, pcl::Normal> ne; 00549 ne.setMaxDepthChangeFactor(0.05f); 00550 ne.setNormalSmoothingSize(5.0f); 00551 ne.setDepthDependentSmoothing(false); 00552 ne.setInputCloud (input_); 00553 ne.compute (surface_normals_); 00554 } 00555 00556 ////////////////////////////////////////////////////////////////////////////////////////////// 00557 template <typename PointInT> void 00558 pcl::SurfaceNormalModality<PointInT>::computeAndQuantizeSurfaceNormals () 00559 { 00560 // compute surface normals 00561 //pcl::LinearLeastSquaresNormalEstimation<PointInT, pcl::Normal> ne; 00562 //ne.setMaxDepthChangeFactor(0.05f); 00563 //ne.setNormalSmoothingSize(5.0f); 00564 //ne.setDepthDependentSmoothing(false); 00565 //ne.setInputCloud (input_); 00566 //ne.compute (surface_normals_); 00567 00568 00569 const float bad_point = std::numeric_limits<float>::quiet_NaN (); 00570 00571 const int width = input_->width; 00572 const int height = input_->height; 00573 00574 surface_normals_.resize (width*height); 00575 surface_normals_.width = width; 00576 surface_normals_.height = height; 00577 surface_normals_.is_dense = false; 00578 00579 quantized_surface_normals_.resize (width, height); 00580 00581 // we compute the normals as follows: 00582 // ---------------------------------- 00583 // 00584 // for the depth-gradient you can make the following first-order Taylor approximation: 00585 // D(x + dx) - D(x) = dx^T \Delta D + h.o.t. 00586 // 00587 // build linear system by stacking up equation for 8 neighbor points: 00588 // Y = X \Delta D 00589 // 00590 // => \Delta D = (X^T X)^{-1} X^T Y 00591 // => \Delta D = (A)^{-1} b 00592 00593 for (int y = 0; y < height; ++y) 00594 { 00595 for (int x = 0; x < width; ++x) 00596 { 00597 const int index = y * width + x; 00598 00599 const float px = input_->points[index].x; 00600 const float py = input_->points[index].y; 00601 const float pz = input_->points[index].z; 00602 00603 if (pcl_isnan(px) || pz > 2.0f) 00604 { 00605 surface_normals_.points[index].normal_x = bad_point; 00606 surface_normals_.points[index].normal_y = bad_point; 00607 surface_normals_.points[index].normal_z = bad_point; 00608 surface_normals_.points[index].curvature = bad_point; 00609 00610 quantized_surface_normals_ (x, y) = 0; 00611 00612 continue; 00613 } 00614 00615 const int smoothingSizeInt = 5; 00616 00617 float matA0 = 0.0f; 00618 float matA1 = 0.0f; 00619 float matA3 = 0.0f; 00620 00621 float vecb0 = 0.0f; 00622 float vecb1 = 0.0f; 00623 00624 for (int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt) 00625 { 00626 for (int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt) 00627 { 00628 if (u < 0 || u >= width || v < 0 || v >= height) continue; 00629 00630 const size_t index2 = v * width + u; 00631 00632 const float qx = input_->points[index2].x; 00633 const float qy = input_->points[index2].y; 00634 const float qz = input_->points[index2].z; 00635 00636 if (pcl_isnan(qx)) continue; 00637 00638 const float delta = qz - pz; 00639 const float i = qx - px; 00640 const float j = qy - py; 00641 00642 const float f = fabs(delta) < 0.05f ? 1.0f : 0.0f; 00643 00644 matA0 += f * i * i; 00645 matA1 += f * i * j; 00646 matA3 += f * j * j; 00647 vecb0 += f * i * delta; 00648 vecb1 += f * j * delta; 00649 } 00650 } 00651 00652 const float det = matA0 * matA3 - matA1 * matA1; 00653 const float ddx = matA3 * vecb0 - matA1 * vecb1; 00654 const float ddy = -matA1 * vecb0 + matA0 * vecb1; 00655 00656 const float nx = ddx; 00657 const float ny = ddy; 00658 const float nz = -det * pz; 00659 00660 const float length = nx * nx + ny * ny + nz * nz; 00661 00662 if (length <= 0.0f) 00663 { 00664 surface_normals_.points[index].normal_x = bad_point; 00665 surface_normals_.points[index].normal_y = bad_point; 00666 surface_normals_.points[index].normal_z = bad_point; 00667 surface_normals_.points[index].curvature = bad_point; 00668 00669 quantized_surface_normals_ (x, y) = 0; 00670 } 00671 else 00672 { 00673 const float normInv = 1.0f / sqrtf (length); 00674 00675 const float normal_x = nx * normInv; 00676 const float normal_y = ny * normInv; 00677 const float normal_z = nz * normInv; 00678 00679 surface_normals_.points[index].normal_x = normal_x; 00680 surface_normals_.points[index].normal_y = normal_y; 00681 surface_normals_.points[index].normal_z = normal_z; 00682 surface_normals_.points[index].curvature = bad_point; 00683 00684 float angle = 11.25f + atan2 (normal_y, normal_x)*180.0f/3.14f; 00685 00686 if (angle < 0.0f) angle += 360.0f; 00687 if (angle >= 360.0f) angle -= 360.0f; 00688 00689 int bin_index = static_cast<int> (angle*8.0f/360.0f) & 7; 00690 00691 quantized_surface_normals_ (x, y) = static_cast<unsigned char> (bin_index); 00692 } 00693 } 00694 } 00695 } 00696 00697 00698 ////////////////////////////////////////////////////////////////////////////////////////////// 00699 // Contains GRANULARITY and NORMAL_LUT 00700 //#include "normal_lut.i" 00701 00702 static void accumBilateral(long delta, long i, long j, long * A, long * b, int threshold) 00703 { 00704 long f = std::abs(delta) < threshold ? 1 : 0; 00705 00706 const long fi = f * i; 00707 const long fj = f * j; 00708 00709 A[0] += fi * i; 00710 A[1] += fi * j; 00711 A[3] += fj * j; 00712 b[0] += fi * delta; 00713 b[1] += fj * delta; 00714 } 00715 00716 /** 00717 * \brief Compute quantized normal image from depth image. 00718 * 00719 * Implements section 2.6 "Extension to Dense Depth Sensors." 00720 * 00721 * \param[in] src The source 16-bit depth image (in mm). 00722 * \param[out] dst The destination 8-bit image. Each bit represents one bin of 00723 * the view cone. 00724 * \param distance_threshold Ignore pixels beyond this distance. 00725 * \param difference_threshold When computing normals, ignore contributions of pixels whose 00726 * depth difference with the central pixel is above this threshold. 00727 * 00728 * \todo Should also need camera model, or at least focal lengths? Replace distance_threshold with mask? 00729 */ 00730 template <typename PointInT> void 00731 pcl::SurfaceNormalModality<PointInT>::computeAndQuantizeSurfaceNormals2 () 00732 { 00733 const int width = input_->width; 00734 const int height = input_->height; 00735 00736 unsigned short * lp_depth = new unsigned short[width*height]; 00737 unsigned char * lp_normals = new unsigned char[width*height]; 00738 memset (lp_normals, 0, width*height); 00739 00740 surface_normal_orientations_.resize (width, height, 0.0f); 00741 00742 for (size_t row_index = 0; row_index < height; ++row_index) 00743 { 00744 for (size_t col_index = 0; col_index < width; ++col_index) 00745 { 00746 const float value = input_->points[row_index*width + col_index].z; 00747 if (pcl_isfinite (value)) 00748 { 00749 lp_depth[row_index*width + col_index] = static_cast<unsigned short> (value * 1000.0f); 00750 } 00751 else 00752 { 00753 lp_depth[row_index*width + col_index] = 0; 00754 } 00755 } 00756 } 00757 00758 const int l_W = width; 00759 const int l_H = height; 00760 00761 const int l_r = 5; // used to be 7 00762 //const int l_offset0 = -l_r - l_r * l_W; 00763 //const int l_offset1 = 0 - l_r * l_W; 00764 //const int l_offset2 = +l_r - l_r * l_W; 00765 //const int l_offset3 = -l_r; 00766 //const int l_offset4 = +l_r; 00767 //const int l_offset5 = -l_r + l_r * l_W; 00768 //const int l_offset6 = 0 + l_r * l_W; 00769 //const int l_offset7 = +l_r + l_r * l_W; 00770 00771 const int offsets_i[] = {-l_r, 0, l_r, -l_r, l_r, -l_r, 0, l_r}; 00772 const int offsets_j[] = {-l_r, -l_r, -l_r, 0, 0, l_r, l_r, l_r}; 00773 const int offsets[] = { offsets_i[0] + offsets_j[0] * l_W 00774 , offsets_i[1] + offsets_j[1] * l_W 00775 , offsets_i[2] + offsets_j[2] * l_W 00776 , offsets_i[3] + offsets_j[3] * l_W 00777 , offsets_i[4] + offsets_j[4] * l_W 00778 , offsets_i[5] + offsets_j[5] * l_W 00779 , offsets_i[6] + offsets_j[6] * l_W 00780 , offsets_i[7] + offsets_j[7] * l_W }; 00781 00782 00783 //const int l_offsetx = GRANULARITY / 2; 00784 //const int l_offsety = GRANULARITY / 2; 00785 00786 const int difference_threshold = 50; 00787 const int distance_threshold = 2000; 00788 00789 //const double scale = 1000.0; 00790 //const double difference_threshold = 0.05 * scale; 00791 //const double distance_threshold = 2.0 * scale; 00792 00793 for (int l_y = l_r; l_y < l_H - l_r - 1; ++l_y) 00794 { 00795 unsigned short * lp_line = lp_depth + (l_y * l_W + l_r); 00796 unsigned char * lp_norm = lp_normals + (l_y * l_W + l_r); 00797 00798 for (int l_x = l_r; l_x < l_W - l_r - 1; ++l_x) 00799 { 00800 long l_d = lp_line[0]; 00801 //float l_d = input_->points[(l_y * l_W + l_r) + l_x].z; 00802 //float px = input_->points[(l_y * l_W + l_r) + l_x].x; 00803 //float py = input_->points[(l_y * l_W + l_r) + l_x].y; 00804 00805 if (l_d < distance_threshold) 00806 { 00807 // accum 00808 long l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0; 00809 long l_b[2]; l_b[0] = l_b[1] = 0; 00810 //double l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0; 00811 //double l_b[2]; l_b[0] = l_b[1] = 0; 00812 00813 accumBilateral(lp_line[offsets[0]] - l_d, offsets_i[0], offsets_j[0], l_A, l_b, difference_threshold); 00814 accumBilateral(lp_line[offsets[1]] - l_d, offsets_i[1], offsets_j[1], l_A, l_b, difference_threshold); 00815 accumBilateral(lp_line[offsets[2]] - l_d, offsets_i[2], offsets_j[2], l_A, l_b, difference_threshold); 00816 accumBilateral(lp_line[offsets[3]] - l_d, offsets_i[3], offsets_j[3], l_A, l_b, difference_threshold); 00817 accumBilateral(lp_line[offsets[4]] - l_d, offsets_i[4], offsets_j[4], l_A, l_b, difference_threshold); 00818 accumBilateral(lp_line[offsets[5]] - l_d, offsets_i[5], offsets_j[5], l_A, l_b, difference_threshold); 00819 accumBilateral(lp_line[offsets[6]] - l_d, offsets_i[6], offsets_j[6], l_A, l_b, difference_threshold); 00820 accumBilateral(lp_line[offsets[7]] - l_d, offsets_i[7], offsets_j[7], l_A, l_b, difference_threshold); 00821 00822 //for (size_t index = 0; index < 8; ++index) 00823 //{ 00824 // //accumBilateral(lp_line[offsets[index]] - l_d, offsets_i[index], offsets_j[index], l_A, l_b, difference_threshold); 00825 00826 // //const long delta = lp_line[offsets[index]] - l_d; 00827 // //const long i = offsets_i[index]; 00828 // //const long j = offsets_j[index]; 00829 // //long * A = l_A; 00830 // //long * b = l_b; 00831 // //const int threshold = difference_threshold; 00832 00833 // //const long f = std::abs(delta) < threshold ? 1 : 0; 00834 00835 // //const long fi = f * i; 00836 // //const long fj = f * j; 00837 00838 // //A[0] += fi * i; 00839 // //A[1] += fi * j; 00840 // //A[3] += fj * j; 00841 // //b[0] += fi * delta; 00842 // //b[1] += fj * delta; 00843 00844 00845 // const double delta = 1000.0f * (input_->points[(l_y * l_W + l_r) + l_x + offsets[index]].z - l_d); 00846 // const double i = offsets_i[index]; 00847 // const double j = offsets_j[index]; 00848 // //const float i = input_->points[(l_y * l_W + l_r) + l_x + offsets[index]].x - px;//offsets_i[index]; 00849 // //const float j = input_->points[(l_y * l_W + l_r) + l_x + offsets[index]].y - py;//offsets_j[index]; 00850 // double * A = l_A; 00851 // double * b = l_b; 00852 // const double threshold = difference_threshold; 00853 00854 // const double f = std::fabs(delta) < threshold ? 1.0f : 0.0f; 00855 00856 // const double fi = f * i; 00857 // const double fj = f * j; 00858 00859 // A[0] += fi * i; 00860 // A[1] += fi * j; 00861 // A[3] += fj * j; 00862 // b[0] += fi * delta; 00863 // b[1] += fj * delta; 00864 //} 00865 00866 //long f = std::abs(delta) < threshold ? 1 : 0; 00867 00868 //const long fi = f * i; 00869 //const long fj = f * j; 00870 00871 //A[0] += fi * i; 00872 //A[1] += fi * j; 00873 //A[3] += fj * j; 00874 //b[0] += fi * delta; 00875 //b[1] += fj * delta; 00876 00877 00878 // solve 00879 long l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1]; 00880 long l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1]; 00881 long l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1]; 00882 00883 /// @todo Magic number 1150 is focal length? This is something like 00884 /// f in SXGA mode, but in VGA is more like 530. 00885 float l_nx = static_cast<float>(1150 * l_ddx); 00886 float l_ny = static_cast<float>(1150 * l_ddy); 00887 float l_nz = static_cast<float>(-l_det * l_d); 00888 00889 //// solve 00890 //double l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1]; 00891 //double l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1]; 00892 //double l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1]; 00893 00894 ///// @todo Magic number 1150 is focal length? This is something like 00895 ///// f in SXGA mode, but in VGA is more like 530. 00896 //const double dummy_focal_length = 1150.0f; 00897 //double l_nx = l_ddx * dummy_focal_length; 00898 //double l_ny = l_ddy * dummy_focal_length; 00899 //double l_nz = -l_det * l_d; 00900 00901 float l_sqrt = sqrtf (l_nx * l_nx + l_ny * l_ny + l_nz * l_nz); 00902 00903 if (l_sqrt > 0) 00904 { 00905 float l_norminv = 1.0f / (l_sqrt); 00906 00907 l_nx *= l_norminv; 00908 l_ny *= l_norminv; 00909 l_nz *= l_norminv; 00910 00911 float angle = 22.5f + atan2f (l_ny, l_nx) * 180.0f / 3.14f; 00912 00913 if (angle < 0.0f) angle += 360.0f; 00914 if (angle >= 360.0f) angle -= 360.0f; 00915 00916 int bin_index = static_cast<int> (angle*8.0f/360.0f) & 7; 00917 00918 surface_normal_orientations_ (l_x, l_y) = angle; 00919 00920 //*lp_norm = fabs(l_nz)*255; 00921 00922 //int l_val1 = static_cast<int>(l_nx * l_offsetx + l_offsetx); 00923 //int l_val2 = static_cast<int>(l_ny * l_offsety + l_offsety); 00924 //int l_val3 = static_cast<int>(l_nz * GRANULARITY + GRANULARITY); 00925 00926 //*lp_norm = NORMAL_LUT[l_val3][l_val2][l_val1]; 00927 *lp_norm = static_cast<unsigned char> (0x1 << bin_index); 00928 } 00929 else 00930 { 00931 *lp_norm = 0; // Discard shadows from depth sensor 00932 } 00933 } 00934 else 00935 { 00936 *lp_norm = 0; //out of depth 00937 } 00938 ++lp_line; 00939 ++lp_norm; 00940 } 00941 } 00942 /*cvSmooth(m_dep[0], m_dep[0], CV_MEDIAN, 5, 5);*/ 00943 00944 unsigned char map[255]; 00945 memset(map, 0, 255); 00946 00947 map[0x1<<0] = 0; 00948 map[0x1<<1] = 1; 00949 map[0x1<<2] = 2; 00950 map[0x1<<3] = 3; 00951 map[0x1<<4] = 4; 00952 map[0x1<<5] = 5; 00953 map[0x1<<6] = 6; 00954 map[0x1<<7] = 7; 00955 00956 quantized_surface_normals_.resize (width, height); 00957 for (size_t row_index = 0; row_index < height; ++row_index) 00958 { 00959 for (size_t col_index = 0; col_index < width; ++col_index) 00960 { 00961 quantized_surface_normals_ (col_index, row_index) = map[lp_normals[row_index*width + col_index]]; 00962 } 00963 } 00964 00965 delete[] lp_depth; 00966 delete[] lp_normals; 00967 } 00968 00969 00970 ////////////////////////////////////////////////////////////////////////////////////////////// 00971 template <typename PointInT> void 00972 pcl::SurfaceNormalModality<PointInT>::extractFeatures (const MaskMap & mask, 00973 const size_t nr_features, 00974 const size_t modality_index, 00975 std::vector<QuantizedMultiModFeature> & features) const 00976 { 00977 const size_t width = mask.getWidth (); 00978 const size_t height = mask.getHeight (); 00979 00980 //cv::Mat maskImage(height, width, CV_8U, mask.mask); 00981 //cv::erode(maskImage, maskImage 00982 00983 // create distance maps for every quantization value 00984 //cv::Mat distance_maps[8]; 00985 //for (int map_index = 0; map_index < 8; ++map_index) 00986 //{ 00987 // distance_maps[map_index] = ::cv::Mat::zeros(height, width, CV_8U); 00988 //} 00989 00990 MaskMap mask_maps[8]; 00991 for (size_t map_index = 0; map_index < 8; ++map_index) 00992 mask_maps[map_index].resize (width, height); 00993 00994 unsigned char map[255]; 00995 memset(map, 0, 255); 00996 00997 map[0x1<<0] = 0; 00998 map[0x1<<1] = 1; 00999 map[0x1<<2] = 2; 01000 map[0x1<<3] = 3; 01001 map[0x1<<4] = 4; 01002 map[0x1<<5] = 5; 01003 map[0x1<<6] = 6; 01004 map[0x1<<7] = 7; 01005 01006 QuantizedMap distance_map_indices (width, height); 01007 //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height); 01008 01009 for (size_t row_index = 0; row_index < height; ++row_index) 01010 { 01011 for (size_t col_index = 0; col_index < width; ++col_index) 01012 { 01013 if (mask (col_index, row_index) != 0) 01014 { 01015 //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index); 01016 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index); 01017 01018 if (quantized_value == 0) 01019 continue; 01020 const int dist_map_index = map[quantized_value]; 01021 01022 distance_map_indices (col_index, row_index) = static_cast<unsigned char> (dist_map_index); 01023 //distance_maps[dist_map_index].at<unsigned char>(row_index, col_index) = 255; 01024 mask_maps[dist_map_index] (col_index, row_index) = 255; 01025 } 01026 } 01027 } 01028 01029 DistanceMap distance_maps[8]; 01030 for (int map_index = 0; map_index < 8; ++map_index) 01031 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]); 01032 01033 DistanceMap mask_distance_maps; 01034 computeDistanceMap (mask, mask_distance_maps); 01035 01036 std::list<Candidate> list1; 01037 std::list<Candidate> list2; 01038 01039 float weights[8] = {0,0,0,0,0,0,0,0}; 01040 01041 const size_t off = 4; 01042 for (size_t row_index = off; row_index < height-off; ++row_index) 01043 { 01044 for (size_t col_index = off; col_index < width-off; ++col_index) 01045 { 01046 if (mask (col_index, row_index) != 0) 01047 { 01048 //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index); 01049 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index); 01050 01051 //const float nx = surface_normals_ (col_index, row_index).normal_x; 01052 //const float ny = surface_normals_ (col_index, row_index).normal_y; 01053 //const float nz = surface_normals_ (col_index, row_index).normal_z; 01054 01055 if (quantized_value != 0)// && !(pcl_isnan (nx) || pcl_isnan (ny) || pcl_isnan (nz))) 01056 { 01057 const int distance_map_index = map[quantized_value]; 01058 01059 //const float distance = distance_maps[distance_map_index].at<float> (row_index, col_index); 01060 const float distance = distance_maps[distance_map_index] (col_index, row_index); 01061 const float distance_to_border = mask_distance_maps (col_index, row_index); 01062 01063 if (distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_) 01064 { 01065 Candidate candidate; 01066 01067 candidate.distance = distance; 01068 candidate.x = col_index; 01069 candidate.y = row_index; 01070 candidate.bin_index = static_cast<unsigned char> (distance_map_index); 01071 01072 list1.push_back (candidate); 01073 01074 ++weights[distance_map_index]; 01075 } 01076 } 01077 } 01078 } 01079 } 01080 01081 for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter) 01082 iter->distance *= 1.0f / weights[iter->bin_index]; 01083 01084 list1.sort (); 01085 01086 if (variable_feature_nr_) 01087 { 01088 int distance = static_cast<int> (list1.size ()); 01089 bool feature_selection_finished = false; 01090 while (!feature_selection_finished) 01091 { 01092 const int sqr_distance = distance*distance; 01093 for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1) 01094 { 01095 bool candidate_accepted = true; 01096 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) 01097 { 01098 const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x); 01099 const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y); 01100 const int tmp_distance = dx*dx + dy*dy; 01101 01102 if (tmp_distance < sqr_distance) 01103 { 01104 candidate_accepted = false; 01105 break; 01106 } 01107 } 01108 01109 01110 float min_min_sqr_distance = std::numeric_limits<float>::max (); 01111 float max_min_sqr_distance = 0; 01112 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) 01113 { 01114 float min_sqr_distance = std::numeric_limits<float>::max (); 01115 for (typename std::list<Candidate>::iterator iter3 = list2.begin (); iter3 != list2.end (); ++iter3) 01116 { 01117 if (iter2 == iter3) 01118 continue; 01119 01120 const float dx = static_cast<float> (iter2->x) - static_cast<float> (iter3->x); 01121 const float dy = static_cast<float> (iter2->y) - static_cast<float> (iter3->y); 01122 01123 const float sqr_distance = dx*dx + dy*dy; 01124 01125 if (sqr_distance < min_sqr_distance) 01126 { 01127 min_sqr_distance = sqr_distance; 01128 } 01129 01130 //std::cerr << min_sqr_distance; 01131 } 01132 //std::cerr << std::endl; 01133 01134 // check current feature 01135 { 01136 const float dx = static_cast<float> (iter2->x) - static_cast<float> (iter1->x); 01137 const float dy = static_cast<float> (iter2->y) - static_cast<float> (iter1->y); 01138 01139 const float sqr_distance = dx*dx + dy*dy; 01140 01141 if (sqr_distance < min_sqr_distance) 01142 { 01143 min_sqr_distance = sqr_distance; 01144 } 01145 } 01146 01147 if (min_sqr_distance < min_min_sqr_distance) 01148 min_min_sqr_distance = min_sqr_distance; 01149 if (min_sqr_distance > max_min_sqr_distance) 01150 max_min_sqr_distance = min_sqr_distance; 01151 01152 //std::cerr << min_sqr_distance << ", " << min_min_sqr_distance << ", " << max_min_sqr_distance << std::endl; 01153 } 01154 01155 if (candidate_accepted) 01156 { 01157 //std::cerr << "feature_index: " << list2.size () << std::endl; 01158 //std::cerr << "min_min_sqr_distance: " << min_min_sqr_distance << std::endl; 01159 //std::cerr << "max_min_sqr_distance: " << max_min_sqr_distance << std::endl; 01160 01161 if (min_min_sqr_distance < 50) 01162 { 01163 feature_selection_finished = true; 01164 break; 01165 } 01166 01167 list2.push_back (*iter1); 01168 } 01169 01170 //if (list2.size () == nr_features) 01171 //{ 01172 // feature_selection_finished = true; 01173 // break; 01174 //} 01175 } 01176 --distance; 01177 } 01178 } 01179 else 01180 { 01181 if (list1.size () <= nr_features) 01182 { 01183 features.reserve (list1.size ()); 01184 for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter) 01185 { 01186 QuantizedMultiModFeature feature; 01187 01188 feature.x = static_cast<int> (iter->x); 01189 feature.y = static_cast<int> (iter->y); 01190 feature.modality_index = modality_index; 01191 feature.quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y); 01192 01193 features.push_back (feature); 01194 } 01195 01196 return; 01197 } 01198 01199 int distance = static_cast<int> (list1.size () / nr_features + 1); // ??? @todo:!:!:!:!:!:! 01200 while (list2.size () != nr_features) 01201 { 01202 const int sqr_distance = distance*distance; 01203 for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1) 01204 { 01205 bool candidate_accepted = true; 01206 01207 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) 01208 { 01209 const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x); 01210 const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y); 01211 const int tmp_distance = dx*dx + dy*dy; 01212 01213 if (tmp_distance < sqr_distance) 01214 { 01215 candidate_accepted = false; 01216 break; 01217 } 01218 } 01219 01220 if (candidate_accepted) 01221 list2.push_back (*iter1); 01222 01223 if (list2.size () == nr_features) break; 01224 } 01225 --distance; 01226 } 01227 } 01228 01229 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) 01230 { 01231 QuantizedMultiModFeature feature; 01232 01233 feature.x = static_cast<int> (iter2->x); 01234 feature.y = static_cast<int> (iter2->y); 01235 feature.modality_index = modality_index; 01236 feature.quantized_value = filtered_quantized_surface_normals_ (iter2->x, iter2->y); 01237 01238 features.push_back (feature); 01239 } 01240 } 01241 01242 ////////////////////////////////////////////////////////////////////////////////////////////// 01243 template <typename PointInT> void 01244 pcl::SurfaceNormalModality<PointInT>::extractAllFeatures ( 01245 const MaskMap & mask, const size_t, const size_t modality_index, 01246 std::vector<QuantizedMultiModFeature> & features) const 01247 { 01248 const size_t width = mask.getWidth (); 01249 const size_t height = mask.getHeight (); 01250 01251 //cv::Mat maskImage(height, width, CV_8U, mask.mask); 01252 //cv::erode(maskImage, maskImage 01253 01254 // create distance maps for every quantization value 01255 //cv::Mat distance_maps[8]; 01256 //for (int map_index = 0; map_index < 8; ++map_index) 01257 //{ 01258 // distance_maps[map_index] = ::cv::Mat::zeros(height, width, CV_8U); 01259 //} 01260 01261 MaskMap mask_maps[8]; 01262 for (size_t map_index = 0; map_index < 8; ++map_index) 01263 mask_maps[map_index].resize (width, height); 01264 01265 unsigned char map[255]; 01266 memset(map, 0, 255); 01267 01268 map[0x1<<0] = 0; 01269 map[0x1<<1] = 1; 01270 map[0x1<<2] = 2; 01271 map[0x1<<3] = 3; 01272 map[0x1<<4] = 4; 01273 map[0x1<<5] = 5; 01274 map[0x1<<6] = 6; 01275 map[0x1<<7] = 7; 01276 01277 QuantizedMap distance_map_indices (width, height); 01278 //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height); 01279 01280 for (size_t row_index = 0; row_index < height; ++row_index) 01281 { 01282 for (size_t col_index = 0; col_index < width; ++col_index) 01283 { 01284 if (mask (col_index, row_index) != 0) 01285 { 01286 //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index); 01287 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index); 01288 01289 if (quantized_value == 0) 01290 continue; 01291 const int dist_map_index = map[quantized_value]; 01292 01293 distance_map_indices (col_index, row_index) = static_cast<unsigned char> (dist_map_index); 01294 //distance_maps[dist_map_index].at<unsigned char>(row_index, col_index) = 255; 01295 mask_maps[dist_map_index] (col_index, row_index) = 255; 01296 } 01297 } 01298 } 01299 01300 DistanceMap distance_maps[8]; 01301 for (int map_index = 0; map_index < 8; ++map_index) 01302 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]); 01303 01304 DistanceMap mask_distance_maps; 01305 computeDistanceMap (mask, mask_distance_maps); 01306 01307 std::list<Candidate> list1; 01308 std::list<Candidate> list2; 01309 01310 float weights[8] = {0,0,0,0,0,0,0,0}; 01311 01312 const size_t off = 4; 01313 for (size_t row_index = off; row_index < height-off; ++row_index) 01314 { 01315 for (size_t col_index = off; col_index < width-off; ++col_index) 01316 { 01317 if (mask (col_index, row_index) != 0) 01318 { 01319 //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index); 01320 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index); 01321 01322 //const float nx = surface_normals_ (col_index, row_index).normal_x; 01323 //const float ny = surface_normals_ (col_index, row_index).normal_y; 01324 //const float nz = surface_normals_ (col_index, row_index).normal_z; 01325 01326 if (quantized_value != 0)// && !(pcl_isnan (nx) || pcl_isnan (ny) || pcl_isnan (nz))) 01327 { 01328 const int distance_map_index = map[quantized_value]; 01329 01330 //const float distance = distance_maps[distance_map_index].at<float> (row_index, col_index); 01331 const float distance = distance_maps[distance_map_index] (col_index, row_index); 01332 const float distance_to_border = mask_distance_maps (col_index, row_index); 01333 01334 if (distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_) 01335 { 01336 Candidate candidate; 01337 01338 candidate.distance = distance; 01339 candidate.x = col_index; 01340 candidate.y = row_index; 01341 candidate.bin_index = static_cast<unsigned char> (distance_map_index); 01342 01343 list1.push_back (candidate); 01344 01345 ++weights[distance_map_index]; 01346 } 01347 } 01348 } 01349 } 01350 } 01351 01352 for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter) 01353 iter->distance *= 1.0f / weights[iter->bin_index]; 01354 01355 list1.sort (); 01356 01357 features.reserve (list1.size ()); 01358 for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter) 01359 { 01360 QuantizedMultiModFeature feature; 01361 01362 feature.x = static_cast<int> (iter->x); 01363 feature.y = static_cast<int> (iter->y); 01364 feature.modality_index = modality_index; 01365 feature.quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y); 01366 01367 features.push_back (feature); 01368 } 01369 } 01370 01371 ////////////////////////////////////////////////////////////////////////////////////////////// 01372 template <typename PointInT> void 01373 pcl::SurfaceNormalModality<PointInT>::quantizeSurfaceNormals () 01374 { 01375 const size_t width = input_->width; 01376 const size_t height = input_->height; 01377 01378 quantized_surface_normals_.resize (width, height); 01379 01380 for (size_t row_index = 0; row_index < height; ++row_index) 01381 { 01382 for (size_t col_index = 0; col_index < width; ++col_index) 01383 { 01384 const float normal_x = surface_normals_ (col_index, row_index).normal_x; 01385 const float normal_y = surface_normals_ (col_index, row_index).normal_y; 01386 const float normal_z = surface_normals_ (col_index, row_index).normal_z; 01387 01388 if (pcl_isnan(normal_x) || pcl_isnan(normal_y) || pcl_isnan(normal_z) || normal_z > 0) 01389 { 01390 quantized_surface_normals_ (col_index, row_index) = 0; 01391 continue; 01392 } 01393 01394 //quantized_surface_normals_.data[row_index*width+col_index] = 01395 // normal_lookup_(normal_x, normal_y, normal_z); 01396 01397 float angle = 11.25f + atan2f (normal_y, normal_x)*180.0f/3.14f; 01398 01399 if (angle < 0.0f) angle += 360.0f; 01400 if (angle >= 360.0f) angle -= 360.0f; 01401 01402 int bin_index = static_cast<int> (angle*8.0f/360.0f); 01403 01404 //quantized_surface_normals_.data[row_index*width+col_index] = 0x1 << bin_index; 01405 quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (bin_index); 01406 } 01407 } 01408 01409 return; 01410 } 01411 01412 ////////////////////////////////////////////////////////////////////////////////////////////// 01413 template <typename PointInT> void 01414 pcl::SurfaceNormalModality<PointInT>::filterQuantizedSurfaceNormals () 01415 { 01416 const int width = input_->width; 01417 const int height = input_->height; 01418 01419 filtered_quantized_surface_normals_.resize (width, height); 01420 01421 //for (int row_index = 2; row_index < height-2; ++row_index) 01422 //{ 01423 // for (int col_index = 2; col_index < width-2; ++col_index) 01424 // { 01425 // std::list<unsigned char> values; 01426 // values.reserve (25); 01427 01428 // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*width+col_index-2; 01429 // values.push_back (dataPtr[0]); 01430 // values.push_back (dataPtr[1]); 01431 // values.push_back (dataPtr[2]); 01432 // values.push_back (dataPtr[3]); 01433 // values.push_back (dataPtr[4]); 01434 // dataPtr += width; 01435 // values.push_back (dataPtr[0]); 01436 // values.push_back (dataPtr[1]); 01437 // values.push_back (dataPtr[2]); 01438 // values.push_back (dataPtr[3]); 01439 // values.push_back (dataPtr[4]); 01440 // dataPtr += width; 01441 // values.push_back (dataPtr[0]); 01442 // values.push_back (dataPtr[1]); 01443 // values.push_back (dataPtr[2]); 01444 // values.push_back (dataPtr[3]); 01445 // values.push_back (dataPtr[4]); 01446 // dataPtr += width; 01447 // values.push_back (dataPtr[0]); 01448 // values.push_back (dataPtr[1]); 01449 // values.push_back (dataPtr[2]); 01450 // values.push_back (dataPtr[3]); 01451 // values.push_back (dataPtr[4]); 01452 // dataPtr += width; 01453 // values.push_back (dataPtr[0]); 01454 // values.push_back (dataPtr[1]); 01455 // values.push_back (dataPtr[2]); 01456 // values.push_back (dataPtr[3]); 01457 // values.push_back (dataPtr[4]); 01458 01459 // values.sort (); 01460 01461 // filtered_quantized_surface_normals_ (col_index, row_index) = values[12]; 01462 // } 01463 //} 01464 01465 01466 //for (int row_index = 2; row_index < height-2; ++row_index) 01467 //{ 01468 // for (int col_index = 2; col_index < width-2; ++col_index) 01469 // { 01470 // filtered_quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (0x1 << (quantized_surface_normals_ (col_index, row_index) - 1)); 01471 // } 01472 //} 01473 01474 01475 // filter data 01476 for (int row_index = 2; row_index < height-2; ++row_index) 01477 { 01478 for (int col_index = 2; col_index < width-2; ++col_index) 01479 { 01480 unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0}; 01481 01482 //{ 01483 // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-1)*width+col_index-1; 01484 // ++histogram[dataPtr[0]]; 01485 // ++histogram[dataPtr[1]]; 01486 // ++histogram[dataPtr[2]]; 01487 //} 01488 //{ 01489 // unsigned char * dataPtr = quantized_surface_normals_.getData () + row_index*width+col_index-1; 01490 // ++histogram[dataPtr[0]]; 01491 // ++histogram[dataPtr[1]]; 01492 // ++histogram[dataPtr[2]]; 01493 //} 01494 //{ 01495 // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+1)*width+col_index-1; 01496 // ++histogram[dataPtr[0]]; 01497 // ++histogram[dataPtr[1]]; 01498 // ++histogram[dataPtr[2]]; 01499 //} 01500 01501 { 01502 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*width+col_index-2; 01503 ++histogram[dataPtr[0]]; 01504 ++histogram[dataPtr[1]]; 01505 ++histogram[dataPtr[2]]; 01506 ++histogram[dataPtr[3]]; 01507 ++histogram[dataPtr[4]]; 01508 } 01509 { 01510 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-1)*width+col_index-2; 01511 ++histogram[dataPtr[0]]; 01512 ++histogram[dataPtr[1]]; 01513 ++histogram[dataPtr[2]]; 01514 ++histogram[dataPtr[3]]; 01515 ++histogram[dataPtr[4]]; 01516 } 01517 { 01518 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index)*width+col_index-2; 01519 ++histogram[dataPtr[0]]; 01520 ++histogram[dataPtr[1]]; 01521 ++histogram[dataPtr[2]]; 01522 ++histogram[dataPtr[3]]; 01523 ++histogram[dataPtr[4]]; 01524 } 01525 { 01526 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+1)*width+col_index-2; 01527 ++histogram[dataPtr[0]]; 01528 ++histogram[dataPtr[1]]; 01529 ++histogram[dataPtr[2]]; 01530 ++histogram[dataPtr[3]]; 01531 ++histogram[dataPtr[4]]; 01532 } 01533 { 01534 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+2)*width+col_index-2; 01535 ++histogram[dataPtr[0]]; 01536 ++histogram[dataPtr[1]]; 01537 ++histogram[dataPtr[2]]; 01538 ++histogram[dataPtr[3]]; 01539 ++histogram[dataPtr[4]]; 01540 } 01541 01542 01543 unsigned char max_hist_value = 0; 01544 int max_hist_index = -1; 01545 01546 if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];} 01547 if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];} 01548 if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];} 01549 if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];} 01550 if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];} 01551 if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];} 01552 if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];} 01553 if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];} 01554 01555 if (max_hist_index != -1 && max_hist_value >= 1) 01556 { 01557 filtered_quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (0x1 << max_hist_index); 01558 } 01559 else 01560 { 01561 filtered_quantized_surface_normals_ (col_index, row_index) = 0; 01562 } 01563 01564 //filtered_quantized_color_gradients_.data[row_index*width+col_index] = quantized_color_gradients_.data[row_index*width+col_index]; 01565 } 01566 } 01567 01568 01569 01570 //cv::Mat data1(quantized_surface_normals_.height, quantized_surface_normals_.width, CV_8U, quantized_surface_normals_.data); 01571 //cv::Mat data2(filtered_quantized_surface_normals_.height, filtered_quantized_surface_normals_.width, CV_8U, filtered_quantized_surface_normals_.data); 01572 01573 //cv::medianBlur(data1, data2, 3); 01574 01575 //for (int row_index = 0; row_index < height; ++row_index) 01576 //{ 01577 // for (int col_index = 0; col_index < width; ++col_index) 01578 // { 01579 // filtered_quantized_surface_normals_.data[row_index*width+col_index] = 0x1 << filtered_quantized_surface_normals_.data[row_index*width+col_index]; 01580 // } 01581 //} 01582 } 01583 01584 ////////////////////////////////////////////////////////////////////////////////////////////// 01585 template <typename PointInT> void 01586 pcl::SurfaceNormalModality<PointInT>::computeDistanceMap (const MaskMap & input, DistanceMap & output) const 01587 { 01588 const size_t width = input.getWidth (); 01589 const size_t height = input.getHeight (); 01590 01591 output.resize (width, height); 01592 01593 // compute distance map 01594 //float *distance_map = new float[input_->points.size ()]; 01595 const unsigned char * mask_map = input.getData (); 01596 float * distance_map = output.getData (); 01597 for (size_t index = 0; index < width*height; ++index) 01598 { 01599 if (mask_map[index] == 0) 01600 distance_map[index] = 0.0f; 01601 else 01602 distance_map[index] = static_cast<float> (width + height); 01603 } 01604 01605 // first pass 01606 float * previous_row = distance_map; 01607 float * current_row = previous_row + width; 01608 for (size_t ri = 1; ri < height; ++ri) 01609 { 01610 for (size_t ci = 1; ci < width; ++ci) 01611 { 01612 const float up_left = previous_row [ci - 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci-1] + 1.4f; 01613 const float up = previous_row [ci] + 1.0f; //distance_map[(ri-1)*input_->width + ci] + 1.0f; 01614 const float up_right = previous_row [ci + 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci+1] + 1.4f; 01615 const float left = current_row [ci - 1] + 1.0f; //distance_map[ri*input_->width + ci-1] + 1.0f; 01616 const float center = current_row [ci]; //distance_map[ri*input_->width + ci]; 01617 01618 const float min_value = std::min (std::min (up_left, up), std::min (left, up_right)); 01619 01620 if (min_value < center) 01621 current_row[ci] = min_value; //distance_map[ri * input_->width + ci] = min_value; 01622 } 01623 previous_row = current_row; 01624 current_row += width; 01625 } 01626 01627 // second pass 01628 float * next_row = distance_map + width * (height - 1); 01629 current_row = next_row - width; 01630 for (int ri = static_cast<int> (height)-2; ri >= 0; --ri) 01631 { 01632 for (int ci = static_cast<int> (width)-2; ci >= 0; --ci) 01633 { 01634 const float lower_left = next_row [ci - 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci-1] + 1.4f; 01635 const float lower = next_row [ci] + 1.0f; //distance_map[(ri+1)*input_->width + ci] + 1.0f; 01636 const float lower_right = next_row [ci + 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci+1] + 1.4f; 01637 const float right = current_row [ci + 1] + 1.0f; //distance_map[ri*input_->width + ci+1] + 1.0f; 01638 const float center = current_row [ci]; //distance_map[ri*input_->width + ci]; 01639 01640 const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right)); 01641 01642 if (min_value < center) 01643 current_row[ci] = min_value; //distance_map[ri*input_->width + ci] = min_value; 01644 } 01645 next_row = current_row; 01646 current_row -= width; 01647 } 01648 } 01649 01650 01651 #endif