diff --git a/include/ground_segmentation_alg_node.h b/include/ground_segmentation_alg_node.h
index 6772d8bab401521f46e9b984a66e34f73d2e1cba..e87cf4fcc5b2911a1200a15dd6d76078dc0a38e2 100644
--- a/include/ground_segmentation_alg_node.h
+++ b/include/ground_segmentation_alg_node.h
@@ -47,6 +47,7 @@ class GroundSegmentationAlgNode : public algorithm_base::IriBaseAlgorithm<Ground
private:
std_msgs::Header common_header_;
bool flag_new_lidar_data_;
+ bool flag_new_ground_truth_lidar_data_;
ros::Publisher lidar_points_ground_segmented_publisher_;
sensor_msgs::PointCloud2 lidar_points_ground_segmented_ros_cloud_;
@@ -78,9 +79,13 @@ private:
sensor_msgs::PointCloud2 lidar_ros_cloud_;
pcl::PointCloud<pcl::PointXYZRGBNormal> lidar_pcl_cloud_;
+ pcl::PointCloud<pcl::PointXYZI> xyzi_lidar_pcl_cloud_;
ros::Subscriber lidar_subscriber_;
void cb_lidar(const sensor_msgs::PointCloud2::ConstPtr &msg);
+ ros::Subscriber ground_truth_lidar_subscriber_;
+ void cb_ground_truth_lidar(const sensor_msgs::PointCloud2::ConstPtr &msg);
+
pthread_mutex_t mutex_;
void mutex_enter(void);
void mutex_exit(void);
@@ -88,6 +93,9 @@ private:
void rosPointCloudtoPCLPointCloud(const sensor_msgs::PointCloud2 &ros_point_cloud,
pcl::PointCloud<pcl::PointXYZRGBNormal> &pcl_point_cloud);
+ void rosPointCloudtoPCLPointCloud(const sensor_msgs::PointCloud2 &ros_point_cloud,
+ pcl::PointCloud<pcl::PointXYZI> &pcl_point_cloud);
+
void PCLPointCloudToRosPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> &pcl_point_cloud,
sensor_msgs::PointCloud2 &ros_point_cloud);
diff --git a/params/iri_ground_segmentation.yaml b/params/iri_ground_segmentation.yaml
index 6403f9c29f91882dc7e2fc7c90ea3ed200d80927..92bdd851f4786d0218553c4a630b5ba9d90ee811 100644
--- a/params/iri_ground_segmentation.yaml
+++ b/params/iri_ground_segmentation.yaml
@@ -36,7 +36,7 @@ iri_ground_segmentation: {
## and a big value will increase the number of false ground points)
# Neural Network related parameters
- use_neural_network: true,
+ use_neural_network: false,
extract_data_to_external_training_of_the_network: false,
dataset_filename_with_global_path: '/home/idelpino/Documentos/dataset.csv', # This is the filename with path where the data for neural net training is dumped
neural_net_filename_with_global_path: '/home/idelpino/iri-lab/ground_segmentation_ws/src/iri_ground_segmentation/shallow_neural_nets/veg_terrain_and_obs_13_features_39_neurons.csv',
diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index 4d5be95dcdc86a00d7e504350e828526b6f3b937..ec81953a9bbd0f5619ece93fa11352064c515d38 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/src/ground_segmentation_alg_node.cpp
@@ -233,9 +233,12 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
this->alg_.filtering_configuration_.show_dense_reconstruction = config_.show_dense_reconstruction;
// [init subscribers]
- this->lidar_subscriber_ = this->private_node_handle_.subscribe("lidar_points_extended_structure", 1,
+ 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);
+
// [init publishers]
//////// Segmentation outputs /////////////////////
@@ -264,6 +267,7 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
> ("ground_dense_reconstruction", 1);
flag_new_lidar_data_ = false;
+ flag_new_ground_truth_lidar_data_ = false;
pthread_mutex_init(&this->mutex_, NULL);
// [init services]
@@ -284,6 +288,14 @@ void GroundSegmentationAlgNode::rosPointCloudtoPCLPointCloud(const sensor_msgs::
pcl::fromPCLPointCloud2(aux, pcl_point_cloud);
}
+void GroundSegmentationAlgNode::rosPointCloudtoPCLPointCloud(const sensor_msgs::PointCloud2 &ros_point_cloud,
+ pcl::PointCloud<pcl::PointXYZI> &pcl_point_cloud)
+{
+ pcl::PCLPointCloud2 aux;
+ pcl_conversions::toPCL(ros_point_cloud, aux);
+ pcl::fromPCLPointCloud2(aux, pcl_point_cloud);
+}
+
void GroundSegmentationAlgNode::PCLPointCloudToRosPointCloud(
const pcl::PointCloud<pcl::PointXYZRGBNormal> &pcl_point_cloud, sensor_msgs::PointCloud2 &ros_point_cloud)
{
@@ -344,7 +356,7 @@ bool GroundSegmentationAlgNode::isAllRequiredDataAlreadyReceived(void)
{
bool ready_to_process = false;
this->mutex_enter();
- if (flag_new_lidar_data_)
+ if (flag_new_lidar_data_ || flag_new_ground_truth_lidar_data_)
{
ready_to_process = true;
//std::cout << "GroundSegmentationAlgNode::isAllRequiredDataAlreadyReceived..." << std::endl;
@@ -362,7 +374,38 @@ void GroundSegmentationAlgNode::storeHeaderForSync(void)
void GroundSegmentationAlgNode::convertInputsToSuitableFormats(void)
{
//std::cout << "GroundSegmentationAlgNode::convertInputsToSuitableFormats..." << std::endl;
- this->rosPointCloudtoPCLPointCloud(lidar_ros_cloud_, lidar_pcl_cloud_);
+ if(flag_new_lidar_data_)
+ {
+ this->rosPointCloudtoPCLPointCloud(lidar_ros_cloud_, xyzi_lidar_pcl_cloud_);
+
+ int i = 0;
+ for (pcl::PointCloud<pcl::PointXYZI>::iterator regular_point = xyzi_lidar_pcl_cloud_.begin();
+ regular_point != xyzi_lidar_pcl_cloud_.end(); ++regular_point)
+ {
+ pcl::PointXYZRGBNormal extended_point;
+ extended_point.x = regular_point->x;
+ extended_point.y = regular_point->y;
+ extended_point.z = regular_point->z;
+
+ extended_point.data_n[DATA_N_0_INTENSITY] = regular_point->intensity;
+ extended_point.data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL] = UNKNOWN_MEAN;
+ extended_point.data_n[DATA_N_1_INTENSITY_VARIANCE] = UNKOWN_VARIANCE;
+ 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.data_c[DATA_C_1_ID_CLASS] = (float)CLASS_UNKNOWN;
+ extended_point.data_c[DATA_C_3_ORIGINAL_INDEX] = (float)i++;
+
+ lidar_pcl_cloud_.points.push_back(extended_point);
+ }
+
+ flag_new_lidar_data_ = false;
+ }
// std::cout << "x = " << lidar_pcl_cloud_.points[100].x << std::endl;
// std::cout << "y = " << lidar_pcl_cloud_.points[100].y << std::endl;
// std::cout << "z = " << lidar_pcl_cloud_.points[100].z << std::endl;
@@ -381,7 +424,12 @@ 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;
- flag_new_lidar_data_ = false;
+ if(flag_new_ground_truth_lidar_data_)
+ {
+ this->rosPointCloudtoPCLPointCloud(lidar_ros_cloud_, lidar_pcl_cloud_);
+ flag_new_ground_truth_lidar_data_ = false;
+ }
+
}
void GroundSegmentationAlgNode::splitSegmentedCloudInOnePerClass(void)
@@ -507,7 +555,6 @@ void GroundSegmentationAlgNode::mainNodeThread(void)
ground_dense_reconstruction_pcl_cloud_, roadmap_MarkerArray_msg_);
//std::cout << "Ground segmentation finished!" << std::endl;
this->alg_.unlock();
-
this->convertOutputsToSuitableFormats();
this->publishAll();
this->prepareForNextIteration();
@@ -528,6 +575,18 @@ void GroundSegmentationAlgNode::cb_lidar(const sensor_msgs::PointCloud2::ConstPt
this->mutex_exit();
}
+void GroundSegmentationAlgNode::cb_ground_truth_lidar(const sensor_msgs::PointCloud2::ConstPtr &msg)
+{
+ this->mutex_enter();
+ lidar_ros_cloud_ = *msg;
+ //std::cout << "GroundSegmentationAlgNode::cb_lidar --> lidar msg received!" << std::endl;
+ assert(msg != NULL && "Null pointer!!! in function cb_lidar!");
+
+ flag_new_ground_truth_lidar_data_ = true;
+
+ this->mutex_exit();
+}
+
void GroundSegmentationAlgNode::mutex_enter(void)
{
pthread_mutex_lock(&this->mutex_);