diff --git a/launch/iri_ground_segmentation.launch b/launch/iri_ground_segmentation.launch
index 78836a434ec27e1de3c46fd7aa688cd7eaa7447a..ea82d23278a159723d4ece321d4a10e5b8f6b2bf 100644
--- a/launch/iri_ground_segmentation.launch
+++ b/launch/iri_ground_segmentation.launch
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
<launch>
- <env name="LIBGL_ALWAYS_SOFTWARE" value="1" />
+ <env name="LIBGL_ALWAYS_SOFTWARE" value="1" /> <!-- for running the rviz in virtual box, comment this if you do not use a VM-->
<rosparam command="load" file="$(find iri_ground_segmentation)/params/iri_ground_segmentation.yaml" />
<remap from="/iri_ground_segmentation/lidar_points" to="/kitti/velo/pointcloud"/>
<remap from="/iri_ground_segmentation/ground_truth_lidar_points" to="/velodyne_points"/>
diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index e0bae418e6d37b49ca1be7fae74ff4837a8b9f89..308fba176e9394b7089edfe3f1fac9d2ad1d264e 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/src/ground_segmentation_alg_node.cpp
@@ -138,8 +138,7 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
"GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'dataset_filename_with_global_path' not found");
}
else
- this->alg_.filtering_configuration_.dataset_filename_with_global_path =
- config_.dataset_filename_with_global_path;
+ this->alg_.filtering_configuration_.dataset_filename_with_global_path = config_.dataset_filename_with_global_path;
if (!this->private_node_handle_.getParam("neural_net_filename_with_global_path",
this->config_.neural_net_filename_with_global_path))
@@ -154,12 +153,10 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
if (!this->private_node_handle_.getParam("neural_net_number_of_features",
this->config_.neural_net_number_of_features))
{
- ROS_WARN(
- "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'neural_net_number_of_features' not found");
+ ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'neural_net_number_of_features' not found");
}
else
- this->alg_.filtering_configuration_.neural_net_number_of_features =
- config_.neural_net_number_of_features;
+ this->alg_.filtering_configuration_.neural_net_number_of_features = config_.neural_net_number_of_features;
if (!this->private_node_handle_.getParam("neural_net_number_of_neurons_in_hidden_layer",
this->config_.neural_net_number_of_neurons_in_hidden_layer))
@@ -236,8 +233,8 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
this->lidar_subscriber_ = this->private_node_handle_.subscribe("lidar_points", 1,
&GroundSegmentationAlgNode::cb_lidar, this);
- this->ground_truth_lidar_subscriber_ = this->private_node_handle_.subscribe("ground_truth_lidar_points", 1,
- &GroundSegmentationAlgNode::cb_ground_truth_lidar, this);
+ this->ground_truth_lidar_subscriber_ = this->private_node_handle_.subscribe(
+ "ground_truth_lidar_points", 1, &GroundSegmentationAlgNode::cb_ground_truth_lidar, this);
// [init publishers]
@@ -382,7 +379,7 @@ void GroundSegmentationAlgNode::storeHeaderForSync(void)
void GroundSegmentationAlgNode::convertInputsToSuitableFormats(void)
{
//std::cout << "GroundSegmentationAlgNode::convertInputsToSuitableFormats..." << std::endl;
- if(flag_new_lidar_data_)
+ if (flag_new_lidar_data_)
{
this->rosPointCloudtoPCLPointCloud(lidar_ros_cloud_, xyzi_lidar_pcl_cloud_);
@@ -432,7 +429,7 @@ void GroundSegmentationAlgNode::convertInputsToSuitableFormats(void)
// std::cout << "DATA_C_2_MEAN_INTENSITY = " << lidar_pcl_cloud_.points[100].data_c[DATA_C_2_MEAN_INTENSITY] << std::endl;
// std::cout << "DATA_C_3_ORIGINAL_INDEX = " << lidar_pcl_cloud_.points[100].data_c[DATA_C_3_ORIGINAL_INDEX] << std::endl;
- if(flag_new_ground_truth_lidar_data_)
+ if (flag_new_ground_truth_lidar_data_)
{
this->rosPointCloudtoPCLPointCloud(lidar_ros_cloud_, ground_truth_pcl_cloud_);
@@ -451,13 +448,63 @@ void GroundSegmentationAlgNode::convertInputsToSuitableFormats(void)
extended_point.data_n[DATA_N_2_Z_MEAN] = UNKNOWN_MEAN;
extended_point.data_n[DATA_N_3_Z_VARIANCE] = UNKOWN_VARIANCE;
- // These three values are stored in data_c[0]
- //extended_point.r = R_CLASS_UNKNOWN;
- //extended_point.g = G_CLASS_UNKNOWN;
- //extended_point.b = B_CLASS_UNKNOWN;
- extended_point.rgb = regular_point->rgb;
+ int gt_label = regular_point->label;
+
+ // We are only interester in ROAD, VEGETATION, SIDEWALK, TERRAIN and OBSTACLE classes, so we consolide them
+ // here. TODO: We leave this todo mark here, because maybe in the future we will want to work with more classes
+ if (gt_label == ROAD || gt_label == PARKING || gt_label == OTHER_GROUND || gt_label == LANE_MARKING)
+ gt_label = ROAD;
+
+ if (gt_label != ROAD && gt_label != TERRAIN && gt_label != VEGETATION && gt_label != SIDEWALK)
+ gt_label = CLASS_OBSTACLE;
+
+ // Experimental thresholds to avoid reflected points in the Ego-Vehicle
+ if ((extended_point.x < 3.0 && extended_point.x > -2.2) && (extended_point.y < 1.8 && extended_point.y > -1.8))
+ {
+ // These three values are stored in data_c[0]
+ extended_point.r = R_CLASS_OUTLIER;
+ extended_point.g = G_CLASS_OUTLIER;
+ extended_point.b = B_CLASS_OUTLIER;
+
+ extended_point.data_c[DATA_C_1_ID_CLASS] = (float)OUTLIER;
+ }
+ else
+ {
+ switch (gt_label)
+ {
+ case ROAD:
+ extended_point.r = R_CLASS_ROAD;
+ extended_point.g = G_CLASS_ROAD;
+ extended_point.b = B_CLASS_ROAD;
+ break;
+ case TERRAIN:
+ extended_point.r = R_CLASS_TERRAIN;
+ extended_point.g = G_CLASS_TERRAIN;
+ extended_point.b = B_CLASS_TERRAIN;
+ break;
+
+ case VEGETATION:
+ extended_point.r = R_CLASS_VEGETATION;
+ extended_point.g = G_CLASS_VEGETATION;
+ extended_point.b = B_CLASS_VEGETATION;
+ break;
+
+ case SIDEWALK:
+ extended_point.r = R_CLASS_SIDEWALK;
+ extended_point.g = G_CLASS_SIDEWALK;
+ extended_point.b = B_CLASS_SIDEWALK;
+ break;
+
+ default:
+ extended_point.r = R_CLASS_KITTI_OBSTACLE;
+ extended_point.g = G_CLASS_KITTI_OBSTACLE;
+ extended_point.b = B_CLASS_KITTI_OBSTACLE;
+ }
+
+ extended_point.data_c[DATA_C_1_ID_CLASS] = (float)CLASS_UNKNOWN; // We need this to be set to unknown for the algorithm to work
+ // We use the color information to get the ground truth
+ }
- extended_point.data_c[DATA_C_1_ID_CLASS] = (float)regular_point->label;
extended_point.data_c[DATA_C_3_ORIGINAL_INDEX] = (float)i++;
lidar_pcl_cloud_.points.push_back(extended_point);
@@ -535,7 +582,6 @@ void GroundSegmentationAlgNode::publishAll(void)
// Visualization topics
this->ground_dense_reconstruction_publisher_.publish(this->ground_dense_reconstruction_ros_cloud_);
-
visualization_msgs::Marker markerD;
markerD.header = common_header_;
markerD.action = visualization_msgs::Marker::DELETEALL;
@@ -723,11 +769,11 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
std::cout << "mahalanobis_threshold = "
<< this->alg_.filtering_configuration_.mahalanobis_threshold << std::endl;
- std::cout << "score_threshold = "
- << this->alg_.filtering_configuration_.score_threshold << std::endl;
+ std::cout << "score_threshold = " << this->alg_.filtering_configuration_.score_threshold
+ << std::endl;
- std::cout << "use_neural_network = "
- << this->alg_.filtering_configuration_.use_neural_network << std::endl;
+ std::cout << "use_neural_network = " << this->alg_.filtering_configuration_.use_neural_network
+ << std::endl;
std::cout << "extract_data_to_external_training_of_the_network = "
<< this->alg_.filtering_configuration_.extract_data_to_external_training_of_the_network << std::endl;