ClearpathDemoTools.h
Go to the documentation of this file.
00001 #ifndef H_CPDEMOTOOLS
00002 #define H_CPDEMOTOOLS
00003 
00004 // Includes
00005 #include <vector>
00006 
00007 // Constants for Static Methods
00008 #define RANSAC_MAX_ITER 150
00009 #define RANSAC_PLANE_SEPDIST 0.1f
00010 #define RANSAC_PLANE_MINNUM 30
00011 #define RANSAC_PLANE_THRESH 0.01f
00012 #define RANSAC_FLOOR_ANGTHRESH 0.1f
00013 #define RANSAC_FLOOR_MAGTHRESH 0.1f
00014 
00015 // Constants for Demo Object
00016 #define MODE_NONE 0
00017 #define MODE_TRACK 1
00018 #define MODE_EXPLORE 2
00019 
00020 #define NUM_INC_ANG 13
00021 #define NUM_INC_RNG 20
00022 #define NUM_INC_X 25
00023 #define NUM_INC_Z 25
00024 
00025 // Structure to hold values about a map square
00026 struct Square
00027 {
00028     public:
00029         double a; // angle (from camera)
00030         double r; // radial distance (from camera)
00031         double x; // dist in x (from camera)
00032         double z; // dist in z (from camera)
00033         unsigned int c; // number of points projected in 2D square
00034 };
00035 
00036 // Structure to hold velocities
00037 struct Twist
00038 {
00039     public:
00040         double linear; // lin vel
00041         double angular; // ang vel
00042 };
00043 
00044 // Structure to hold point
00045 struct Point
00046 {
00047     public:
00048         float x;
00049         float y;
00050         float z;
00051         float rgb;
00052 };
00053 
00054 // Structure to hold point cloud
00055 struct PointCloud
00056 {
00057     public:
00058         std::vector<Point> points;
00059 };
00060 
00061 class ClearpathDemoTools
00062 {
00063         public:
00064 
00065                 ClearpathDemoTools();
00066                 virtual ~ClearpathDemoTools();
00067 
00068         //
00069         // Static Helper Functions
00070         //
00071 
00072         static bool PlaneRANSAC(PointCloud* cloud, double* normal_coeff, bool match_coeff);
00073         static bool PlaneLS(PointCloud* cloud, double* normal_coeff);
00074         static void PlaneSegment(PointCloud* cloud, PointCloud* plane, PointCloud* seg_cloud, double* norm, double thresh);
00075         static bool TransformByNormal(PointCloud* cloud, PointCloud* cloud_out, double* normal_coeff);
00076         static void VerticalCut(PointCloud* cloud, PointCloud* cloud_out, double min, double max, int skim_factor);
00077         static Twist DetermineVelocity(double x, double z, double speed);
00078 
00079         //
00080         // Demo Functions
00081         //
00082 
00083         void InitTrack(double _lin_speed,
00084                        double _ang_speed,
00085                        double _cam_height,
00086                        double _cam_z_trans,
00087                        double _window_size);
00088         void InitExplore(double _lin_speed,
00089                          double _ang_speed,
00090                          double _cam_height,
00091                          double _cam_z_trans,
00092                          double _wall_buffer,
00093                          int _random_walk);
00094         void NIUpdate(float* px, float* py, float* pz, unsigned int len, double* return_lin_vel, double* return_ang_vel);
00095         Twist Update(PointCloud* cloud, PointCloud* result);
00096 
00097         //
00098         // Demo Variables
00099         //
00100 
00101         int mode;
00102 
00103         unsigned int numIncX;
00104         unsigned int numIncZ;
00105 
00106         double lin_speed;
00107         double ang_speed;
00108         double cam_height;
00109         double cam_z_trans;
00110 
00111         Square* init_map;
00112         Square* mapc;
00113 
00114         double last_normal[4];
00115         double base_normal[4];
00116         bool normal_initialized;
00117 
00118         PointCloud cloudCopy;
00119         PointCloud cloudCut;
00120         PointCloud seg;
00121         PointCloud plane;
00122         PointCloud final;
00123 
00124         double minAng;
00125         double maxAng;
00126         double minX;
00127         double maxX;
00128         double minZ;
00129         double maxZ;
00130         double incX;
00131         double incZ;
00132         unsigned int drawInc;
00133         unsigned int obstacleThreshold;
00134 
00135         //
00136         // Track Demo Variables
00137         //
00138 
00139         double targetX;
00140         double targetZ;
00141         double targetW;
00142 
00143         //
00144         // Explore Demo Variables
00145         //
00146 
00147         int random_walk;
00148         double wall_buffer;
00149         bool old_clearpath;
00150         int old_dir;
00151 
00152         private:
00153 
00154         //
00155         // Static Helper Functions
00156         //
00157 
00158         static bool GetPlaneFromRnd3(PointCloud* cloud, double* normal_coeff);
00159 
00160         //
00161         // Demo Functions
00162         //
00163 
00164         void Init(double _lin_speed,
00165                   double _ang_speed,
00166                   double _cam_height,
00167                   double _cam_z_trans,
00168                   unsigned int _numIncX,
00169                   unsigned int _numIncZ);
00170 
00171         void getBins(double x, double z, unsigned int* binX, unsigned int* binZ);
00172 };
00173 
00174 #endif


clearpath_kinect_demo
Author(s): Sean Anderson
autogenerated on Thu Jan 2 2014 11:06:26