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
00032 #ifndef _TYPES_DECLARATION_H_
00033 #define _TYPES_DECLARATION_H_
00034
00035 #include <iostream>
00036 #include <iomanip>
00037 #include <boost/shared_ptr.hpp>
00038 #include <boost/bind.hpp>
00039 #include <vector>
00040
00041 #include <opencv/cv.h>
00042 #include <opencv/cxcore.h>
00043 #include <opencv/cxcore.hpp>
00044
00045 #include <pcl/point_cloud.h>
00046 #include <pcl/point_types.h>
00047
00048 #define deg2rad(a) (a*M_PI/180.)
00049 #define rad2deg(a) (a*180./M_PI)
00050
00051 using namespace std;
00052 using namespace boost;
00053
00054 typedef struct
00055 {
00056 double theta;
00057 CvSeq*range;
00058 CvMemStorage*range_storage;
00059 }ray_measurment_t;
00060
00061 typedef struct
00062 {
00063 double angular_resolution;
00064 double minimum_delta;
00065 int max_points_per_ray;
00066 int num_rays;
00067 double x,y,t;
00068 double timestamp;
00069 }ray_config_t;
00070
00071 typedef struct
00072 {
00073 ray_config_t cfg;
00074 ray_measurment_t*dt;
00075 }ray_definition;
00076
00077 typedef struct
00078 {
00079 CvSeq*data;
00080 CvMemStorage*data_storage;
00081 }ray_history;
00082
00083 typedef struct
00084 {
00085 double fps[100];
00086 unsigned int position;
00087 unsigned int current_size;
00088 }t_fps;
00089
00093 typedef struct
00094 {
00096 bool fp_s;
00098 bool fi;
00100 bool raw_only;
00101 }t_flag;
00102
00103 typedef struct s_pose
00104 {
00105 s_pose()
00106 {
00107 x=0;
00108 y=0;
00109 phi=0;
00110 orientation=0;
00111 vl=0;
00112 vr=0;
00113 m_phi=0;
00114 m_vl=0;
00115 timestamp=0;
00116 }
00117
00118 double x, y, phi;
00119
00120 double orientation;
00121 double vl,vr;
00122
00123 double m_phi,m_vl;
00124
00125 double timestamp;
00126
00127 friend ostream& operator<< (ostream &o, const s_pose &i)
00128 {
00129 return o << setprecision(4) << fixed
00130 <<i.x<<" "<<i.y<<" "<<i.phi<<" "<<i.orientation<<" "<<i.vl<<" "<<0<<" "<<i.m_phi<<" "<<i.m_vl<<" "<<i.timestamp;
00131 }
00132
00133 } t_pose;
00134
00135 typedef boost::shared_ptr<t_pose> t_posePtr;
00136
00137 double max_orientation(const std::vector<t_posePtr>& vec);
00138 double min_orientation(const std::vector<t_posePtr>& vec);
00139 double max_x(const std::vector<t_posePtr>& vec);
00140 double min_x(const std::vector<t_posePtr>& vec);
00141 double max_y(const std::vector<t_posePtr>& vec);
00142 double min_y(const std::vector<t_posePtr>& vec);
00143
00144 double get_m_phi(t_pose&i);
00145 double get_m_vl(t_pose&i);
00146 double get_phi(t_pose&i);
00147 double get_orientation(t_pose&i);
00148
00149 double get_vl(t_pose&i);
00150
00151 void ClearRays(ray_measurment_t*rays,ray_config_t*ray_config);
00152 void ClearRays(ray_definition*src);
00153 int Theta2Index(double theta,ray_config_t*config);
00154 void RemoveOverlappingPoints(ray_config_t*config,ray_measurment_t*rays);
00155 void RemoveOverlappingPoints(ray_definition*src);
00156 void Pointcloud2Rays(pcl::PointCloud<pcl::PointXYZ>& point_cloud,ray_config_t*config,ray_measurment_t*rays);
00157 void PointCloud2Ray(pcl::PointCloud<pcl::PointXYZ>& point_cloud,ray_definition*dst);
00158 void InitRayDefinition(ray_definition*src);
00159 void InitRayHistory(ray_history*h_rays);
00160 void AddToHistory(ray_definition*rays,ray_history*h_rays);
00161 double get_fps(double dt,t_fps*acc);
00162 void CopyRays(ray_config_t*cfg,ray_measurment_t*src,ray_measurment_t*dst);
00163 void CopyRays(ray_definition*src,ray_definition*dst);
00164
00165 #endif