Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
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
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
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
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
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_