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
00035 #pragma once
00036 #ifndef BUT_PLANE_DET_PARAMSPACE_H
00037 #define BUT_PLANE_DET_PARAMSPACE_H
00038
00039
00040 #include <opencv2/highgui/highgui.hpp>
00041 #include <opencv2/imgproc/imgproc_c.h>
00042
00043
00044 #include <sensor_msgs/CameraInfo.h>
00045
00046 #include <srs_env_model_percp/but_segmentation/normals.h>
00047
00048
00049 namespace srs_env_model_percp
00050 {
00051
00052 #define DEFAULT_ANGLE_STEP 5
00053 #define DEFAULT_SHIFT_STEP 5
00054
00058 class ParameterSpace
00059 {
00060 public:
00061 typedef but_plane_detector::Plane<float> tPlane;
00062 typedef std::vector<tPlane, Eigen::aligned_allocator<tPlane> > tPlanes;
00063
00064 public:
00074 ParameterSpace(double anglemin, double anglemax, double zmin, double zmax, double angleRes = DEFAULT_ANGLE_STEP, double shiftRes = DEFAULT_SHIFT_STEP);
00075
00079 ~ParameterSpace();
00080
00085 int findMaxima(tPlanes &indices);
00086
00092 void generateGaussIn(double angleSigma, double shiftSigma);
00093
00101 void addVolume(ParameterSpace &second, int angle1, int angle2, int shift);
00102
00107 double getAngle(int index);
00108
00113 double getShift(int irndex);
00114
00121 double &operator() (int angle1, int angle2, int z);
00122
00127 double &operator[] (int index);
00128
00135 double &operator() (double angle1, double angle2, double z);
00136
00140 int getSize();
00141
00148 int getIndex(double angle1, double angle2, double z);
00149
00159 void getIndex(double angle1, double angle2, double z, int &angle1Index, int &angle2Index, int &shiftIndex);
00160
00169 static void toAngles(float x, float y, float z, float &a1, float &a2);
00170
00179 static void toEuklid(float a1, float a2, float &x, float &y, float &z);
00180
00184 bool m_init;
00185
00189 double m_angleStep;
00190
00194 double m_shiftStep;
00195
00199 double m_shiftmin;
00200
00204 double m_shiftmax;
00205
00209 double m_anglemin;
00210
00214 double m_anglemax;
00215
00219 int m_size;
00220
00224 int m_angleSize;
00225
00229 int m_angleSize2;
00230
00234 int m_shiftSize;
00235
00239 cv::Mat m_paramSpace;
00240
00244 double *m_data;
00245 };
00246 }
00247
00248 #endif