mtt_common.h

Go to the documentation of this file.

/**************************************************************************************************
 Software License Agreement (BSD License)

 Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
 All rights reserved.

 Redistribution and use in source and binary forms, with or without modification, are permitted
 provided that the following conditions are met:

  *Redistributions of source code must retain the above copyright notice, this list of
   conditions and the following disclaimer.
  *Redistributions in binary form must reproduce the above copyright notice, this list of
   conditions and the following disclaimer in the documentation and/or other materials provided
   with the distribution.
  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
   endorse or promote products derived from this software without specific prior written permission.
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
 IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***************************************************************************************************/
#ifndef _MTT_COMMON_H_
#define _MTT_COMMON_H_

//System Includes

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>
#include <iostream>
#include <opencv/cv.h>
#include <opencv/cxcore.h>
#include <opencv/cxcore.hpp>
#include <signal.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/surface/mls.h>
#include <pcl_conversions/pcl_conversions.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <boost/lexical_cast.hpp>
#include <cstdlib>
#include <fstream>
#include <map>
#include <cmath>
#include <colormap/colormap.h>

#define _MAX_CLUSTERS_ 5000
#define _MAX_TRACK_LENGHT_ 1000

#define deg2rad(a) (a*M_PI/180.)
#define rad2deg(a) (a*180./M_PI)

#define pi M_PI

enum enum_velocity_classification {MOVING=5, STATIONARY=6};
enum enum_motion_model {CV,CA,MIX};

using namespace ros;
using namespace std;

struct st_cluster
{
        int id;                 
        int stp;
        int enp;        
        int n_points;   
        double rmin,tm;
        double cx,cy,cxp,cyp,cxe,cye;
        double r;
        bool partialy_occluded;
        double lenght;
        
        friend ostream& operator<< (ostream &o, const st_cluster &i)
        {
                return o
                <<"id "<<i.id<<endl
                <<"stp "<<i.stp<<" enp "<<i.enp<<endl
                <<"n_points "<<i.n_points<<endl
                <<"rmin "<<i.rmin<<" tm "<<i.tm<<endl
                <<"C(x,y) "<<i.cx<<" "<<i.cy<<endl
                <<"Cp(x,y) "<<i.cxp<<" "<<i.cyp<<endl
                <<"Ce(x,y) "<<i.cxe<<" "<<i.cye<<endl
                <<"r "<<i.r<<endl
                <<"partialy_occluded "<<i.partialy_occluded<<endl
                <<"lenght "<<i.lenght<<endl;
        }
};

typedef st_cluster t_cluster;
typedef boost::shared_ptr<t_cluster> t_clustersPtr;

typedef struct
{
        double x[2160],y[2160];
        double r[2160],t[2160];
        bool flag[2160],flag2[2160];
        bool occlusion_data;
        int initial_position[2160];
        int n_points;
}t_data;

typedef struct
{
        double r;
        double t;
}t_point;

typedef struct
{
        double maxr,in_clip,out_clip;
        double in_angle,out_angle;
        
        int w,h;                                                        
        
        double fi,beta,C0;                                      
        
        double pm_threshold;                            
        
        int max_line_per_object;
        double max_mean_variance;
        
        int data_acc_scans;
        double filter_max_variation;
        double excluding_dr;
        int max_missing_iterations;
        int overlap_max;
        double overlap_distance;
        unsigned int display_min_lifetime;
        double point_occlusion_distance;
        unsigned int estimation_window_size;
        double dt;
        
        unsigned int velocity_acc_size;
        
        double max_stationary_velocity;
        double min_moving_velocity;
        
        unsigned int path_lenght;
        double cluster_break_distance;
        
        double default_ellipse_axis;
        double max_ellipse_axis;
        double min_ellipse_axis;
        
        double ezA;
        double ezB;
        
}t_config;

struct st_line
{
        double ro,alpha;
        double xi,yi,xf,yf;
};

typedef st_line t_line;
typedef boost::shared_ptr<t_line> t_linePtr;

struct st_object
{
        vector<t_linePtr> lines;
        double cx,cy;
        
        double rmin,tm;
        double size;
        
        int id;
        
        bool object_found;
        bool partialy_occluded;
        
        double min_distance_to_existing_object;
        
        friend ostream& operator<< (ostream &o, const st_object &i)
        {
                return o
                <<"C(x,y) "<<i.cx<<" "<<i.cy<<endl
                <<"rmin "<<i.rmin<<" tm "<<i.tm<<endl
                <<"size "<<i.size<<endl
                <<"n_lines "<<i.lines.size()<<endl
                <<"id "<<i.id<<endl
                <<"object_found "<<i.object_found<<endl
                <<"partialy_occluded "<<i.partialy_occluded<<endl
                <<"min_distance_to_existing_object "<<i.min_distance_to_existing_object<<endl;
        }
};

typedef st_object t_object;
typedef boost::shared_ptr<t_object> t_objectPtr;

typedef struct
{
        unsigned int lifetime;
        int occludedtime;
}t_timers;

typedef struct
{
        CvKalman*cv_x_kalman;
        CvKalman*cv_y_kalman;
        
        CvKalman*ca_x_kalman;
        CvKalman*ca_y_kalman;
}t_motion_models;

typedef struct
{
        double*x_innovation;
        double*x_residue;
        
        double*y_innovation;
        double*y_residue;
        
        double*lateral_error;
        
        double x_inno_cov;
        double x_resi_cov;
        
        double y_inno_cov;
        double y_resi_cov;
        
        double lateral_error_cov;
        
        unsigned int number_points;
        unsigned int max_number_points;
        unsigned int position;
        unsigned int latest;
        unsigned int next;
}t_errors;

typedef struct
{
        double*x;
        double*y;
        unsigned int number_points;
        unsigned int max_number_points;
        unsigned int position;
        unsigned int latest;
        unsigned int next;
}t_path;

typedef struct
{
        double velocity_x;
        double velocity_y;
        double velocity_module;
        double velocity_angle;
}t_velocity;

typedef struct
{
        double ellipse_A;
        double ellipse_B;
        double angle;
        double center_x,center_y;
}t_search_area;

typedef struct
{
        double apparent_size;
}t_object_morphology;

typedef struct
{
        enum_velocity_classification velocity_classification;
        bool occluded;
        bool partialy_occluded;
}t_classification;

typedef struct
{
        bool fp_s,fi,raw_only;  
}t_flag;

typedef struct
{
        double x;
        double y;
}t_measurements;

typedef struct
{
        double estimated_x;
        double estimated_y;
        double predicted_x;
        double predicted_y;
}t_position;

struct t_linked_list
{
        unsigned int id;
        t_search_area search_area;
        t_path path_cv;
        t_path path_ca;
        t_velocity velocity;
        t_motion_models motion_models;
        t_timers timers;
        t_errors errors_cv;
        t_errors errors_ca;
        t_object_morphology object_morphology;
        t_classification classification;
        t_measurements measurements;
        t_position position;
        t_object shape;
        enum_motion_model model;
};

typedef t_linked_list t_list;
typedef boost::shared_ptr<t_list> t_listPtr;

typedef struct
{
        double fps[100];
        unsigned int position;
        unsigned int current_size;
}t_fps;

#endif