dynamic_obstacle_tracking.hpp

Go to the documentation of this file.

//#ifndef __DYNAMIC_OBSTACLE_TRACKING_HPP__
//#define __DYNAMIC_OBSTACLE_TRACKING_HPP__

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>

#include <dynamic_reconfigure/server.h>


// PCL specific includes
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <math.h>
#include <tf/LinearMath/Matrix3x3.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include "tf_conversions/tf_eigen.h"
#include <pcl/filters/passthrough.h>
#include <pcl/registration/icp.h>
//#include <pcl/filters/voxel_grid.h>
//#
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/octree/octree_impl.h>
#include <pcl/octree/octree.h>
#include <pcl/octree/octree_key.h>
#include <pcl/octree/octree_pointcloud_adjacency.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <ctime>




#define PI 3.14159265
#define SQR(a) ((a)*(a))

using namespace Eigen;
using namespace std;






namespace datmo
{

struct Float3{
   float x;
   float y;
   float z;
};


    class cloud_segmentation{

    public:
        cloud_segmentation();
        ~cloud_segmentation();
        void init(ros::NodeHandle &nh, ros::NodeHandle &private_nh);

    protected:
        ros::Publisher pub; 
        ros::Publisher pub_2; 
        ros::Publisher pub_3; 
        //ros::Publisher marker_pub;
        ros::Publisher centroids_pub; 

        ros::Subscriber sub;  

        //visualization_msgs::MarkerArray marker_array;
        //nav_msgs::Odometry odom;


        tf::TransformListener listener; 
        tf::TransformBroadcaster br; 

        tf::StampedTransform current_cloud_transform; 
        tf::StampedTransform ref_cloud_transform; 
        tf::StampedTransform prev_cloud_transform; 



        pcl::PointCloud<pcl::PointXYZ>:: Ptr cloud_transformed; 
        pcl::PointCloud<pcl::PointXYZ>::Ptr obj_cloud; 
        pcl::PointCloud<pcl::PointXYZ>:: Ptr ref_obj_cloud; 
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr occuluded_cloud; 
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr new_discovered_cloud; 
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_dynamic_cloud; 
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr classified_cloud; 






        vector < pcl::PointCloud<pcl::PointXYZ>::Ptr, Eigen::aligned_allocator <pcl::PointCloud<pcl::PointXYZ>::Ptr > > sourceClouds; 
        vector<tf::StampedTransform> sourceTransforms; 
        //pcl::PointCloud<pcl::PointXYZ> last_centroids;
        Eigen::Affine3d odom_transform_matrix; 
        Eigen::Affine3f icp_transform_matrix;



        int counter;
        string frame_id;
        string base_frame_id;
        string sensor_frame_id;
        //pcl::PointCloud<pcl::PointXYZ>:: Ptr cloud_filter;
        bool ENABLE_ICP; 
        bool ENABLE_GROUND_REMOVAL; 
        bool ENABLE_VOXELISE; 
        bool ENABLE_OCCLUSION_DETECTION; 

        float VOXEL_LEAF_SIZE;
        //Statistical outlier parameters
        bool ENABLE_SOR; 
        float SOR_MEAN_K;              
        float SOR_STD_DEV_MUL_THRESH; 
        //Passthrough filter parameters

        float PASS_X_MIN;  
        float PASS_X_MAX;  
        float PASS_Y_MIN;  
        float PASS_Y_MAX;  
        float PASS_Z_MIN;  
        float PASS_Z_MAX;  


        int OCTREE_WINDOW; 
        float OCTREE_RESOLUTION; 

        float SEG_CLUSTER_TOLERANCE; 
        int SEG_MIN_CLUSTER_SIZE; 
        int SEG_MAX_CLUSTER_SIZE; 





    private:
        void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg);


        void cloud_transform (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud_in, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_out);

        void cloud_filter (const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in, const string field, float min, float max);

        void Voxel_filter ( const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in,
                            const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_out,
                            float VOXEL_LEAF_SIZE);
        void SOR_filter (const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in,
                         const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_out,
                         float SOR_MEAN_K,
                         float SOR_STD_DEV_MUL_THRESH );


        void Ransac_plane (const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_out);
        //void euclideancluster (const pcl::PointCloud<pcl::PointXYZ>::Ptr& obj_cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cluster_cloud, std::vector<Float3> centroids);

        void euclideancluster  (const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                                float SEG_CLUSTER_TOLERANCE,
                                int SEG_MIN_CLUSTER_SIZE,
                                int SEG_MAX_CLUSTER_SIZE);

        void detect_spatial_change (const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_now,
                                    const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_to_compare,
                                    const pcl::PointCloud<pcl::PointXYZ>::Ptr& dynamic_cloud,
                                    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& occuluded_cloud,
                                    float OCTREE_RESOLUTION,
                                    bool ENABLE_OCCLUSION_DETECTION);
        bool is_in_bounding_area(const pcl::PointCloud<pcl::PointXYZ>::Ptr& coord, pcl::PointXYZRGB pt );


        //void mark_cluster(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster, std::string ns ,int id, float r, float g, float b);
        void integrateOdom_ICP (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& obj_cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr& ref_obj_cloud );
        //void integrateOdom ( pcl::PointCloud<pcl::PointXYZ> &last_centroids);


        //void integrateOdom (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud);
        //void integrateOdom (std::vector<Float3>& last_centroids);
        //void dynamic_reconfigure_cb(datmo::datmoConfig &config, uint32_t level);
        //void odom_cb (const nav_msgs::Odometry odom_msg);
    };

}
//#endif // __DYNAMIC_OBSTACLE_TRACKING_HPP__