diff --git a/CMakeLists.txt b/CMakeLists.txt
index 24cc12db0fe8cc6fd480995877afc40ecf7b4118..6951f66f49ee02b409b026dc2b78ed3858616b58 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -29,7 +29,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm message_generation)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm message_generation cv_bridge)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
diff --git a/params/iri_ground_segmentation.yaml b/params/iri_ground_segmentation.yaml
index f0e1eb8d5ae3563c685aee4da4991a451d06ef13..e8e551363078dac1077a08709a61528be33f5660 100644
--- a/params/iri_ground_segmentation.yaml
+++ b/params/iri_ground_segmentation.yaml
@@ -7,12 +7,12 @@ iri_ground_segmentation: {
# Parameters affecting the exploration of the pointcloud
ROI_delta_x_and_y: 3.0, ## This value sets the size of the ROIs: ROI_size = (2*ROI_delta_x_and_y)^2
- ROI_shadow_area: 5.5, #6.0, #5.5, ## This value is the same that the above, but only used in the root vertex to overcome the shadow area
+ ROI_shadow_area: 7.0, #5.5, #6.0, ## This value is the same that the above, but only used in the root vertex to overcome the shadow area
- ground_reference_search_resolution_deg: 40.0, #20.0, #40 #12.00, ## It is the angular resolution when generating new ground references (graph nodes),
+ ground_reference_search_resolution_deg: 40.0, #6.0, #40.0, #20.0, #40 #12.00, ## It is the angular resolution when generating new ground references (graph nodes),
## it will affect the number of nodes in the graph: lower values generate more nodes
- elevation_grid_resolution: 2.1, #1.5, #2.1, #1.0 #0.5, ## This value is used to create the "elevation point cloud" which is a reduction of the original pointcloud, where
+ elevation_grid_resolution: 2.1, #1.2, #1.5, #1.0 #0.5, ## This value is used to create the "elevation point cloud" which is a reduction of the original pointcloud, where
## only the lowest z values in each cell are stored (along with a vector of indexes pointing to the remaining points
## in the cell, so that the original pointcloud can be recovered or labeled using the info in the "elevation cloud").
## Big values can speed up the algorithm but generates a lower resolution ground model, on the other hand, small values
@@ -39,7 +39,7 @@ iri_ground_segmentation: {
use_neural_network: false,
extract_data_to_external_training_of_the_network: false,
dataset_filename_with_global_path: '/home/idelpino/Documentos/dataset_rgb_hsv_olbp_10_frame_inc.csv',
- neural_net_filename_with_global_path: '/media/sf_virtual_box_shared/neural_networks/veg_terrain_and_obs_13_features_39_neurons.csv',
+ neural_net_filename_with_global_path: '/media/sf_virtual_box_shared/neural_networks/veg_terrain_and_obs_13_features_39_neurons.csv', #five_classes_13_features_39_neurons.csv'
neural_net_number_of_features: 13,
neural_net_number_of_neurons_in_hidden_layer: 39,
neural_net_number_of_neurons_in_output_layer: 2,
@@ -47,7 +47,7 @@ iri_ground_segmentation: {
# labeling parameters
max_pred_std_dev_for_labelling: 0.5, ## ONLY IN USE TO GIVE COLOUR TO DENSE RECONSTRUCTION
score_threshold: 0.0, ## for assigning ground class label: one means maha. dist. equal to zero, zero means mahalanobis dist equal to maha. thres
- classify_not_labeled_points_as_obstacles: true, ## when a point has no reference satisfying the max_pred_std_dev_for_labelling threshold we can leave as unknown or label it as obstacle
+ classify_not_labeled_points_as_obstacles: false, ## when a point has no reference satisfying the max_pred_std_dev_for_labelling threshold we can leave as unknown or label it as obstacle
ground_threshold_in_not_analyzed_areas: 0.1, ## when it is not possible to make a local analysis of the data, we will use the lowest point (the one in elevation_cloud) as ground height, and
## all the points above it and below this threshold will be classified as ground
diff --git a/src/ground_segmentation_alg.cpp b/src/ground_segmentation_alg.cpp
index 3f8b11bc89276afeb7bcaab36fdf2a49a57f6167..c53ef6ae70988cfc9039fb1bcae0efe012f865f2 100644
--- a/src/ground_segmentation_alg.cpp
+++ b/src/ground_segmentation_alg.cpp
@@ -1,4 +1,6 @@
#include "ground_segmentation_alg.h"
+#include "tgs.hpp"
+#include "aos.hpp"
GroundSegmentationAlgorithm::GroundSegmentationAlgorithm(void)
{
@@ -40,210 +42,303 @@ void GroundSegmentationAlgorithm::segmentGround(
std::vector<std::vector<int>> edges(100000);
std::vector < std::vector < kf_based_terrain_analysis_lib::Edge >> discarded_edges(100000);
- //std::cout << "Calling to processor_.groundSegmentation!" << std::endl;
-
-// std::string filename = "/home/idelpino/Documentos/dataset.txt";
-// std::cout << "Creating file " << filename.c_str() << std::endl;
-//
-// dataset_file_.open(filename.c_str(), std::ios_base::trunc);
-
- processor_.groundSegmentation(lidar_configuration_, filtering_configuration_, lidar_pcl_cloud,
- roadmap_vertices_pcl_cloud, edges, discarded_edges,
- ground_dense_reconstruction_pcl_cloud);
+ bool use_travel = false;
+ if (use_travel)
+ {
+ // Using TRAVEL for comparison!
+ pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in;
+ pcl::PointCloud<pcl::PointXYZI>::Ptr ground_pc;
+ pcl::PointCloud<pcl::PointXYZI>::Ptr nonground_pc;
+
+ cloud_in.reset(new pcl::PointCloud<pcl::PointXYZI>());
+ ground_pc.reset(new pcl::PointCloud<pcl::PointXYZI>());
+ nonground_pc.reset(new pcl::PointCloud<pcl::PointXYZI>());
+
+ boost::shared_ptr < travel::TravelGroundSeg < pcl::PointXYZI >> travel_ground_seg;
+
+ travel_ground_seg.reset(new travel::TravelGroundSeg<pcl::PointXYZI>());
+
+ double min_range = 1.0;
+ double max_range = 64.0;
+
+ double resolution = 8.0;
+ int num_iter = 3;
+ int num_lpr = 5;
+ int num_min_pts = 10;
+ double th_seeds = 0.5;
+ double th_dist = 0.125;
+ double th_outlier = 0.3;
+ double th_normal = 0.940;
+ double th_weight = 200;
+ double th_lcc_normal = 0.03;
+ double th_lcc_planar = 0.1;
+ double th_obstacle = 1.0;
+ bool refine_mode = true;
+ bool visualization = true;
+
+ std::cout << "Setting TRAVEL parameters!" << std::endl;
+ travel_ground_seg->setParams(max_range, min_range, resolution, num_iter, num_lpr, num_min_pts, th_seeds, th_dist,
+ th_outlier, th_normal, th_weight, th_lcc_normal, th_lcc_planar, th_obstacle,
+ refine_mode, visualization);
+ std::cout << "Parameters set!" << std::endl;
+
+ std::cout << "Copying input data to XYZI format!" << std::endl;
+ for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = lidar_pcl_cloud.begin(); i != lidar_pcl_cloud.end(); ++i)
+ {
+ pcl::PointXYZI point;
+ point.x = i->x;
+ point.y = i->y;
+ point.z = i->z;
+ point.intensity = 0;
+ cloud_in->points.push_back(point);
+ }
+ std::cout << "Data copied!" << std::endl;
- //std::cout << "Number of vertices = " << roadmap_vertices_pcl_cloud.points.size();
- //std::cout << "Number of edge vectors = " << edges.size() << std::endl;
+ double ground_seg_time = 0.0;
+ std::cout << "Starting TRAVEL ground segmentation!" << std::endl;
+ travel_ground_seg->estimateGround(*cloud_in, *ground_pc, *nonground_pc, ground_seg_time);
+ std::cout << "\033[1;35m Traversable-Ground Seg: " << cloud_in->size() << " -> Ground: " << ground_pc->size()
+ << ", NonGround: " << nonground_pc->size() << "\033[0m" << std::endl;
+ std::cout << "Traversable-Ground Seg time: " << ground_seg_time << std::endl;
- // Now we create the markers for visualization and change the format of edges to implement the interface
- // between the library and the node
+ pcl::KdTreeFLANN < pcl::PointXYZI > kdtree_ground;
+ kdtree_ground.setInputCloud(ground_pc);
- int vertex_id = 0;
- int discarded_vertex_id = 0;
- int edge_id = 0;
- int discarded_edge_id = 0;
+ int K = 1;
+ std::vector<int> ground_pointIdxKNNSearch(K);
+ std::vector<float> ground_pointKNNSquaredDistance(K);
- for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = roadmap_vertices_pcl_cloud.begin();
- i != roadmap_vertices_pcl_cloud.end(); ++i)
- {
- pcl::PointXYZRGBNormal ground_ref = *i;
+ int index = 0;
+ for (pcl::PointCloud<pcl::PointXYZI>::iterator i = cloud_in->begin(); i != cloud_in->end(); ++i)
+ {
+ kdtree_ground.nearestKSearch(*i, K, ground_pointIdxKNNSearch, ground_pointKNNSquaredDistance);
- // DEBUG!
- //std::cout << "Vertex Pitch = " << ground_ref.data_c[GRND_REF_DATA_C_2_PITCH] << " ; Vertex Roll = "
- // << ground_ref.data_c[GRND_REF_DATA_C_1_ROLL] << std::endl;
+ bool ground = false;
+ if (sqrt(ground_pointKNNSquaredDistance[0]) < 0.001)
+ ground = true;
- assert(
- vertex_id == ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]
- && "Something unexpected in GroundSegmentationAlgorithm::segmentGround happened! vertex_id not equal to ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]!");
+ if (ground)
+ {
+ lidar_pcl_cloud.points[index].r = 0;
+ lidar_pcl_cloud.points[index].g = 0;
+ lidar_pcl_cloud.points[index].b = 255;
- //std::cout << "Converting vertex with id = " << vertex_id << " to marker!" << std::endl;
- //std::cout << "Number of edges for vertex " << vertex_id << " = " << edges[ground_ref.data_n[3]].size() << std::endl;
+ lidar_pcl_cloud.points[index].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_GROUND + 0.995;
+ }
+ else
+ {
+ lidar_pcl_cloud.points[index].r = 0;
+ lidar_pcl_cloud.points[index].g = 255;
+ lidar_pcl_cloud.points[index].b = 0;
- // TODO: comment out the marker array creation to see how much time it consumes
- visualization_msgs::Marker vertex;
- vertex.action = visualization_msgs::Marker::ADD;
- vertex.header = common_header;
+ lidar_pcl_cloud.points[index].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + 0.0;
+ }
- 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++;
+ ++index;
}
+ }
+ else
+ {
+ //std::cout << "Calling to processor_.groundSegmentation!" << std::endl;
- vertex.type = visualization_msgs::Marker::SPHERE;
+ processor_.groundSegmentation(lidar_configuration_, filtering_configuration_, lidar_pcl_cloud,
+ roadmap_vertices_pcl_cloud, edges, discarded_edges,
+ ground_dense_reconstruction_pcl_cloud);
- vertex.pose.position.x = ground_ref.x;
- vertex.pose.position.y = ground_ref.y;
- vertex.pose.position.z = ground_ref.z;
+ //std::cout << "Number of vertices = " << roadmap_vertices_pcl_cloud.points.size();
+ //std::cout << "Number of edge vectors = " << edges.size() << std::endl;
- vertex.pose.orientation.x = 0.0;
- vertex.pose.orientation.y = 0.0;
- vertex.pose.orientation.z = 0.0;
- vertex.pose.orientation.w = 1.0;
+ // Now we create the markers for visualization and change the format of edges to implement the interface
+ // between the library and the node
- vertex.scale.x = 0.15;
- vertex.scale.y = 0.15;
- vertex.scale.z = 0.15;
+ int vertex_id = 0;
+ int discarded_vertex_id = 0;
+ int edge_id = 0;
+ int discarded_edge_id = 0;
- vertex.color.a = 1.0; // Don't forget to set the alpha!
- if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
+ for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = roadmap_vertices_pcl_cloud.begin();
+ i != roadmap_vertices_pcl_cloud.end(); ++i)
{
- 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);
+ pcl::PointXYZRGBNormal ground_ref = *i;
- roadmap_markers.markers.push_back(vertex);
+ // DEBUG!
+ //std::cout << "Vertex Pitch = " << ground_ref.data_c[GRND_REF_DATA_C_2_PITCH] << " ; Vertex Roll = "
+ // << ground_ref.data_c[GRND_REF_DATA_C_1_ROLL] << std::endl;
- iri_ground_segmentation::edges edges_temp;
- for (std::vector<int>::iterator e = edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].begin();
- e != edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].end(); e++)
- {
- edges_temp.edges.push_back(*e);
+ assert(
+ vertex_id == ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]
+ && "Something unexpected in GroundSegmentationAlgorithm::segmentGround happened! vertex_id not equal to ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]!");
- visualization_msgs::Marker edge;
- edge.action = visualization_msgs::Marker::ADD;
- edge.header = common_header;
+ //std::cout << "Converting vertex with id = " << vertex_id << " to marker!" << std::endl;
+ //std::cout << "Number of edges for vertex " << vertex_id << " = " << edges[ground_ref.data_n[3]].size() << std::endl;
- if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
+ // TODO: comment out the marker array creation to see how much time it consumes
+ visualization_msgs::Marker vertex;
+ vertex.action = visualization_msgs::Marker::ADD;
+ vertex.header = common_header;
+
+ if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] < VERTEX_NOT_CONNECTED_TO_ROOT)
{
- edge.ns = "edges";
- edge.id = edge_id++;
+ vertex.ns = "vertices_connected_to_root";
+ vertex.id = vertex_id++;
}
else
{
- edge.ns = "discarded_edges";
- edge.id = discarded_edge_id++;
+ vertex.ns = "vertices_NOT_connected_to_root";
+ vertex.id = discarded_vertex_id++;
}
- edge.type = visualization_msgs::Marker::LINE_LIST;
-
- geometry_msgs::Point p;
- p.x = ground_ref.x;
- p.y = ground_ref.y;
- p.z = ground_ref.z;
- edge.points.push_back(p);
+ vertex.type = visualization_msgs::Marker::SPHERE;
- pcl::PointXYZRGBNormal child_ground_ref = roadmap_vertices_pcl_cloud.points[*e];
- p.x = child_ground_ref.x;
- p.y = child_ground_ref.y;
- p.z = child_ground_ref.z;
- edge.points.push_back(p);
+ vertex.pose.position.x = ground_ref.x;
+ vertex.pose.position.y = ground_ref.y;
+ vertex.pose.position.z = ground_ref.z;
- edge.scale.x = 0.05;
+ vertex.pose.orientation.x = 0.0;
+ vertex.pose.orientation.y = 0.0;
+ vertex.pose.orientation.z = 0.0;
+ vertex.pose.orientation.w = 1.0;
- edge.pose.orientation.x = 0.0;
- edge.pose.orientation.y = 0.0;
- edge.pose.orientation.z = 0.0;
- edge.pose.orientation.w = 1.0;
+ vertex.scale.x = 0.15;
+ vertex.scale.y = 0.15;
+ vertex.scale.z = 0.15;
- edge.color.a = 0.7; // Don't forget to set the alpha!
+ vertex.color.a = 1.0; // Don't forget to set the alpha!
if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
{
- edge.color.r = 0.8;
- edge.color.g = 0.53;
- edge.color.b = 0.0;
+ vertex.color.r = 1.0;
+ vertex.color.g = 0.66;
+ vertex.color.b = 0.0;
}
else
{
- edge.color.r = R_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
- edge.color.g = G_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
- edge.color.b = B_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
+ vertex.color.r = 1.0;
+ vertex.color.g = 0.0;
+ vertex.color.b = 0.0;
}
- edge.lifetime = ros::Duration(0.1);
+ vertex.lifetime = ros::Duration(0); //0.1
- roadmap_markers.markers.push_back(edge);
- }
+ roadmap_markers.markers.push_back(vertex);
- edges_array.edges_array.push_back(edges_temp);
+ iri_ground_segmentation::edges edges_temp;
+ for (std::vector<int>::iterator e = edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].begin();
+ e != edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].end(); e++)
+ {
+ edges_temp.edges.push_back(*e);
+
+ visualization_msgs::Marker edge;
+ edge.action = visualization_msgs::Marker::ADD;
+ edge.header = common_header;
+
+ if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_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;
+ p.x = ground_ref.x;
+ p.y = ground_ref.y;
+ p.z = ground_ref.z;
+ edge.points.push_back(p);
+
+ pcl::PointXYZRGBNormal child_ground_ref = roadmap_vertices_pcl_cloud.points[*e];
+ p.x = child_ground_ref.x;
+ p.y = child_ground_ref.y;
+ p.z = child_ground_ref.z;
+ edge.points.push_back(p);
+
+ edge.scale.x = 0.05;
+
+ edge.pose.orientation.x = 0.0;
+ edge.pose.orientation.y = 0.0;
+ edge.pose.orientation.z = 0.0;
+ edge.pose.orientation.w = 1.0;
+
+ edge.color.a = 0.7; // Don't forget to set the alpha!
+ if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
+ {
+ edge.color.r = 0.8;
+ edge.color.g = 0.53;
+ edge.color.b = 0.0;
+ }
+ else
+ {
+ edge.color.r = R_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
+ edge.color.g = G_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
+ edge.color.b = B_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
+ }
+ edge.lifetime = ros::Duration(0); // 0.1
+
+ roadmap_markers.markers.push_back(edge);
+ }
- // We create markers for the discarded edges
- for (int e = 0; e < discarded_edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].size(); e++)
- {
- kf_based_terrain_analysis_lib::Edge discarded_edge_index_and_class =
- discarded_edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]][e];
-
- visualization_msgs::Marker discarded_edge;
- discarded_edge.action = visualization_msgs::Marker::ADD;
- discarded_edge.header = common_header;
- discarded_edge.ns = "discarded_edges";
- discarded_edge.id = discarded_edge_id++;
- discarded_edge.type = visualization_msgs::Marker::LINE_LIST;
-
- geometry_msgs::Point p;
- p.x = ground_ref.x;
- p.y = ground_ref.y;
- p.z = ground_ref.z;
- discarded_edge.points.push_back(p);
-
- pcl::PointXYZRGBNormal child_ground_ref =
- roadmap_vertices_pcl_cloud.points[discarded_edge_index_and_class.vertex_index];
- p.x = child_ground_ref.x;
- p.y = child_ground_ref.y;
- p.z = child_ground_ref.z;
- discarded_edge.points.push_back(p);
-
- discarded_edge.scale.x = 0.05;
-
- discarded_edge.pose.orientation.x = 0.0;
- discarded_edge.pose.orientation.y = 0.0;
- discarded_edge.pose.orientation.z = 0.0;
- discarded_edge.pose.orientation.w = 1.0;
-
- discarded_edge.color.a = 0.7;
- switch (discarded_edge_index_and_class.edge_class)
+ edges_array.edges_array.push_back(edges_temp);
+
+ // We create markers for the discarded edges
+ for (int e = 0; e < discarded_edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].size(); e++)
{
- case CLASS_EDGE_DISCARDED_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_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
- discarded_edge.color.g = (float)G_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
- discarded_edge.color.b = (float)B_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
- break;
- default:
- //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;
- break;
+ kf_based_terrain_analysis_lib::Edge discarded_edge_index_and_class =
+ discarded_edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]][e];
+
+ visualization_msgs::Marker discarded_edge;
+ discarded_edge.action = visualization_msgs::Marker::ADD;
+ discarded_edge.header = common_header;
+ discarded_edge.ns = "discarded_edges";
+ discarded_edge.id = discarded_edge_id++;
+ discarded_edge.type = visualization_msgs::Marker::LINE_LIST;
+
+ geometry_msgs::Point p;
+ p.x = ground_ref.x;
+ p.y = ground_ref.y;
+ p.z = ground_ref.z;
+ discarded_edge.points.push_back(p);
+
+ pcl::PointXYZRGBNormal child_ground_ref =
+ roadmap_vertices_pcl_cloud.points[discarded_edge_index_and_class.vertex_index];
+ p.x = child_ground_ref.x;
+ p.y = child_ground_ref.y;
+ p.z = child_ground_ref.z;
+ discarded_edge.points.push_back(p);
+
+ discarded_edge.scale.x = 0.05;
+
+ discarded_edge.pose.orientation.x = 0.0;
+ discarded_edge.pose.orientation.y = 0.0;
+ discarded_edge.pose.orientation.z = 0.0;
+ discarded_edge.pose.orientation.w = 1.0;
+
+ discarded_edge.color.a = 0.7;
+ switch (discarded_edge_index_and_class.edge_class)
+ {
+ case CLASS_EDGE_DISCARDED_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_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
+ discarded_edge.color.g = (float)G_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
+ discarded_edge.color.b = (float)B_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
+ break;
+ default:
+ //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;
+ break;
+ }
+ discarded_edge.lifetime = ros::Duration(0); // 0.1
+ roadmap_markers.markers.push_back(discarded_edge);
}
- discarded_edge.lifetime = ros::Duration(0.1);
- roadmap_markers.markers.push_back(discarded_edge);
}
- }
- //std::cout << "Total number of edges = " << edge_id << std::endl;
- //getchar();
+ //std::cout << "Total number of edges = " << edge_id << std::endl;
+ //getchar();
+ }
}
diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index 6c217f0acc7e37a98c5cd62cfdcaf2e61200f62c..0040cfd9958f53d894a898ec17877c02e8046174 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/src/ground_segmentation_alg_node.cpp
@@ -450,6 +450,13 @@ 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;
+ this->roadmap_markers_publisher_.publish(markerD);
+
this->roadmap_markers_publisher_.publish(this->roadmap_MarkerArray_msg_);
}