Skip to content
Snippets Groups Projects
Commit fdc22ed2 authored by Iván del Pino's avatar Iván del Pino
Browse files

organized parameters

parent f3e52c11
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
cfg/GroundSegmentation.cfg
+ 25
− 18
View file @ fdc22ed2
......@@ -40,28 +40,35 @@ gen = ParameterGenerator()
# Name Type Reconfiguration level Description Default Min Max
gen.add("rate", double_t, 0, "Main loop rate (Hz)", 100.0, 0.1, 1000.0)
gen.add("sensor_height", double_t, 0, "Height of the sensor center", 1.75, 0.0, 5.0); #KITTI = 1.75: BLUE = 1.3
gen.add("mahalanobis_threshold", double_t, 0, "Max mahalanobis distance for ground points", 3.0, 0.0, 1000.0);
gen.add("z_initial_std_dev", double_t, 0, "for ground reference initialization", 0.01, 0.0, 1000.0);
gen.add("initial_angular_std_dev_deg", double_t, 0, "for ground reference initialization", 1.333, 0.0, 1000.0);
gen.add("ground_reference_search_resolution_deg", double_t, 0, "defines the angular search regions in degrees", 30.0, 0.001, 1000.0);
gen.add("elevation_grid_resolution", double_t, 0, "define the coarseness of the elevation points", 1.0, 0.001, 1000.0);
# Robot related geometric parameters
gen.add("sensor_height", double_t, 0, "Ground is expected to be at z = -1*sensor_height", 0.27, 0.0, 2.0); #KITTI = 1.75: BLUE = 1.3 ADD baselink = 0.27
gen.add("robot_height", double_t, 0, "All obstacle points above this value (w.r.t the z ground predicted by the algorithm) are treated as overhanging obstacles", 1.50, 0.0, 3.0);
gen.add("angle_reduction_factor", double_t, 0, "To make the search area dependent to the data sparseness", 2.0, -100.0, 100.0);
gen.add("search_limit_in_shadow_area", double_t, 0, "minimum ROI area = 4*search_limit_in_shadow_area squared", 3.0, 0.0, 1000.0);
gen.add("propagation_z_additive_noise_per_meter", double_t, 0, "Noise added to new ground references scaled by the range", 0.1, 0.0, 1000.0);
gen.add("propagation_additive_noise_deg_per_meter", double_t, 0, "Noise added to new ground references scaled by the range", 0.1, 0.0, 1000.0);
gen.add("z_observation_std_dev", double_t, 0, "Measurement std_dev", 0.1, 0.0, 1000.0);
# Parameters affecting the pointcloud exploration
gen.add("ROI_delta_x_and_y", double_t, 0, "ROI area = ( 2 * ROI_delta_x_and_y ) ^ 2", 2.0, 0.0, 10.0);
gen.add("ground_reference_search_resolution_deg", double_t, 0, "It is the angular resolution when generating new ground references (graph nodes) in degrees", 20.0, 1.0, 100.0);
gen.add("elevation_grid_resolution", double_t, 0, "define the coarseness of the elevation points", 0.5, 0.01, 10.0);
# Kalman filter noise parameters
## initial uncertainties
gen.add("z_initial_std_dev", double_t, 0, "for ground reference initialization", 0.05, 0.01, 1.0);
gen.add("initial_angular_std_dev_deg", double_t, 0, "for ground reference initialization", 1.5, 0.1, 10.0);
## propagation additive noises
gen.add("propagation_z_additive_noise_per_meter", double_t, 0, "Noise added to new ground references scaled by the range", 0.05, 0.0, 1.0);
gen.add("propagation_additive_noise_deg_per_meter", double_t, 0, "Noise added to new ground references scaled by the range", 0.8, 0.0, 10.0);
# measurement noise
gen.add("z_observation_std_dev", double_t, 0, "Measurement std_dev", 0.2, 0.0, 1.0);
gen.add("robot_height", double_t, 0, "used to discard edges", 1.20, 0.0, 3.0);
# threshold for outlier rejection and classification
gen.add("mahalanobis_threshold", double_t, 0, "Max mahalanobis distance for ground points", 3.0, 0.01, 6.0);
gen.add("max_traversable_obstacle_height", double_t, 0, "used to discard edges", 0.15, 0.01, 0.99);
gen.add("max_slope_abs", double_t, 0, "used to discard edges", 0.3, 0.01, 0.99);
# labeling parameters
gen.add("number_of_references_used_for_labelling", int_t, 0, "used to evaluate each elevation point cloud from different POVs", 4, 1, 100);
gen.add("max_pred_std_dev_for_labelling", double_t, 0, "To give up trying to label ground points if we don't have enough confidence in our predictions", 0.3, 0.01, 1.0);
gen.add("classify_not_labeled_points_as_obstacles", bool_t, 0, "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", True);
gen.add("discard_not_connected_references_for_labelling", bool_t, 0, "to exclude not connected vertices in the roadmap from the labelling step", True);
# visualization and debug parameters
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);
......
src/ground_segmentation_alg.cpp
+ 1
− 15
View file @ fdc22ed2
......@@ -47,7 +47,7 @@ void GroundSegmentationAlgorithm::segmentGround(
ground_dense_reconstruction_pcl_cloud);
//std::cout << "Number of vertices = " << roadmap_vertices_pcl_cloud.points.size();
//std::cout << "Number of edge vectors = " << edges.size();
//std::cout << "Number of edge vectors = " << edges.size() << 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
......@@ -218,13 +218,6 @@ void GroundSegmentationAlgorithm::segmentGround(
discarded_edge.color.a = 0.7;
switch (discarded_edge_index_and_class.edge_class)
{
case CLASS_EDGE_DISCARDED_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_EDGE_DISCARDED_TOO_MUCH_UNCERTAINTY / 255.0;
discarded_edge.color.g = (float)G_EDGE_DISCARDED_TOO_MUCH_UNCERTAINTY / 255.0;
discarded_edge.color.b = (float)B_EDGE_DISCARDED_TOO_MUCH_UNCERTAINTY / 255.0;
break;
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;
......@@ -232,13 +225,6 @@ void GroundSegmentationAlgorithm::segmentGround(
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;
case CLASS_EDGE_DISCARDED_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_EDGE_DISCARDED_TOO_MUCH_SLOPE / 255.0;
discarded_edge.color.g = (float)G_EDGE_DISCARDED_TOO_MUCH_SLOPE / 255.0;
discarded_edge.color.b = (float)B_EDGE_DISCARDED_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;
......
src/ground_segmentation_alg_node.cpp
+ 101
− 69
View file @ fdc22ed2
......@@ -11,6 +11,7 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->setRate(this->config_.rate);
//////////////// Robot related geometric parameters
if (!this->private_node_handle_.getParam("sensor_height", this->config_.sensor_height))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'sensor_height' not found");
......@@ -18,31 +19,20 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->alg_.lidar_configuration_.sensor_height = config_.sensor_height;
if (!this->private_node_handle_.getParam("mahalanobis_threshold", this->config_.mahalanobis_threshold))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'mahalanobis_threshold' not found");
}
else
this->alg_.filtering_configuration_.mahalanobis_threshold = config_.mahalanobis_threshold;
if (!this->private_node_handle_.getParam("z_initial_std_dev", this->config_.z_initial_std_dev))
if (!this->private_node_handle_.getParam("robot_height", this->config_.robot_height))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'z_initial_std_dev' not found");
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'robot_height' not found");
}
else
this->alg_.filtering_configuration_.z_initial_std_dev = config_.z_initial_std_dev;
this->alg_.filtering_configuration_.robot_height = config_.robot_height;
if (!this->private_node_handle_.getParam("initial_angular_std_dev_deg", this->config_.initial_angular_std_dev_deg))
//////////////// Parameters affecting the exploration of the pointcloud
if (!this->private_node_handle_.getParam("ROI_delta_x_and_y", this->config_.ROI_delta_x_and_y))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'initial_angular_std_dev_deg' not found");
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'ROI_delta_x_and_y' not found");
}
else
{
// we estimate the slopes in x and y axis, so it is the tangent of the angle, but we set the values in degrees because
// it results more intuitive
this->alg_.filtering_configuration_.roll_initial_std_dev = tan(config_.initial_angular_std_dev_deg * M_PI / 180.0);
this->alg_.filtering_configuration_.pitch_initial_std_dev = tan(config_.initial_angular_std_dev_deg * M_PI / 180.0);
}
this->alg_.filtering_configuration_.ROI_delta_x_and_y = config_.ROI_delta_x_and_y;
if (!this->private_node_handle_.getParam("ground_reference_search_resolution_deg",
this->config_.ground_reference_search_resolution_deg))
......@@ -61,20 +51,27 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->alg_.filtering_configuration_.elevation_grid_resolution = config_.elevation_grid_resolution;
if (!this->private_node_handle_.getParam("angle_reduction_factor", this->config_.angle_reduction_factor))
/////////// Kalman filter noise parameters
///////////////// initial uncertainties
if (!this->private_node_handle_.getParam("z_initial_std_dev", this->config_.z_initial_std_dev))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'angle_reduction_factor' not found");
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'z_initial_std_dev' not found");
}
else
this->alg_.filtering_configuration_.angle_reduction_factor = config_.angle_reduction_factor;
this->alg_.filtering_configuration_.z_initial_std_dev = config_.z_initial_std_dev;
if (!this->private_node_handle_.getParam("search_limit_in_shadow_area", this->config_.search_limit_in_shadow_area))
if (!this->private_node_handle_.getParam("initial_angular_std_dev_deg", this->config_.initial_angular_std_dev_deg))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'search_limit_in_shadow_area' not found");
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'initial_angular_std_dev_deg' not found");
}
else
this->alg_.filtering_configuration_.search_limit_in_shadow_area = config_.search_limit_in_shadow_area;
{
// we estimate the slopes in x and y axis, so it is the tangent of the angle, but we set the values in degrees because
// it results more intuitive
this->alg_.filtering_configuration_.roll_initial_std_dev = tan(config_.initial_angular_std_dev_deg * M_PI / 180.0);
this->alg_.filtering_configuration_.pitch_initial_std_dev = tan(config_.initial_angular_std_dev_deg * M_PI / 180.0);
}
////////////////// propagation additive noises
if (!this->private_node_handle_.getParam("propagation_z_additive_noise_per_meter",
this->config_.propagation_z_additive_noise_per_meter))
{
......@@ -91,9 +88,9 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
"GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'propagation_additive_noise_deg_per_meter' not found");
}
else
this->alg_.filtering_configuration_.propagation_additive_noise = tan(config_.propagation_additive_noise_deg_per_meter
* M_PI / 180.0);
this->alg_.filtering_configuration_.propagation_additive_noise = tan(
config_.propagation_additive_noise_deg_per_meter * M_PI / 180.0);
///////////////// measurement noise
if (!this->private_node_handle_.getParam("z_observation_std_dev", this->config_.z_observation_std_dev))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'z_observation_std_dev' not found");
......@@ -101,29 +98,54 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->alg_.filtering_configuration_.z_observation_std_dev = config_.z_observation_std_dev;
/////////////////// threshold for outlier rejection and classification
if (!this->private_node_handle_.getParam("mahalanobis_threshold", this->config_.mahalanobis_threshold))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'mahalanobis_threshold' not found");
}
else
this->alg_.filtering_configuration_.mahalanobis_threshold = config_.mahalanobis_threshold;
if (!this->private_node_handle_.getParam("robot_height", this->config_.robot_height))
////////////////// labeling parameters
if (!this->private_node_handle_.getParam("number_of_references_used_for_labelling",
this->config_.number_of_references_used_for_labelling))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'robot_height' not found");
ROS_WARN(
"GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'number_of_references_used_for_labelling' not found");
}
else
this->alg_.filtering_configuration_.robot_height = config_.robot_height;
this->alg_.filtering_configuration_.number_of_references_used_for_labelling =
config_.number_of_references_used_for_labelling;
if (!this->private_node_handle_.getParam("max_pred_std_dev_for_labelling",
this->config_.max_pred_std_dev_for_labelling))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'max_pred_std_dev_for_labelling' not found");
}
else
this->alg_.filtering_configuration_.max_pred_std_dev_for_labelling = config_.max_pred_std_dev_for_labelling;
if (!this->private_node_handle_.getParam("max_slope_abs", this->config_.max_slope_abs))
if (!this->private_node_handle_.getParam("classify_not_labeled_points_as_obstacles",
this->config_.classify_not_labeled_points_as_obstacles))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'max_slope_abs' not found");
ROS_WARN(
"GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'classify_not_labeled_points_as_obstacles' not found");
}
else
this->alg_.filtering_configuration_.max_slope_abs = config_.max_slope_abs;
this->alg_.filtering_configuration_.classify_not_labeled_points_as_obstacles =
config_.classify_not_labeled_points_as_obstacles;
if (!this->private_node_handle_.getParam("max_traversable_obstacle_height",
this->config_.max_traversable_obstacle_height))
if (!this->private_node_handle_.getParam("discard_not_connected_references_for_labelling",
this->config_.discard_not_connected_references_for_labelling))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'max_traversable_obstacle_height' not found");
ROS_WARN(
"GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'discard_not_connected_references_for_labelling' not found");
}
else
this->alg_.filtering_configuration_.max_traversable_obstacle_height = config_.max_traversable_obstacle_height;
this->alg_.filtering_configuration_.discard_not_connected_references_for_labelling =
config_.discard_not_connected_references_for_labelling;
////////////////// visualization and debug parameters
if (!this->private_node_handle_.getParam("measure_performance", this->config_.measure_performance))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'measure_performance' not found");
......@@ -420,31 +442,41 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
if (config_.rate != this->getRate())
this->setRate(config_.rate);
// Robot related geometric parameters
this->alg_.lidar_configuration_.sensor_height = config.sensor_height;
this->alg_.filtering_configuration_.robot_height = config.robot_height;
this->alg_.filtering_configuration_.mahalanobis_threshold = config.mahalanobis_threshold;
this->alg_.filtering_configuration_.z_initial_std_dev = config.z_initial_std_dev;
this->alg_.filtering_configuration_.roll_initial_std_dev = tan(config.initial_angular_std_dev_deg * M_PI / 180.0);
this->alg_.filtering_configuration_.pitch_initial_std_dev = tan(config.initial_angular_std_dev_deg * M_PI / 180.0);
// Parameters affecting the exploration of the pointcloud
this->alg_.filtering_configuration_.ROI_delta_x_and_y = config.ROI_delta_x_and_y;
this->alg_.filtering_configuration_.ground_reference_search_resolution = config.ground_reference_search_resolution_deg
* M_PI / 180.0;
this->alg_.filtering_configuration_.elevation_grid_resolution = config.elevation_grid_resolution;
this->alg_.filtering_configuration_.angle_reduction_factor = config.angle_reduction_factor;
this->alg_.filtering_configuration_.search_limit_in_shadow_area = config.search_limit_in_shadow_area;
// Kalman filter noise parameters
// Initial uncertainties
this->alg_.filtering_configuration_.z_initial_std_dev = config.z_initial_std_dev;
this->alg_.filtering_configuration_.roll_initial_std_dev = tan(config.initial_angular_std_dev_deg * M_PI / 180.0);
this->alg_.filtering_configuration_.pitch_initial_std_dev = tan(config.initial_angular_std_dev_deg * M_PI / 180.0);
// Propagation additive noises
this->alg_.filtering_configuration_.propagation_z_additive_noise = config.propagation_z_additive_noise_per_meter;
this->alg_.filtering_configuration_.propagation_additive_noise = tan(config.propagation_additive_noise_deg_per_meter
* M_PI / 180.0);
this->alg_.filtering_configuration_.propagation_additive_noise = tan(
config.propagation_additive_noise_deg_per_meter * M_PI / 180.0);
// Measurement noise
this->alg_.filtering_configuration_.z_observation_std_dev = config.z_observation_std_dev;
this->alg_.filtering_configuration_.robot_height = config.robot_height;
// threshold for outlier rejection and classification
this->alg_.filtering_configuration_.mahalanobis_threshold = config.mahalanobis_threshold;
this->alg_.filtering_configuration_.max_traversable_obstacle_height = config_.max_traversable_obstacle_height;
this->alg_.filtering_configuration_.max_slope_abs = config.max_slope_abs;
// labeling parameters
this->alg_.filtering_configuration_.number_of_references_used_for_labelling =
config.number_of_references_used_for_labelling;
this->alg_.filtering_configuration_.max_pred_std_dev_for_labelling = config.max_pred_std_dev_for_labelling;
this->alg_.filtering_configuration_.classify_not_labeled_points_as_obstacles =
config_.classify_not_labeled_points_as_obstacles;
this->alg_.filtering_configuration_.discard_not_connected_references_for_labelling =
config_.discard_not_connected_references_for_labelling;
// visualization and debug parameters
this->alg_.filtering_configuration_.measure_performance = config.measure_performance;
this->alg_.filtering_configuration_.show_dense_reconstruction = config.show_dense_reconstruction;
......@@ -454,9 +486,14 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
std::cout << "sensor_height = " << this->alg_.lidar_configuration_.sensor_height
<< std::endl;
std::cout << "robot_height = " << this->alg_.filtering_configuration_.robot_height << std::endl;
std::cout << "mahalanobis_threshold = "
<< this->alg_.filtering_configuration_.mahalanobis_threshold << std::endl;
std::cout << "ROI_delta_x_and_y = " << this->alg_.filtering_configuration_.ROI_delta_x_and_y
<< std::endl;
std::cout << "ground_reference_search_resolution (deg) = "
<< this->alg_.filtering_configuration_.ground_reference_search_resolution * 180.0 / M_PI << std::endl;
std::cout << "elevation_grid_resolution = "
<< this->alg_.filtering_configuration_.elevation_grid_resolution << std::endl;
std::cout << "z_initial_std_dev = "
<< this->alg_.filtering_configuration_.z_initial_std_dev << std::endl;
......@@ -464,17 +501,6 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
<< atan((double)this->alg_.filtering_configuration_.roll_initial_std_dev) * 180.0 / M_PI << std::endl;
std::cout << "pitch_initial_std_dev (deg) = "
<< atan((double)this->alg_.filtering_configuration_.pitch_initial_std_dev) * 180.0 / M_PI << std::endl;
std::cout << "ground_reference_search_resolution (deg) = "
<< this->alg_.filtering_configuration_.ground_reference_search_resolution * 180.0 / M_PI << std::endl;
std::cout << "elevation_grid_resolution = "
<< this->alg_.filtering_configuration_.elevation_grid_resolution << std::endl;
std::cout << "angle_reduction_factor = "
<< this->alg_.filtering_configuration_.angle_reduction_factor << std::endl;
std::cout << "search_limit_in_shadow_area = "
<< this->alg_.filtering_configuration_.search_limit_in_shadow_area << std::endl;
std::cout << "propagation_z_additive_noise in m/m = "
<< this->alg_.filtering_configuration_.propagation_z_additive_noise << std::endl;
std::cout << "propagation_additive_noise in deg/m = "
......@@ -482,11 +508,17 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
std::cout << "z_observation_std_dev = "
<< this->alg_.filtering_configuration_.z_observation_std_dev << std::endl;
std::cout << "robot_height = " << this->alg_.filtering_configuration_.robot_height << std::endl;
std::cout << "mahalanobis_threshold = "
<< this->alg_.filtering_configuration_.mahalanobis_threshold << std::endl;
std::cout << "max_traversable_obstacle_height = "
<< this->alg_.filtering_configuration_.max_traversable_obstacle_height << std::endl;
std::cout << "max_slope_abs (adimensional) = " << this->alg_.filtering_configuration_.max_slope_abs << std::endl;
std::cout << "number_of_references_used_for_labelling = "
<< this->alg_.filtering_configuration_.number_of_references_used_for_labelling << std::endl;
std::cout << "max_pred_std_dev_for_labelling = "
<< this->alg_.filtering_configuration_.max_pred_std_dev_for_labelling << std::endl;
std::cout << "classify_not_labeled_points_as_obstacles = "
<< this->alg_.filtering_configuration_.classify_not_labeled_points_as_obstacles << std::endl;
std::cout << "discard_not_connected_references_for_labelling = "
<< this->alg_.filtering_configuration_.discard_not_connected_references_for_labelling << std::endl;
std::cout << "measure_performance = " << this->alg_.filtering_configuration_.measure_performance << std::endl;
std::cout << "show_dense_reconstruction = " << this->alg_.filtering_configuration_.show_dense_reconstruction
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment