spin_image_estimation.h
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2010, Hozefa Indorewala <indorewala@ias.in.tum.de>
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Authors: Zoltan-Csaba Marton, Hozefa Indorewala
00032  */
00033 
00034 #ifndef _SPIN_IMAGE_ESTIMATION_H_
00035 #define _SPIN_IMAGE_ESTIMATION_H_
00036 
00037 #include <iostream>
00038 #include <pcl/features/feature.h>
00039 
00040 namespace pcl
00041 {
00042   template <typename PointInT, typename PointNT, typename PointOutT>
00043   class SpinImageEstimation: public FeatureFromNormals<PointInT, PointNT, PointOutT>
00044   {
00045     public:
00046       typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00047       typedef typename Feature<PointInT, PointOutT>::PointCloudIn  PointCloudIn;
00048 
00049     public:
00050       //empty contructor
00051       SpinImageEstimation() {};
00052 
00053       //empty virtual destructor
00054       virtual ~SpinImageEstimation(){};
00055 
00056       inline bool
00057         computeAlphaBeta (const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
00058                              int p_idx, int q_idx, float &alpha, float &beta)
00059       {
00060         if( p_idx == q_idx)
00061           return(false);
00062         Eigen::Vector3f delta_vector, u;
00063 
00064         // Compute the Cartesian difference between the two points i.e. (p_idx - q_idx)
00065         delta_vector[0] = cloud.points[p_idx].x - cloud.points[q_idx].x;
00066         delta_vector[1] = cloud.points[p_idx].y - cloud.points[q_idx].y;
00067         delta_vector[2] = cloud.points[p_idx].z - cloud.points[q_idx].z;
00068 
00069         if (delta_vector.squaredNorm() == 0.0 )
00070         {
00071           ROS_ERROR ("Euclidean distance between points %d and %d is 0!", p_idx, q_idx);
00072           return (false);
00073         }
00074 
00075         //Eigen::Vector4f u = Eigen::Vector4f::Map (normals.points[q_idx].normal[0], 3);
00076         u[0] = normals.points[q_idx].normal[0];
00077         u[1] = normals.points[q_idx].normal[1];
00078         u[2] = normals.points[q_idx].normal[2];
00079 
00080         // Estimate beta_vector= u * delta
00081         beta = fabs(delta_vector.dot(u));
00082 
00083         // Estimate alpha_vector = (- delta_vecto)r x (- u)
00084         alpha = ( ( -delta_vector ).cross( -u ) ).norm();
00085 
00086         return (true);
00087       }
00088 
00090 
00093       inline void
00094         setNrSubdivisions (int nr_subdiv)
00095       {
00096         nr_subdiv_ = nr_subdiv;
00097       }
00098 
00100 
00103       inline void
00104         getNrSubdivisions (int &nr_subdiv)
00105       {
00106         nr_subdiv = nr_subdiv_;
00107       }
00108 
00110 
00113       inline void
00114         setRadius (double radius)
00115       {
00116         radius_ = radius;
00117       }
00118 
00120 
00123       inline void
00124         getRadius (double &radius)
00125       {
00126         radius = radius_;
00127       }
00128 
00129     private:
00130 
00131       inline void
00132         computeFeature (PointCloudOut &output)
00133       {
00134         // Check if input was set
00135         if (!this->normals_)
00136         {
00137           ROS_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!", getName ().c_str ());
00138           return;
00139         }
00140         if (this->normals_->points.size () != this->surface_->points.size ())
00141         {
00142           ROS_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!", getName ().c_str ());
00143           return;
00144         }
00145 
00146         spin_image_histogram_.setZero (nr_subdiv_ * nr_subdiv_);
00147 
00148         // Allocate enough space to hold the results
00149         // \note This resize is irrelevant for a radiusSearch ().
00150         std::vector<int> nn_indices (this->k_);
00151         std::vector<float> nn_dists (this->k_);
00152 
00153         double step_size = radius_ / nr_subdiv_;
00154 
00155         // Iterating over the entire index vector
00156         for (size_t idx = 0; idx < this->indices_->size (); ++idx)
00157         {
00158           searchForNeighbors ((*this->indices_)[idx], this->search_parameter_, nn_indices, nn_dists);
00159 
00160           //select only those neigbors that lie within the radius
00161           for(size_t i = 0 ; i < nn_indices.size(); i++)
00162           {
00163             if(sqrt(nn_dists[i]) > radius_)
00164               continue;
00165             else
00166             {
00167               float alpha, beta;
00168               if( computeAlphaBeta(*this->surface_, *this->normals_, (*this->indices_)[idx], nn_indices[i], alpha, beta) )
00169               {
00170                 int alpha_idx = (int) floor( alpha / step_size);
00171                 int beta_idx = (int)  floor( beta / step_size);
00172                 spin_image_histogram_[ alpha_idx + beta_idx * nr_subdiv_] ++ ;
00173               }
00174             }
00175           }
00176 
00177           // Copy into the resultant cloud
00178           for (int d = 0; d < spin_image_histogram_.size (); ++d)
00179             output.points[idx].histogram[d] = spin_image_histogram_[d];
00180 
00181           // set the histogram values back to zero
00182           spin_image_histogram_.setZero();
00183         }
00184 
00185       }
00186 
00187     private:
00189       double radius_;
00190 
00192       int nr_subdiv_;
00193 
00195       Eigen::VectorXf spin_image_histogram_;
00196 
00198 
00199       std::string getName () const { return ("SpinImageEstimation"); }
00200 
00201   }; //class SpinImageEstimation
00202 } // end namespace pcl
00203 
00204 #endif /* #ifndef _SPIN_IMAGE_ESTIMATION_H_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pointcloud_registration
Author(s): Hozefa Indorewala
autogenerated on Sun Oct 6 2013 11:55:58