Go to the documentation of this file.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
00031 #ifndef _MTT_COMMON_H_
00032 #define _MTT_COMMON_H_
00033
00034
00035
00036 #include <ros/ros.h>
00037 #include <tf/transform_listener.h>
00038 #include <laser_geometry/laser_geometry.h>
00039 #include <iostream>
00040 #include <opencv/cv.h>
00041 #include <opencv/cxcore.h>
00042 #include <opencv/cxcore.hpp>
00043 #include <signal.h>
00044 #include <pcl/point_types.h>
00045 #include <pcl/io/pcd_io.h>
00046 #include <pcl/kdtree/kdtree_flann.h>
00047 #include <pcl/surface/mls.h>
00048 #include <pcl_conversions/pcl_conversions.h>
00049 #include <visualization_msgs/Marker.h>
00050 #include <visualization_msgs/MarkerArray.h>
00051 #include <boost/lexical_cast.hpp>
00052 #include <cstdlib>
00053 #include <fstream>
00054 #include <map>
00055 #include <cmath>
00056 #include <colormap/colormap.h>
00057
00058 #define _MAX_CLUSTERS_ 5000
00059 #define _MAX_TRACK_LENGHT_ 1000
00060
00061 #define deg2rad(a) (a*M_PI/180.)
00062 #define rad2deg(a) (a*180./M_PI)
00063
00064 #define pi M_PI
00065
00067 enum enum_velocity_classification {MOVING=5, STATIONARY=6};
00069 enum enum_motion_model {CV,CA,MIX};
00070
00071 using namespace ros;
00072 using namespace std;
00073
00077 struct st_cluster
00078 {
00080 int id;
00082 int stp;
00084 int enp;
00086 int n_points;
00087 double rmin,tm;
00088 double cx,cy,cxp,cyp,cxe,cye;
00089 double r;
00090 bool partialy_occluded;
00091 double lenght;
00092
00093 friend ostream& operator<< (ostream &o, const st_cluster &i)
00094 {
00095 return o
00096 <<"id "<<i.id<<endl
00097 <<"stp "<<i.stp<<" enp "<<i.enp<<endl
00098 <<"n_points "<<i.n_points<<endl
00099 <<"rmin "<<i.rmin<<" tm "<<i.tm<<endl
00100 <<"C(x,y) "<<i.cx<<" "<<i.cy<<endl
00101 <<"Cp(x,y) "<<i.cxp<<" "<<i.cyp<<endl
00102 <<"Ce(x,y) "<<i.cxe<<" "<<i.cye<<endl
00103 <<"r "<<i.r<<endl
00104 <<"partialy_occluded "<<i.partialy_occluded<<endl
00105 <<"lenght "<<i.lenght<<endl;
00106 }
00107 };
00108
00109 typedef st_cluster t_cluster;
00110 typedef boost::shared_ptr<t_cluster> t_clustersPtr;
00111
00115 typedef struct
00116 {
00117 double x[2160],y[2160];
00118 double r[2160],t[2160];
00119 bool flag[2160],flag2[2160];
00120 bool occlusion_data;
00121 int initial_position[2160];
00122 int n_points;
00123 }t_data;
00124
00126 typedef struct
00127 {
00129 double r;
00131 double t;
00132 }t_point;
00133
00137 typedef struct
00138 {
00139 double maxr,in_clip,out_clip;
00140 double in_angle,out_angle;
00141
00143 int w,h;
00144
00146 double fi,beta,C0;
00147
00149 double pm_threshold;
00150
00151 int max_line_per_object;
00153 double max_mean_variance;
00154
00156 int data_acc_scans;
00158 double filter_max_variation;
00160 double excluding_dr;
00162 int max_missing_iterations;
00164 int overlap_max;
00166 double overlap_distance;
00168 unsigned int display_min_lifetime;
00170 double point_occlusion_distance;
00172 unsigned int estimation_window_size;
00174 double dt;
00175
00176 unsigned int velocity_acc_size;
00177
00178 double max_stationary_velocity;
00179 double min_moving_velocity;
00180
00181 unsigned int path_lenght;
00182 double cluster_break_distance;
00183
00184 double default_ellipse_axis;
00185 double max_ellipse_axis;
00186 double min_ellipse_axis;
00187
00188 double ezA;
00189 double ezB;
00190
00191 }t_config;
00192
00196 struct st_line
00197 {
00199 double ro,alpha;
00201 double xi,yi,xf,yf;
00202 };
00203
00204 typedef st_line t_line;
00205 typedef boost::shared_ptr<t_line> t_linePtr;
00206
00210 struct st_object
00211 {
00212 vector<t_linePtr> lines;
00213 double cx,cy;
00214
00215 double rmin,tm;
00216 double size;
00217
00218 int id;
00219
00220 bool object_found;
00221 bool partialy_occluded;
00222
00223 double min_distance_to_existing_object;
00224
00225 friend ostream& operator<< (ostream &o, const st_object &i)
00226 {
00227 return o
00228 <<"C(x,y) "<<i.cx<<" "<<i.cy<<endl
00229 <<"rmin "<<i.rmin<<" tm "<<i.tm<<endl
00230 <<"size "<<i.size<<endl
00231 <<"n_lines "<<i.lines.size()<<endl
00232 <<"id "<<i.id<<endl
00233 <<"object_found "<<i.object_found<<endl
00234 <<"partialy_occluded "<<i.partialy_occluded<<endl
00235 <<"min_distance_to_existing_object "<<i.min_distance_to_existing_object<<endl;
00236 }
00237 };
00238
00239 typedef st_object t_object;
00240 typedef boost::shared_ptr<t_object> t_objectPtr;
00241
00243 typedef struct
00244 {
00246 unsigned int lifetime;
00248 int occludedtime;
00249 }t_timers;
00250
00252 typedef struct
00253 {
00255 CvKalman*cv_x_kalman;
00257 CvKalman*cv_y_kalman;
00258
00260 CvKalman*ca_x_kalman;
00262 CvKalman*ca_y_kalman;
00263 }t_motion_models;
00264
00266 typedef struct
00267 {
00269 double*x_innovation;
00271 double*x_residue;
00272
00274 double*y_innovation;
00276 double*y_residue;
00277
00279 double*lateral_error;
00280
00282 double x_inno_cov;
00284 double x_resi_cov;
00285
00287 double y_inno_cov;
00289 double y_resi_cov;
00290
00292 double lateral_error_cov;
00293
00295 unsigned int number_points;
00297 unsigned int max_number_points;
00299 unsigned int position;
00301 unsigned int latest;
00303 unsigned int next;
00304 }t_errors;
00305
00307 typedef struct
00308 {
00310 double*x;
00312 double*y;
00314 unsigned int number_points;
00316 unsigned int max_number_points;
00318 unsigned int position;
00320 unsigned int latest;
00322 unsigned int next;
00323 }t_path;
00324
00326 typedef struct
00327 {
00329 double velocity_x;
00331 double velocity_y;
00333 double velocity_module;
00335 double velocity_angle;
00336 }t_velocity;
00337
00339 typedef struct
00340 {
00342 double ellipse_A;
00344 double ellipse_B;
00346 double angle;
00348 double center_x,center_y;
00349 }t_search_area;
00350
00352 typedef struct
00353 {
00355 double apparent_size;
00356 }t_object_morphology;
00357
00359 typedef struct
00360 {
00361 enum_velocity_classification velocity_classification;
00362 bool occluded;
00363 bool partialy_occluded;
00364 }t_classification;
00365
00369 typedef struct
00370 {
00372 bool fp_s,fi,raw_only;
00373 }t_flag;
00374
00376 typedef struct
00377 {
00378 double x;
00379 double y;
00380 }t_measurements;
00381
00383 typedef struct
00384 {
00385 double estimated_x;
00386 double estimated_y;
00387 double predicted_x;
00388 double predicted_y;
00389 }t_position;
00390
00394 struct t_linked_list
00395 {
00396 unsigned int id;
00397 t_search_area search_area;
00398 t_path path_cv;
00399 t_path path_ca;
00400 t_velocity velocity;
00401 t_motion_models motion_models;
00402 t_timers timers;
00403 t_errors errors_cv;
00404 t_errors errors_ca;
00405 t_object_morphology object_morphology;
00406 t_classification classification;
00407 t_measurements measurements;
00408 t_position position;
00409 t_object shape;
00410 enum_motion_model model;
00411 };
00412
00413 typedef t_linked_list t_list;
00414 typedef boost::shared_ptr<t_list> t_listPtr;
00415
00417 typedef struct
00418 {
00420 double fps[100];
00422 unsigned int position;
00424 unsigned int current_size;
00425 }t_fps;
00426
00427 #endif