diff --git a/cfg/GroundSegmentation.cfg b/cfg/GroundSegmentation.cfg
index a65ac071587891c1cfb2fbe74ebb8098f88c4735..4673e08c21ec6dd3c524f235eeb0a30a288ebdb0 100644
--- a/cfg/GroundSegmentation.cfg
+++ b/cfg/GroundSegmentation.cfg
@@ -85,6 +85,7 @@ gen.add("sensor_best_resolution_deg", double_t, 0, "Either vertical or horizonta
 gen.add("max_navigation_slope", double_t, 0, "slopes greater than this threshold will be considered obstacles", 0.2, 0.01, 1.0);
 
 # visualization and debug parameters
+gen.add("evaluate_segmentation", bool_t, 0, "generates quantitative evaluation metrics", False);
 gen.add("measure_performance", bool_t, 0, "to measure times of inner functions", False);
 gen.add("show_dense_reconstruction", bool_t, 0, "creates dense pointcloud with ground predictions", False);
  
diff --git a/include/ground_segmentation_alg_node.h b/include/ground_segmentation_alg_node.h
index b18dd8cdf96953152bb123aabe29c9aac280185e..2bb4ea4ceb9b2fe1c62686fcdf931f63aef1e1c8 100644
--- a/include/ground_segmentation_alg_node.h
+++ b/include/ground_segmentation_alg_node.h
@@ -69,6 +69,7 @@ private:
   bool flag_new_lidar_data_;
   bool flag_new_ground_truth_lidar_data_;
   bool flag_use_low_obstacle_detection_;
+  bool flag_evaluate_segmentation_;
 
   ros::Publisher lidar_points_ground_segmented_publisher_;
   sensor_msgs::PointCloud2 lidar_points_ground_segmented_ros_cloud_;
@@ -96,6 +97,10 @@ private:
   sensor_msgs::PointCloud2 ground_points_ros_cloud_;
   pcl::PointCloud<pcl::PointXYZ> ground_points_pcl_cloud_;
 
+  ros::Publisher evaluated_segmentation_pointcloud_publisher_;
+  sensor_msgs::PointCloud2 evaluated_segmentation_pointcloud_ros_cloud_;
+  pcl::PointCloud<PointXYZIRGBL> evaluated_segmentation_pointcloud_pcl_cloud_;
+
   // [subscriber attributes]
   // Variables for copying the callbacks inputs
   sensor_msgs::PointCloud2 lidar_ros_cloud_;
@@ -153,6 +158,8 @@ private:
 
   void detectLowObstacles(float h, float res, float alpha);
 
+  void evaluateSegmentation(void);
+
   // [service attributes]
 
   // [client attributes]
diff --git a/params/iri_ground_segmentation.yaml b/params/iri_ground_segmentation.yaml
index b74d51e44f588bbf6e57aab5c877d109e2dbb162..4eba5ee5d1dec71707e812b1ef06af6c89ce73b3 100644
--- a/params/iri_ground_segmentation.yaml
+++ b/params/iri_ground_segmentation.yaml
@@ -58,6 +58,7 @@ iri_ground_segmentation: {
   max_navigation_slope: 0.5,                           
                                                          
   # visualization and debug parameters
+  evaluate_segmentation: true,
   measure_performance: false,            ## (feature still not debugged) Flag to measure number of execution and execution times of the different functions of the algorithm 
   show_dense_reconstruction: false,      ## To show a dense ground surface reconstruction using the predictions of the ground mode (colored using the std_dev of z coordinate)   
                                          ## or alternatively the "elevation point cloud" (useful for parameter tunning) 
diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index 514aaa0366a8fbe643305197a647e376880995f2..c12978e2badb6e6b55edb2f65b4d22af06c4f714 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/src/ground_segmentation_alg_node.cpp
@@ -257,6 +257,13 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
   else
     this->alg_.filtering_configuration_.max_navigation_slope = config_.max_navigation_slope;
 
+  if (!this->private_node_handle_.getParam("evaluate_segmentation", this->config_.evaluate_segmentation))
+  {
+    ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'evaluate_segmentation' not found");
+  }
+  else
+    this->flag_evaluate_segmentation_ = config_.evaluate_segmentation;
+
   // [init subscribers]
   this->lidar_subscriber_ = this->private_node_handle_.subscribe("lidar_points", 1,
                                                                  &GroundSegmentationAlgNode::cb_lidar, this);
@@ -277,6 +284,9 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
   this->ground_points_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
       > ("ground_points", 1);
 
+  this->evaluated_segmentation_pointcloud_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
+      > ("evaluated_segmentation_point_cloud", 1);
+
   //////// Roadmap outputs //////////////////////////
   this->roadmap_edges_publisher_ = this->private_node_handle_.advertise < iri_ground_segmentation::edges_array
       > ("roadmap_edges", 1);
@@ -742,6 +752,11 @@ void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float
   }
 }
 
+void GroundSegmentationAlgNode::evaluateSegmentation(void)
+{
+  return;
+}
+
 void GroundSegmentationAlgNode::mainNodeThread(void)
 {
   if (this->isAllRequiredDataAlreadyReceived())
@@ -765,6 +780,11 @@ void GroundSegmentationAlgNode::mainNodeThread(void)
                                this->alg_.filtering_configuration_.max_navigation_slope);
     }
 
+    if (this->flag_evaluate_segmentation_)
+    {
+      this->evaluateSegmentation();
+    }
+
     //std::cout << "Ground segmentation finished!" << std::endl;
     this->alg_.unlock();
     this->convertOutputsToSuitableFormats();