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 <visualization_msgs/Marker.h>
00049 #include <visualization_msgs/MarkerArray.h>
00050 #include <boost/lexical_cast.hpp>
00051 #include <cstdlib>
00052 #include <fstream>
00053 #include <map>
00054 #include <cmath>
00055 #include <colormap/colormap.h>
00056
00057 #define _MAX_CLUSTERS_ 5000
00058 #define _MAX_TRACK_LENGHT_ 1000
00059
00060 #define deg2rad(a) (a*M_PI/180.)
00061 #define rad2deg(a) (a*180./M_PI)
00062
00063 #define pi M_PI
00064
00066 enum enum_velocity_classification {MOVING=5, STATIONARY=6};
00068 enum enum_motion_model {CV,CA,MIX};
00069
00070 using namespace ros;
00071 using namespace std;
00072
00076 struct st_cluster
00077 {
00079 int id;
00081 int stp;
00083 int enp;
00085 int n_points;
00086 double rmin,tm;
00087 double cx,cy,cxp,cyp,cxe,cye;
00088 double r;
00089 bool partialy_occluded;
00090 double lenght;
00091
00092 friend ostream& operator<< (ostream &o, const st_cluster &i)
00093 {
00094 return o
00095 <<"id "<<i.id<<endl
00096 <<"stp "<<i.stp<<" enp "<<i.enp<<endl
00097 <<"n_points "<<i.n_points<<endl
00098 <<"rmin "<<i.rmin<<" tm "<<i.tm<<endl
00099 <<"C(x,y) "<<i.cx<<" "<<i.cy<<endl
00100 <<"Cp(x,y) "<<i.cxp<<" "<<i.cyp<<endl
00101 <<"Ce(x,y) "<<i.cxe<<" "<<i.cye<<endl
00102 <<"r "<<i.r<<endl
00103 <<"partialy_occluded "<<i.partialy_occluded<<endl
00104 <<"lenght "<<i.lenght<<endl;
00105 }
00106 };
00107
00108 typedef st_cluster t_cluster;
00109 typedef boost::shared_ptr<t_cluster> t_clustersPtr;
00110
00114 typedef struct
00115 {
00116 double x[2160],y[2160];
00117 double r[2160],t[2160];
00118 bool flag[2160],flag2[2160];
00119 bool occlusion_data;
00120 int initial_position[2160];
00121 int n_points;
00122 }t_data;
00123
00125 typedef struct
00126 {
00128 double r;
00130 double t;
00131 }t_point;
00132
00136 typedef struct
00137 {
00138 double maxr,in_clip,out_clip;
00139 double in_angle,out_angle;
00140
00142 int w,h;
00143
00145 double fi,beta,C0;
00146
00148 double pm_threshold;
00149
00150 int max_line_per_object;
00152 double max_mean_variance;
00153
00155 int data_acc_scans;
00157 double filter_max_variation;
00159 double excluding_dr;
00161 int max_missing_iterations;
00163 int overlap_max;
00165 double overlap_distance;
00167 unsigned int display_min_lifetime;
00169 double point_occlusion_distance;
00171 unsigned int estimation_window_size;
00173 double dt;
00174
00175 unsigned int velocity_acc_size;
00176
00177 double max_stationary_velocity;
00178 double min_moving_velocity;
00179
00180 unsigned int path_lenght;
00181 double cluster_break_distance;
00182
00183 double default_ellipse_axis;
00184 double max_ellipse_axis;
00185 double min_ellipse_axis;
00186
00187 double ezA;
00188 double ezB;
00189
00190 }t_config;
00191
00195 struct st_line
00196 {
00198 double ro,alpha;
00200 double xi,yi,xf,yf;
00201 };
00202
00203 typedef st_line t_line;
00204 typedef boost::shared_ptr<t_line> t_linePtr;
00205
00209 struct st_object
00210 {
00211 vector<t_linePtr> lines;
00212 double cx,cy;
00213
00214 double rmin,tm;
00215 double size;
00216
00217 int id;
00218
00219 bool object_found;
00220 bool partialy_occluded;
00221
00222 double min_distance_to_existing_object;
00223
00224 friend ostream& operator<< (ostream &o, const st_object &i)
00225 {
00226 return o
00227 <<"C(x,y) "<<i.cx<<" "<<i.cy<<endl
00228 <<"rmin "<<i.rmin<<" tm "<<i.tm<<endl
00229 <<"size "<<i.size<<endl
00230 <<"n_lines "<<i.lines.size()<<endl
00231 <<"id "<<i.id<<endl
00232 <<"object_found "<<i.object_found<<endl
00233 <<"partialy_occluded "<<i.partialy_occluded<<endl
00234 <<"min_distance_to_existing_object "<<i.min_distance_to_existing_object<<endl;
00235 }
00236 };
00237
00238 typedef st_object t_object;
00239 typedef boost::shared_ptr<t_object> t_objectPtr;
00240
00242 typedef struct
00243 {
00245 unsigned int lifetime;
00247 int occludedtime;
00248 }t_timers;
00249
00251 typedef struct
00252 {
00254 CvKalman*cv_x_kalman;
00256 CvKalman*cv_y_kalman;
00257
00259 CvKalman*ca_x_kalman;
00261 CvKalman*ca_y_kalman;
00262 }t_motion_models;
00263
00265 typedef struct
00266 {
00268 double*x_innovation;
00270 double*x_residue;
00271
00273 double*y_innovation;
00275 double*y_residue;
00276
00278 double*lateral_error;
00279
00281 double x_inno_cov;
00283 double x_resi_cov;
00284
00286 double y_inno_cov;
00288 double y_resi_cov;
00289
00291 double lateral_error_cov;
00292
00294 unsigned int number_points;
00296 unsigned int max_number_points;
00298 unsigned int position;
00300 unsigned int latest;
00302 unsigned int next;
00303 }t_errors;
00304
00306 typedef struct
00307 {
00309 double*x;
00311 double*y;
00313 unsigned int number_points;
00315 unsigned int max_number_points;
00317 unsigned int position;
00319 unsigned int latest;
00321 unsigned int next;
00322 }t_path;
00323
00325 typedef struct
00326 {
00328 double velocity_x;
00330 double velocity_y;
00332 double velocity_module;
00334 double velocity_angle;
00335 }t_velocity;
00336
00338 typedef struct
00339 {
00341 double ellipse_A;
00343 double ellipse_B;
00345 double angle;
00347 double center_x,center_y;
00348 }t_search_area;
00349
00351 typedef struct
00352 {
00354 double apparent_size;
00355 }t_object_morphology;
00356
00358 typedef struct
00359 {
00360 enum_velocity_classification velocity_classification;
00361 bool occluded;
00362 bool partialy_occluded;
00363 }t_classification;
00364
00368 typedef struct
00369 {
00371 bool fp_s,fi,raw_only;
00372 }t_flag;
00373
00375 typedef struct
00376 {
00377 double x;
00378 double y;
00379 }t_measurements;
00380
00382 typedef struct
00383 {
00384 double estimated_x;
00385 double estimated_y;
00386 double predicted_x;
00387 double predicted_y;
00388 }t_position;
00389
00393 struct t_linked_list
00394 {
00395 unsigned int id;
00396 t_search_area search_area;
00397 t_path path_cv;
00398 t_path path_ca;
00399 t_velocity velocity;
00400 t_motion_models motion_models;
00401 t_timers timers;
00402 t_errors errors_cv;
00403 t_errors errors_ca;
00404 t_object_morphology object_morphology;
00405 t_classification classification;
00406 t_measurements measurements;
00407 t_position position;
00408 t_object shape;
00409 enum_motion_model model;
00410 };
00411
00412 typedef t_linked_list t_list;
00413 typedef boost::shared_ptr<t_list> t_listPtr;
00414
00416 typedef struct
00417 {
00419 double fps[100];
00421 unsigned int position;
00423 unsigned int current_size;
00424 }t_fps;
00425
00426 #endif
