Point Cloud Library (PCL)  1.3.1
shot_common.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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  * $Id: shot_common.hpp 3275 2011-11-30 16:59:04Z mdixon $
00035  */
00036 
00037 #ifndef PCL_FEATURES_IMPL_SHOT_COMMON_H_
00038 #define PCL_FEATURES_IMPL_SHOT_COMMON_H_
00039 
00040 #include <utility>
00041 
00042 // Useful constants.
00043 #define PST_PI 3.1415926535897932384626433832795
00044 #define PST_RAD_45 0.78539816339744830961566084581988
00045 #define PST_RAD_90 1.5707963267948966192313216916398
00046 #define PST_RAD_135 2.3561944901923449288469825374596
00047 #define PST_RAD_180 PST_PI
00048 #define PST_RAD_360 6.283185307179586476925286766558
00049 #define PST_RAD_PI_7_8 2.7488935718910690836548129603691
00050 
00051 const double zeroDoubleEps15 = 1E-15;
00052 const float zeroFloatEps8 = 1E-8f;
00053 
00055 // Compute a local Reference Frame for a 3D feature; the output is stored in the "rf" vector
00056 template <typename PointInT> float
00057 pcl::getLocalRF (const pcl::PointCloud<PointInT> &cloud, 
00058                  const double search_radius, 
00059                  const Eigen::Vector4f & central_point, 
00060                  const std::vector<int> &indices, 
00061                  const std::vector<float> &dists, 
00062   std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &rf)
00063 {
00064   if (rf.size () != 3)
00065     rf.resize (3);
00066 
00067   // Allocate enough space
00068   Eigen::Vector4d *vij = new Eigen::Vector4d[indices.size ()];
00069 
00070   Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
00071 
00072   double distance = 0.0;
00073   double sum = 0.0;
00074 
00075   int valid_nn_points = 0;
00076 
00077   for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
00078   {
00079     /*if (indices[i_idx] == index)
00080       continue;*/
00081 
00082     Eigen::Vector4f pt = cloud.points[indices[i_idx]].getVector4fMap (); 
00083     pt[3] = 0;
00084 
00085   if (pt == central_point)
00086     continue;
00087 
00088     // Difference between current point and origin
00089     vij[valid_nn_points] = (pt - central_point).cast<double> ();
00090     vij[valid_nn_points][3] = 0;
00091 
00092     distance = search_radius - sqrt (dists[i_idx]);
00093 
00094     // Multiply vij * vij'
00095     cov_m += distance * (vij[valid_nn_points].head<3> () * vij[valid_nn_points].head<3> ().transpose ());
00096 
00097     sum += distance;
00098     valid_nn_points++;
00099   }
00100 
00101   if (valid_nn_points < 5)
00102   {
00103     PCL_ERROR ("[pcl::%s::getSHOTLocalRF] Warning! Neighborhood has less than 5 vertexes. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOT", central_point[0], central_point[1], central_point[2]);
00104     rf[0].setZero ();
00105     rf[1].setZero ();
00106     rf[2].setZero ();
00107 
00108     rf[0][0] = 1;
00109     rf[1][1] = 1;
00110     rf[2][2] = 1;
00111 
00112     delete [] vij;
00113 
00114     return (std::numeric_limits<float>::max ());
00115   }
00116 
00117   cov_m /= sum;
00118 
00119   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
00120 
00121   // Disambiguation
00122  
00123   Eigen::Vector3d v1c = solver.eigenvectors ().col (0);
00124   Eigen::Vector3d v2c = solver.eigenvectors ().col (1);
00125   Eigen::Vector3d v3c = solver.eigenvectors ().col (2);
00126 
00127   double e1c = solver.eigenvalues ()[0];
00128   double e2c = solver.eigenvalues ()[1];
00129   double e3c = solver.eigenvalues ()[2];
00130 
00131   Eigen::Vector4d v1 = Eigen::Vector4d::Zero ();
00132   Eigen::Vector4d v3 = Eigen::Vector4d::Zero ();
00133 
00134   if (e1c > e2c)
00135   {
00136     if (e1c > e3c) // v1c > max(v2c,v3c)
00137     {
00138       v1.head<3> () = v1c;
00139 
00140       if (e2c > e3c)  // v1c > v2c > v3c
00141         v3.head<3> () = v3c;
00142       else // v1c > v3c > v2c
00143         v3.head<3> () = v2c;
00144     }
00145     else // v3c > v1c > v2c
00146     {
00147       v1.head<3> () = v3c;
00148       v3.head<3> () = v2c;
00149     }
00150   }
00151   else
00152   {
00153     if (e2c > e3c) // v2c > max(v1c,v3c)
00154     {
00155       v1.head<3> () = v2c;
00156 
00157       if (e1c > e3c)  // v2c > v1c > v3c
00158         v3.head<3> () = v3c;
00159       else // v2c > v3c > v1c
00160         v3.head<3> () = v1c;
00161     }
00162     else // v3c > v2c > v1c
00163     {
00164       v1.head<3> () = v3c;
00165       v3.head<3> () = v1c;
00166     }
00167   }
00168 
00169   int plusNormal = 0, plusTangentDirection1=0;
00170   for (int ne = 0; ne < valid_nn_points; ne++)
00171   {
00172     double dp = vij[ne].dot (v1);
00173     if (dp >= 0)
00174       plusTangentDirection1++;
00175 
00176     dp = vij[ne].dot (v3);
00177     if (dp >= 0)
00178       plusNormal++;
00179   }
00180 
00181   //TANGENT
00182   if( abs ( plusTangentDirection1 - valid_nn_points + plusTangentDirection1 )  > 0 ) 
00183   {
00184     if (plusTangentDirection1 < valid_nn_points - plusTangentDirection1)
00185       v1 *= - 1;
00186   }
00187   else
00188   {
00189     plusTangentDirection1=0;
00190     int points = 5; 
00191     int medianIndex = valid_nn_points/2;
00192 
00193     for (int i = -points/2; i <= points/2; i++)
00194       if ( vij[medianIndex- i ].dot (v1) > 0)
00195         plusTangentDirection1 ++; 
00196     
00197     if (plusTangentDirection1 < points/2+1)
00198       v1 *= - 1;
00199   }
00200 
00201   //Normal
00202   if( abs ( plusNormal - valid_nn_points + plusNormal )  > 0 ) 
00203   {
00204     if (plusNormal < valid_nn_points - plusNormal)
00205       v3 *= - 1;
00206   }
00207   else 
00208   {
00209     plusNormal = 0;
00210     int points = 5; //std::min(valid_nn_points*2/2+1, 11);
00211     //std::cout << points << std::endl;
00212     int medianIndex = valid_nn_points/2;
00213 
00214     for (int i = -points/2; i <= points/2; i++)
00215       if ( vij[medianIndex- i ].dot (v3) > 0)
00216         plusNormal ++;  
00217   
00218     if (plusNormal < points/2+1)
00219       v3 *= - 1;
00220   }
00221 
00222 
00223 
00224   rf[0] = v1.cast<float>();
00225   rf[2] = v3.cast<float>();
00226   rf[1] = rf[2].cross3 (rf[0]);
00227   rf[0][3] = 0; rf[1][3] = 0; rf[2][3] = 0;
00228 
00229   delete [] vij;
00230 
00231   return (0.0f);
00232 }
00233 
00234 #endif    // PCL_FEATURES_IMPL_SHOT_COMMON_H_
00235 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines