diff --git a/src/ground_segmentation_alg.cpp b/src/ground_segmentation_alg.cpp
index 8f56c3200874cc541014b84780f8cf9e3a2c3a43..e8edffc24ac36e95786ef03d121d00b3cd58befc 100644
--- a/src/ground_segmentation_alg.cpp
+++ b/src/ground_segmentation_alg.cpp
@@ -48,8 +48,10 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
//std::cout << "Number of edge vectors = " << edges.size();
int vertex_id = 0;
+ int discarded_vertex_id = 0;
int edge_id = 0;
int discarded_edge_id = 0;
+
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = ground_references_pcl_cloud.begin();
i != ground_references_pcl_cloud.end(); ++i)
{
@@ -69,8 +71,18 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
visualization_msgs::Marker vertex;
vertex.action = visualization_msgs::Marker::ADD;
vertex.header = common_header;
- vertex.ns = "vertices";
- vertex.id = vertex_id++;
+
+ if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] < VERTEX_NOT_CONNECTED_TO_ROOT)
+ {
+ vertex.ns = "vertices_connected_to_root";
+ vertex.id = vertex_id++;
+ }
+ else
+ {
+ vertex.ns = "vertices_NOT_connected_to_root";
+ vertex.id = discarded_vertex_id++;
+ }
+
vertex.type = visualization_msgs::Marker::CUBE;
vertex.pose.position.x = ground_ref.x;
@@ -87,10 +99,18 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
vertex.scale.z = 0.30;
vertex.color.a = 1.0; // Don't forget to set the alpha!
- vertex.color.r = 1.0;
- vertex.color.g = 0.66;
- vertex.color.b = 0.0;
-
+ if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] < VERTEX_NOT_CONNECTED_TO_ROOT)
+ {
+ vertex.color.r = 1.0;
+ vertex.color.g = 0.66;
+ vertex.color.b = 0.0;
+ }
+ else
+ {
+ vertex.color.r = 1.0;
+ vertex.color.g = 0.0;
+ vertex.color.b = 0.0;
+ }
vertex.lifetime = ros::Duration(0.1);
//vertex.frame_locked = true;
@@ -105,8 +125,18 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
visualization_msgs::Marker edge;
edge.action = visualization_msgs::Marker::ADD;
edge.header = common_header;
- edge.ns = "edges";
- edge.id = edge_id++;
+
+ if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] < VERTEX_NOT_CONNECTED_TO_ROOT)
+ {
+ edge.ns = "edges";
+ edge.id = edge_id++;
+ }
+ else
+ {
+ edge.ns = "discarded_edges";
+ edge.id = discarded_edge_id++;
+ }
+
edge.type = visualization_msgs::Marker::LINE_LIST;
geometry_msgs::Point p;
@@ -130,10 +160,18 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
edge.pose.orientation.w = 1.0;
edge.color.a = 0.7; // Don't forget to set the alpha!
- edge.color.r = 0.8;
- edge.color.g = 0.53;
- edge.color.b = 0.0;
-
+ if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] < VERTEX_NOT_CONNECTED_TO_ROOT)
+ {
+ edge.color.r = 0.8;
+ edge.color.g = 0.53;
+ edge.color.b = 0.0;
+ }
+ else
+ {
+ edge.color.r = R_CLASS_VIRTUAL_OBSTACLE_NOT_CONNECTED_TO_ROOT / 255.0;
+ edge.color.g = G_CLASS_VIRTUAL_OBSTACLE_NOT_CONNECTED_TO_ROOT / 255.0;
+ edge.color.b = B_CLASS_VIRTUAL_OBSTACLE_NOT_CONNECTED_TO_ROOT / 255.0;
+ }
edge.lifetime = ros::Duration(0.1);
//edge.frame_locked = true;
@@ -178,25 +216,29 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
switch (discarded_edge_index_and_class.edge_class)
{
case CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_UNCERTAINTY:
- std::cout << "Coloring edge too much uncertainty! class = " << discarded_edge_index_and_class.edge_class << std::endl;
- discarded_edge.color.r = (float) R_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_UNCERTAINTY / 255.0;
- discarded_edge.color.g = (float) G_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_UNCERTAINTY / 255.0;
- discarded_edge.color.b = (float) B_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_UNCERTAINTY / 255.0;
+ //std::cout << "Coloring edge too much uncertainty! class = " << discarded_edge_index_and_class.edge_class
+ // << std::endl;
+ discarded_edge.color.r = (float)R_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_UNCERTAINTY / 255.0;
+ discarded_edge.color.g = (float)G_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_UNCERTAINTY / 255.0;
+ discarded_edge.color.b = (float)B_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_UNCERTAINTY / 255.0;
break;
case CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_MAHALANOBIS_DISTANCE:
- std::cout << "Coloring edge too much Mahalanobis distance! class = " << discarded_edge_index_and_class.edge_class << std::endl;
- discarded_edge.color.r = (float) R_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
- discarded_edge.color.g = (float) G_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
- discarded_edge.color.b = (float) B_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
+ //std::cout << "Coloring edge too much Mahalanobis distance! class = "
+ // << discarded_edge_index_and_class.edge_class << std::endl;
+ discarded_edge.color.r = (float)R_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
+ discarded_edge.color.g = (float)G_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
+ discarded_edge.color.b = (float)B_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
break;
case CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_SLOPE:
- std::cout << "Coloring edge too much slope! class = " << discarded_edge_index_and_class.edge_class << std::endl;
- discarded_edge.color.r = (float) R_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_SLOPE / 255.0;
- discarded_edge.color.g = (float) G_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_SLOPE / 255.0;
- discarded_edge.color.b = (float) B_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_SLOPE / 255.0;
+ //std::cout << "Coloring edge too much slope! class = " << discarded_edge_index_and_class.edge_class
+ // << std::endl;
+ discarded_edge.color.r = (float)R_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_SLOPE / 255.0;
+ discarded_edge.color.g = (float)G_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_SLOPE / 255.0;
+ discarded_edge.color.b = (float)B_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_SLOPE / 255.0;
break;
default:
- std::cout << "WARNING! Discarded edge class not known!! class = " << discarded_edge_index_and_class.edge_class << std::endl;
+ //std::cout << "WARNING! Discarded edge class not known!! class = " << discarded_edge_index_and_class.edge_class
+ // << std::endl;
discarded_edge.color.r = 1.0;
discarded_edge.color.g = 1.0;
discarded_edge.color.b = 1.0;