33 #ifndef _COIMBRA_LIDAR_SEGMENTATION_H_
34 #define _COIMBRA_LIDAR_SEGMENTATION_H_
37 #include "std_msgs/String.h"
38 #include <rosbag/bag.h>
39 #include <visualization_msgs/Marker.h>
40 #include <visualization_msgs/MarkerArray.h>
41 #include <tf/transform_broadcaster.h>
43 #include "sensor_msgs/LaserScan.h"
44 #include "geometry_msgs/Point.h"
48 #include <colormap/colormap.h>
53 #include <ros/package.h>
58 # define SIMPLE_SEG 1;
63 # define SANTOS_C_SEG 6;
64 # define SANTOS_B_SEG 7;
122 int convertPointsToCluster(
const sensor_msgs::LaserScan::ConstPtr& scan_msg , vector<PointPtr>& points, vector<ClusterPtr>& clusters_GT);
143 void filterPoints(vector<PointPtr>& points_in , vector<PointPtr>& points_out,
double min_range,
double max_range);
153 void removeInvalidPoints(vector<PointPtr>& laser_points, vector<ClusterPtr> clusters, uint min_points );
174 int writeResults_GT(uint iteration , vector<PointPtr>& points ,
int id_result);
195 int writeResults_paths(vector<PointPtr>& points,
int algorithm_id,
double initial_value,
double increment,
int number_of_iterations , uint iteration,
int id_result);
bool correctClusterId(PointPtr p, int cluster_id)
Compares the point id to a cluster_id, if true that point is erased.
int createPointsFromFile(vector< PointPtr > &points, C_DataFromFilePtr data_gt)
Creates a Point class from the file with the Ground-truth points.
boost::shared_ptr< Point > PointPtr
Clustering related functions header.
void removeInvalidPoints(vector< PointPtr > &laser_points, vector< ClusterPtr > clusters, uint min_points)
Removes points belonging to clusters with less than min_points.
bool comparePoints(PointPtr p1, PointPtr p2)
Comparison function.
void filterPoints(vector< PointPtr > &points_in, vector< PointPtr > &points_out, double min_range, double max_range)
Function that outputs a set of points with range between min_range and max_range values.
int writeResults_paths(vector< PointPtr > &points, int algorithm_id, double initial_value, double increment, int number_of_iterations, uint iteration, int id_result)
Wirtes properties of the Algorithms segments into a text file.
boost::shared_ptr< C_DataFromFile > C_DataFromFilePtr
int convertPointsToCluster(const sensor_msgs::LaserScan::ConstPtr &scan_msg, vector< PointPtr > &points, vector< ClusterPtr > &clusters_GT)
Convert the all Point class from a scan to a Cluster class.
int writeResults_GT(uint iteration, vector< PointPtr > &points, int id_result)
Wirtes properties of the Ground-truth segments into a text file.
Groundtruth related functions header.
boost::shared_ptr< ::sensor_msgs::LaserScan > LaserScanPtr
boost::shared_ptr< Point > PointPtr