navigability_map.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 _NAVIGABILITY_MAP_H_
#define _NAVIGABILITY_MAP_H_

#include <stdio.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <boost/shared_ptr.hpp>
#include <iostream> 
#include <Eigen/Core>

#include <algorithm>
#include <stdlib.h>
#include <math.h>

//PCL
#include <pcl_ros/transforms.h>
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/pcd_io.h>
#include <pcl/surface/gp3.h>
#include <pcl/filters/passthrough.h>
//
#include <vector>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <colormap/colormap.h>

//opencv
#include <cv.h>                 // open cv general include file
#include <highgui.h>    // open cv GUI include file
#include <iostream>             // standard C++ I/O

using namespace Eigen;
using namespace std;

class grid_node
{
        public:

                MatrixXd Matrix_Points;
                
                MatrixXd Angles;
                
                bool has_normal;
                
                double med_angle_X;
                
                double med_angle_Y;
                
                double med_angle_Z;
                
                int num_points;
                
                double Zmed;
                
                double Standard_Deviation_Z;
                
                double Z_confidence;
                                
                double angle_confidence_x;
                
                double angle_confidence_y;
                
                double angle_confidence_z;
                
                bool interpolate_z_data;
                
                bool interpolate_normal_data;
                
                std::vector<double> z_difference_neighbours;
                
                std::vector<double> z_confidence_neighbours;
                
                std::vector<double> angleX_difference_neighbours;
                
                std::vector<double> angleY_difference_neighbours;
                
                std::vector<double> angleZ_difference_neighbours;
                
                std::vector<double> angleX_confidence_neighbours;
                
                std::vector<double> angleY_confidence_neighbours;
                
                std::vector<double> angleZ_confidence_neighbours;
                
                std::vector<bool> has_fulldata_direction; //N,NE,E,SE,S,SW,W,NW /only for cell fully data
                
                double z_accessibility;
                
                double angleX_accessibility;
                
                double angleY_accessibility;
                
                double angleZ_accessibility;
                
                double total_accessibility;
                

                grid_node()
                {                       
                        
                        z_accessibility=0;
                        
                        angleX_accessibility=0;
                        angleY_accessibility=0;
                        angleZ_accessibility=0;
                        
                        total_accessibility=0;

                        interpolate_z_data=false;
                        interpolate_normal_data=false;
                        has_normal=false;
                        Z_confidence=0;
        
                        angle_confidence_x=0;
                        angle_confidence_y=0;
                        angle_confidence_z=0;
                        
                        num_points=0;           
                        Zmed=-1000;
                }
                
                ~grid_node()
                {}
                
        private:
        
};

//stores a pointer to a dynamically allocated object
typedef boost::shared_ptr<grid_node> grid_nodePtr;

//stores a pointer to a dynamically allocated object
typedef Matrix<grid_nodePtr, Dynamic, Dynamic> Grid;



class Navigability_Map
{
        public:

                pcl::PointCloud<pcl::Normal>::Ptr normals;
                
                double Xmin_filter;
                
                double Xmax_filter;
                
                double Ymin_filter;
                
                double Ymax_filter;
                
                double Zmax_filter;
                
                double Zmin_filter;
                
                double Radius_neighbors;
                
                int K_neighbors;
                
                int Use_Radius_Search;
                
                int colorrange;

                Grid grid;
                
                double Sx;
                
                double Sy;
                
                int total_row;
                
                int total_col;
                
                double default_confidence;
                
                double fator_confidence_neighbour_limit;
                
                int CARaxis_col;
                
                double Zmax_heigh_difference;
                
                double Standard_Deviation_max;
                
                double angleX_max_difference;
                
                double angleY_max_difference;
                
                double angleZ_max_difference;

                double Standard_Deviation_anglex_max_confidence;
                
                double Standard_Deviation_angley_max_confidence;
                
                double Standard_Deviation_anglez_max_confidence;

                int debug_accessibility;

                
                std::vector<geometry_msgs::Point> polygon_points;
                std::vector<geometry_msgs::Point> obstacle_points;
                std::vector<cv::Point2f> xy_polygon;
                std::vector<cv::Point2f> obstacle_polygon;
                
                MatrixXd Matrix_Cell_inpolygon_rowcol;
                MatrixXd Matrix_obstacle_inpolygon_rowcol;
                
                Navigability_Map()
                {
                        debug_accessibility=1;
                        default_confidence=0.5;
                        fator_confidence_neighbour_limit=1.5;
                        
                        Zmax_heigh_difference=0.1;
                        Standard_Deviation_max=0.2;
                        
                        angleX_max_difference=0.1;
                        angleY_max_difference=0.1;
                        angleZ_max_difference=0.1;
                        Standard_Deviation_anglex_max_confidence=0.2;
                        Standard_Deviation_angley_max_confidence=0.2;
                        Standard_Deviation_anglez_max_confidence=0.2;
                        
                        Radius_neighbors=0.4;
                        K_neighbors=10;
                        Use_Radius_Search=1;
                        
                        Xmin_filter=0;
                        Xmax_filter=22.5;
                        Ymin_filter=-22.5;
                        Ymax_filter=22.5;
                        Zmax_filter=2;
                        Zmin_filter=-15;
                        
                        Sx=0.2;
                        Sy=0.2;
                        colorrange=64;
                }
                
                ~Navigability_Map()
                {}
                
                void Filter_PointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in,pcl::PointCloud<pcl::PointXYZ>::Ptr &pc_filter);
                
                void Normal_Estimation(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
                
                void inicialize_grid(void);
                
                void setGrid_parameter(double max_Xval,double max_Yval);
        
                void setGrid_data(pcl::PointCloud<pcl::PointXYZ>& cloud);
                
                
                void calcGrid_data(void);

                void fill_data_cells(void);
                
                std::vector<grid_nodePtr> cell_neighbours(int row_pos, int col_pos, int &empty_neighbours, int &number_neighbours);
                
                void set_Cells_accessibility(void);
                
                void polygon_groundtruth(void);
                void getCell_inpolygon(void);
                void dataCell_inpolygon(void);
                
                
                
        private:
        
};


#endif