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
00033 #pragma once
00034 #ifndef BUT_PLANE_DET_SCENEMODEL_H
00035 #define BUT_PLANE_DET_SCENEMODEL_H
00036
00037
00038 #include <pcl/point_types.h>
00039 #include <pcl/point_cloud.h>
00040 #include <pcl/io/pcd_io.h>
00041
00042 #include <pcl/kdtree/kdtree_flann.h>
00043 #include <pcl/features/normal_3d.h>
00044 #include <pcl/surface/gp3.h>
00045 #include <pcl/io/vtk_io.h>
00046 #include <pcl/filters/voxel_grid.h>
00047 #include <pcl_ros/point_cloud.h>
00048
00049
00050 #include <opencv2/highgui/highgui.hpp>
00051 #include <opencv2/imgproc/imgproc_c.h>
00052
00053
00054 #include <srs_env_model_percp/but_segmentation/normals.h>
00055 #include <srs_env_model_percp/but_plane_detector/parameter_space.h>
00056 #include <srs_env_model_percp/but_plane_detector/parameter_space_hierarchy.h>
00057
00058
00059 #include <tf/transform_listener.h>
00060 #include <tf/message_filter.h>
00061 #include <tf/tfMessage.h>
00062
00063 namespace srs_env_model_percp
00064 {
00065 class SceneModel
00066 {
00067 public:
00068 typedef but_plane_detector::Plane<float> tPlane;
00069 typedef std::vector<tPlane, Eigen::aligned_allocator<tPlane> > tPlanes;
00070
00071 public:
00084 SceneModel( double max_depth = 3.0,
00085 double min_shift = -40.0,
00086 double max_shift = 40.0,
00087 int angle_resolution = 512,
00088 int shift_resolution = 4096,
00089 int gauss_angle_res = 11,
00090 int gauss_shift_res = 11,
00091 double gauss_angle_sigma = 0.04,
00092 double gauss_shift_sigma = 0.15,
00093 int lvl1_gauss_angle_res = 21,
00094 int lvl1_gauss_shift_res = 21,
00095 double lvl1_gauss_angle_sigma = 5.0,
00096 double lvl1_gauss_shift_sigma = 5.0,
00097 double plane_merge_angle = 0.3,
00098 double plane_merge_shift = 0.1);
00099
00106 void AddNext(cv::Mat &depth, const sensor_msgs::CameraInfoConstPtr& cam_info, but_plane_detector::Normals &normals, double min_current);
00107
00112 void AddNext(but_plane_detector::Normals &normals, double min_current);
00113
00118 void clearNoise(double minValue);
00119
00125 void recomputePlanes(double min_current, double min_global, int blur, int search_neighborhood, double substraction);
00126
00130 pcl::PointCloud<pcl::PointXYZRGB>::Ptr scene_cloud;
00131
00135 tPlanes planes;
00136
00137 public:
00141 ParameterSpaceHierarchy space;
00142 ParameterSpaceHierarchy current_space;
00143
00147
00148
00152 ParameterSpace gauss;
00153 ParameterSpace gaussPlane;
00154
00158 int max_plane;
00159
00163 double indexFactor;
00164
00168 double m_depth;
00169
00170 double m_angle_min;
00171 double m_angle_max;
00172 double m_shift_min;
00173 double m_shift_max;
00174 int m_angle_res;
00175 int m_shift_res;
00176
00177 double m_plane_merge_shift;
00178 double m_plane_merge_angle;
00179 };
00180 }
00181
00182 #endif