00001 #include <sys/time.h>
00002 #include "c3_hlac/c3_hlac.h"
00003 #include "c3_hlac/c3_hlac_tools.h"
00004 #include <pcl_cloud_algos/pcl_cloud_algos_point_types.h>
00005 #include <pcl/point_types.h>
00006 #include <pcl/features/feature.h>
00007 #include <pcl/io/pcd_io.h>
00008 #include <pcl/features/normal_3d.h>
00009 #include <pcl/features/rsd.h>
00010
00011
00012 #define NR_CLASS 5
00013 #define NOISE 0
00014 #define PLANE 1
00015 #define CYLINDER 2
00016 #define SPHERE 3
00017 #define EDGE 4
00018 #define EMPTY 5
00019
00020 #define NR_DIV 7 // number of normal angle divisions
00021
00022
00023
00024
00025 const double min_radius_plane_ = 0.066;
00026 const double min_radius_noise_ = 0.030, max_radius_noise_ = 0.050;
00027 const double max_min_radius_diff_ = 0.02;
00028 const double min_radius_edge_ = 0.030;
00029 const double rsd_radius_search = 0.01;
00030 const double normals_radius_search = 0.02;
00031
00032
00033
00034 const float NORMALIZE_GRSD = 20.0 / 26;
00035 const int GRSD_LARGE_DIM = 325;
00036
00037
00039 template <typename T>
00040 bool readPoints( const char *name, pcl::PointCloud<T>& cloud );
00041
00042
00044 template <typename T1, typename T2>
00045 void computeNormal( pcl::PointCloud<T1> input_cloud, pcl::PointCloud<T2>& output_cloud );
00046
00047
00049 int getType (float min_radius, float max_radius);
00050
00051
00053 template <typename T>
00054 Eigen::Vector3i extractGRSDSignature21(pcl::VoxelGrid<T> grid, pcl::PointCloud<T> cloud, pcl::PointCloud<T> cloud_downsampled, std::vector< std::vector<float> > &feature, const float voxel_size, const int subdivision_size = 0, const int offset_x = 0, const int offset_y = 0, const int offset_z = 0, const bool is_normalize = false );
00055
00056 template <typename T>
00057 void extractGRSDSignature21(pcl::VoxelGrid<T> grid, pcl::PointCloud<T> cloud, pcl::PointCloud<T> cloud_downsampled, std::vector<float> &feature, const float voxel_size, const bool is_normalize = false );
00058
00059
00061 template <typename T>
00062 Eigen::Vector3i extractGRSDSignature325(pcl::VoxelGrid<T> grid, pcl::PointCloud<T> cloud, pcl::PointCloud<T> cloud_downsampled, std::vector< std::vector<float> > &feature, const float voxel_size, const int subdivision_size = 0, const int offset_x = 0, const int offset_y = 0, const int offset_z = 0, const bool is_normalize = false );
00063
00064 template <typename T>
00065 void extractGRSDSignature325(pcl::VoxelGrid<T> grid, pcl::PointCloud<T> cloud, pcl::PointCloud<T> cloud_downsampled, std::vector<float> &feature, const float voxel_size, const bool is_normalize = false );
00066
00067
00069 template <typename T>
00070 Eigen::Vector3i extractPlusGRSDSignature110(pcl::VoxelGrid<T> grid, pcl::PointCloud<T> cloud, pcl::PointCloud<T> cloud_downsampled, std::vector< std::vector<float> > &feature, const float voxel_size, const int subdivision_size = 0, const int offset_x = 0, const int offset_y = 0, const int offset_z = 0, const bool is_normalize = false );
00071
00072 template <typename T>
00073 void extractPlusGRSDSignature110(pcl::VoxelGrid<T> grid, pcl::PointCloud<T> cloud, pcl::PointCloud<T> cloud_downsampled, std::vector<float> &feature, const float voxel_size, const bool is_normalize = false );
00074
00075
00077 const std::vector<float> concVector( const std::vector<float> v1, const std::vector<float> v2 );
00078
00079
00081 template <typename PointT>
00082 Eigen::Vector3i extractVOSCH(pcl::VoxelGrid<PointT> grid, pcl::PointCloud<PointT> cloud, pcl::PointCloud<PointT> cloud_downsampled, std::vector< std::vector<float> > &feature, int color_threshold_r, int color_threshold_g, int color_threshold_b, const float voxel_size, const int subdivision_size = 0, const int offset_x = 0, const int offset_y = 0, const int offset_z = 0, const bool is_normalize = false );
00083
00084 template <typename PointT>
00085 void extractVOSCH(pcl::VoxelGrid<PointT> grid, pcl::PointCloud<PointT> cloud, pcl::PointCloud<PointT> cloud_downsampled, std::vector<float> &feature, int color_threshold_r, int color_threshold_g, int color_threshold_b, const float voxel_size, const int subdivision_size = 0, const int offset_x = 0, const int offset_y = 0, const int offset_z = 0 );
00086
00087
00089 template <typename PointT>
00090 Eigen::Vector3i extractConVOSCH(pcl::VoxelGrid<PointT> grid, pcl::PointCloud<PointT> cloud, pcl::PointCloud<PointT> cloud_downsampled, std::vector< std::vector<float> > &feature, int color_threshold_r, int color_threshold_g, int color_threshold_b, const float voxel_size, const int subdivision_size = 0, const int offset_x = 0, const int offset_y = 0, const int offset_z = 0, const bool is_normalize = false );
00091
00092 template <typename PointT>
00093 void extractConVOSCH(pcl::VoxelGrid<PointT> grid, pcl::PointCloud<PointT> cloud, pcl::PointCloud<PointT> cloud_downsampled, std::vector<float> &feature, int color_threshold_r, int color_threshold_g, int color_threshold_b, const float voxel_size, const int subdivision_size = 0, const int offset_x = 0, const int offset_y = 0, const int offset_z = 0 );
00094
00095 #include <vosch/vosch_tools.hpp>