Point Cloud Library (PCL)  1.3.1
normal_based_signature.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Alexandru-Eugen Ichim
00005  *                      Willow Garage, Inc
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *  $Id: normal_based_signature.hpp 3023 2011-11-01 03:42:32Z svn $
00036  */
00037 
00038 #ifndef PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_
00039 #define PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_
00040 
00041 #include "pcl/features/normal_based_signature.h"
00042 
00043 template <typename PointT, typename PointNT, typename PointFeature> void
00044 pcl::NormalBasedSignatureEstimation<PointT, PointNT, PointFeature>::computeFeature (FeatureCloud &output)
00045 {
00046   // do a few checks before starting the computations
00047 
00048   PointFeature test_feature;
00049   if (N_prime_ * M_prime_ != sizeof (test_feature.values) / sizeof (float))
00050   {
00051     PCL_ERROR ("NormalBasedSignatureEstimation: not using the proper signature size: %u vs %u\n", N_prime_ * M_prime_, sizeof (test_feature.values) / sizeof (float));
00052     return;
00053   }
00054 
00055   std::vector<int> k_indices;
00056   std::vector<float> k_sqr_distances;
00057 
00058   tree_->setInputCloud (input_);
00059   output.points.resize (indices_->size ());
00060 
00061   for (size_t index_i = 0; index_i < indices_->size (); ++index_i)
00062   {
00063     size_t point_i = (*indices_)[index_i];
00064     Eigen::MatrixXf s_matrix (N_, M_);
00065 
00066     Eigen::Vector4f center_point = input_->points[point_i].getVector4fMap ();
00067 
00068     for (size_t k = 0; k < N_; ++k)
00069     {
00070       Eigen::VectorXf s_row (M_);
00071 
00072       for (size_t l = 0; l < M_; ++l)
00073       {
00074         Eigen::Vector4f normal = normals_->points[point_i].getNormalVector4fMap ();
00075         Eigen::Vector4f normal_u = Eigen::Vector4f::Zero ();
00076         Eigen::Vector4f normal_v = Eigen::Vector4f::Zero ();
00077 
00078         if (fabs (normal.x ()) > 0.0001f)
00079         {
00080           normal_u.x () = - normal.y () / normal.x ();
00081           normal_u.y () = 1.0f;
00082           normal_u.z () = 0.0f;
00083           normal_u.normalize ();
00084 
00085         }
00086         else if (fabs (normal.y ()) > 0.0001f)
00087         {
00088           normal_u.x () = 1.0f;
00089           normal_u.y () = - normal.x () / normal.y ();
00090           normal_u.z () = 0.0f;
00091           normal_u.normalize ();
00092         }
00093         else
00094         {
00095           normal_u.x () = 0.0f;
00096           normal_u.y () = 1.0f;
00097           normal_u.z () = - normal.y () / normal.z ();
00098         }
00099         normal_v = normal.cross3 (normal_u);
00100 
00101         Eigen::Vector4f zeta_point = 2.0f * (l+1) * scale_h_ / M_ * (cos (2.0f * M_PI * (k+1) / N_) * normal_u + sin (2.0f * M_PI * (k+1) / N_) * normal_v);
00102 
00103         // Compute normal by using the neighbors
00104         Eigen::Vector4f zeta_point_plus_center = zeta_point + center_point;
00105         PointT zeta_point_pcl;
00106         zeta_point_pcl.x = zeta_point_plus_center.x (); zeta_point_pcl.y = zeta_point_plus_center.y (); zeta_point_pcl.z = zeta_point_plus_center.z ();
00107 
00108         tree_->radiusSearch (zeta_point_pcl, search_radius_, k_indices, k_sqr_distances);
00109 
00110         // Do k nearest search if there are no neighbors nearby
00111         if (k_indices.size () == 0)
00112         {
00113           k_indices.resize (5);
00114           k_sqr_distances.resize (5);
00115           tree_->nearestKSearch (zeta_point_pcl, 5, k_indices, k_sqr_distances);
00116         }
00117         
00118         Eigen::Vector4f average_normal = Eigen::Vector4f::Zero ();
00119 
00120         float average_normalization_factor = 0.0f;
00121 
00122         // Normals weighted by 1/squared_distances
00123         for (size_t nn_i = 0; nn_i < k_indices.size (); ++nn_i)
00124         {
00125           if (k_sqr_distances[nn_i] < 1e-7f)
00126           {
00127             average_normal = normals_->points[k_indices[nn_i]].getNormalVector4fMap ();
00128             average_normalization_factor = 1.0f;
00129             break;
00130           }
00131           average_normal += normals_->points[k_indices[nn_i]].getNormalVector4fMap () / k_sqr_distances[nn_i];
00132           average_normalization_factor += 1.0f / k_sqr_distances[nn_i];
00133         }
00134         average_normal /= average_normalization_factor;
00135         float s = zeta_point.dot (average_normal) / zeta_point.norm ();
00136         s_row[l] = s;
00137       }
00138 
00139       // do DCT on the s_matrix row-wise
00140       Eigen::VectorXf dct_row (M_);
00141       for (int m = 0; m < s_row.size (); ++m)
00142       {
00143         float Xk = 0.0f;
00144         for (int n = 0; n < s_row.size (); ++n)
00145           Xk += s_row[n] * cos (M_PI / M_ * (n + 0.5f) * k);
00146         dct_row[m] = Xk;
00147       }
00148       s_row = dct_row;
00149       s_matrix.row (k) = dct_row;
00150     }
00151 
00152     // do DFT on the s_matrix column-wise
00153     Eigen::MatrixXf dft_matrix (N_, M_);
00154     for (size_t column_i = 0; column_i < M_; ++column_i)
00155     {
00156       Eigen::VectorXf dft_col (N_);
00157       for (size_t k = 0; k < N_; ++k)
00158       {
00159         float Xk_real = 0.0f, Xk_imag = 0.0f;
00160         for (size_t n = 0; n < N_; ++n)
00161         {
00162           Xk_real += s_matrix(n, column_i) * cos (2.0f * M_PI / N_ * k * n);
00163           Xk_imag += s_matrix(n, column_i) * sin (2.0f * M_PI / N_ * k * n);
00164         }
00165         dft_col[k] = sqrt (Xk_real*Xk_real + Xk_imag*Xk_imag);
00166       }
00167       dft_matrix.col (column_i) = dft_col;
00168     }
00169 
00170     Eigen::MatrixXf final_matrix = dft_matrix.block (0, 0, N_prime_, M_prime_);
00171 
00172     PointFeature feature_point;
00173     for (size_t i = 0; i < N_prime_; ++i)
00174       for (size_t j = 0; j < M_prime_; ++j)
00175         feature_point.values[i*M_prime_ + j] = final_matrix (i, j);
00176 
00177     output.points[index_i] = feature_point;
00178   }
00179 }
00180 
00181 
00182 
00183 #define PCL_INSTANTIATE_NormalBasedSignatureEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::NormalBasedSignatureEstimation<T,NT,OutT>;
00184 
00185 
00186 #endif /* PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines