sgf5.h
Go to the documentation of this file.
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_FEATURES_SGF5_H_
00039 #define PCL_FEATURES_SGF5_H_
00040 
00041 #include <pcl17/features/feature.h>
00042 
00043 namespace pcl17
00044 {
00045   const int SGF5_SIZE = 3;
00046 
00047   template <typename PointInT, typename PointOutT>
00048   class SGF5Estimation : public Feature<PointInT, PointOutT>
00049   {
00050 
00051     public:
00052 
00053       using Feature<PointInT, PointOutT>::feature_name_;
00054       using Feature<PointInT, PointOutT>::input_;
00055       using Feature<PointInT, PointOutT>::indices_;
00056       using Feature<PointInT, PointOutT>::k_;
00057 
00058       typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00059       typedef typename Feature<PointInT, PointOutT>::PointCloudIn  PointCloudIn;
00060 
00062       SGF5Estimation ()
00063       {
00064         feature_name_ = "SGF5Estimation";
00065         k_ = 1;
00066       };
00067 
00068 
00070       void
00071       computeFeature (PointCloudOut &output)
00072       {
00073         // Copy the points specified by the index vector into a new cloud
00074         typename PointCloud<PointInT>::Ptr cloud (new PointCloud<PointInT> ());
00075         cloud->width = indices_->size ();
00076         cloud->height = 1;
00077         cloud->points.resize (cloud->width * cloud->height);
00078         for (size_t idx = 0; idx < indices_->size (); ++idx)
00079         {
00080           cloud->points[idx] = input_->points[(*indices_)[idx]];
00081         }
00082 
00083 
00084         // Compute eigenvectors and eigenvalues
00085         EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00086         Eigen::Vector4f centroid3;
00087         compute3DCentroid (*cloud, centroid3);
00088         computeCovarianceMatrix (*cloud, centroid3, covariance_matrix);
00089         EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
00090         EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
00091         pcl17::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
00092         Eigen::Vector3f e1 (eigen_vectors (0, 0), eigen_vectors (1, 0), eigen_vectors (2, 0));
00093         Eigen::Vector3f e2 (eigen_vectors (0, 1), eigen_vectors (1, 1), eigen_vectors (2, 1));
00094         Eigen::Vector3f e3 (eigen_vectors (0, 2), eigen_vectors (1, 2), eigen_vectors (2, 2));
00095 
00096 
00097         // Project the cloud onto the eigenvectors
00098         typename PointCloud<PointXYZ>::Ptr proj_cloud (new PointCloud<PointXYZ> ());
00099         proj_cloud->width = cloud->width * cloud->height;
00100         proj_cloud->height = 1;
00101         proj_cloud->points.resize (proj_cloud->width * proj_cloud->height);
00102         for (size_t idx = 0; idx < cloud->width * cloud->height; ++idx)
00103         {
00104           Eigen::Vector3f curr_point (cloud->points[idx].x, cloud->points[idx].y, cloud->points[idx].z);
00105           proj_cloud->points[idx].x = curr_point.dot (e1);
00106           proj_cloud->points[idx].y = curr_point.dot (e2);
00107           proj_cloud->points[idx].z = curr_point.dot (e3);
00108         }
00109 
00110 
00111         // Compute the variances of the projected cloud
00112         EIGEN_ALIGN16 Eigen::Matrix3f proj_cov;
00113         Eigen::Vector4f proj_cent;
00114         compute3DCentroid (*proj_cloud, proj_cent);
00115         computeCovarianceMatrix (*cloud, proj_cent, proj_cov);
00116 
00117 
00118         // Compute the feature vector
00119         output.points[0].histogram[0] = proj_cov (0, 0);
00120         output.points[0].histogram[1] = proj_cov (1, 1);
00121         output.points[0].histogram[2] = proj_cov (2, 2);
00122       }
00124 
00125 
00126     private:
00127 
00131       void
00132       computeFeatureEigen (pcl17::PointCloud<Eigen::MatrixXf> &) {}
00133   };
00134 }
00135 
00136 #endif  //#ifndef PCL_FEATURES_SGF5_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


furniture_features
Author(s): Stefan
autogenerated on Thu May 23 2013 16:09:52