diff --git a/cfg/GroundSegmentation.cfg b/cfg/GroundSegmentation.cfg
index 04606b670178e3aad1df945bc7afb031b52b2723..c9eceaa10be223ce6b6abf9f44cf7a8eab3ecb96 100644
--- a/cfg/GroundSegmentation.cfg
+++ b/cfg/GroundSegmentation.cfg
@@ -80,7 +80,7 @@ gen.add("ground_threshold_in_not_analyzed_areas", double_t, 0, "when it is not p
# Low obstacle detection parameters
gen.add("low_obstacle_min_height", double_t, 0, "obstacle below this height are not considered", 0.05, 0.001, 0.3);
-gen.add("sensor_vertical_resolution_deg", double_t, 0, "to calculate distances at which low obstacles can be detected", 0.333, 0.01, 10.0);
+gen.add("sensor_best_resolution_deg", double_t, 0, "Either vertical or horizontal, used to calculate distances at which low obstacles can be detected", 0.333, 0.01, 10.0);
gen.add("max_navigation_slope", double_t, 0, "slopes greater than this threshold will be considered obstacles", 0.2, 0.01, 1.0);
# visualization and debug parameters
diff --git a/kf_based_terrain_analysis b/kf_based_terrain_analysis
index 7c5334ad89b0b8f0677640b66844605d07e1fd00..c955f0f6500e197e3975c3194e9cb73655a70357 160000
--- a/kf_based_terrain_analysis
+++ b/kf_based_terrain_analysis
@@ -1 +1 @@
-Subproject commit 7c5334ad89b0b8f0677640b66844605d07e1fd00
+Subproject commit c955f0f6500e197e3975c3194e9cb73655a70357
diff --git a/params/iri_ground_segmentation.yaml b/params/iri_ground_segmentation.yaml
index 3d4e13fe356b289df566cd30a25be09488cca03a..083bfd61689222c7f0d61fa6068848c73817ad23 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: false,
+ use_neural_network: true,
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',
@@ -53,8 +53,8 @@ iri_ground_segmentation: {
# low obstacle detection parameters
low_obstacle_min_height: 0.05,
- sensor_vertical_resolution_deg: 0.333,
- max_navigation_slope: 0.2,
+ sensor_best_resolution_deg: 0.2, # Best resolution, either horizontal or vertical
+ max_navigation_slope: 0.5,
# visualization and debug parameters
measure_performance: false, ## (feature still not debugged) Flag to measure number of execution and execution times of the different functions of the algorithm
diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index 231983975baafefe2b2fe512bbda4fe4bc16a866..748edaa52044972c5430e51a81d7a27fde729a80 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/src/ground_segmentation_alg_node.cpp
@@ -236,12 +236,12 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->alg_.filtering_configuration_.low_obstacle_min_height = config_.low_obstacle_min_height;
- if (!this->private_node_handle_.getParam("sensor_vertical_resolution_deg", this->config_.sensor_vertical_resolution_deg))
+ if (!this->private_node_handle_.getParam("sensor_best_resolution_deg", this->config_.sensor_best_resolution_deg))
{
- ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'sensor_vertical_resolution_deg' not found");
+ ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'sensor_best_resolution_deg' not found");
}
else
- this->alg_.filtering_configuration_.sensor_vertical_resolution_deg = config_.sensor_vertical_resolution_deg;
+ this->alg_.filtering_configuration_.sensor_best_resolution_deg = config_.sensor_best_resolution_deg;
if (!this->private_node_handle_.getParam("max_navigation_slope", this->config_.max_navigation_slope))
{
@@ -644,7 +644,7 @@ void GroundSegmentationAlgNode::showPerformanceStatistics(void)
struct Cell
{
float z_min, z_max;
- int label = -1; // -1 sin inicializar, 0 transitable, 1 obstáculo
+ int label = -1; // -1 uninitialized, 0 traversable, 1 obstacle
};
void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float alpha)
@@ -654,14 +654,14 @@ void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float
float r_max = h / alpha;
float K = 1.0 / sin(res);
float d_max = K * r_max;
- float cell_side = r_max / sqrt(8.0);
+ float cell_side = r_max / sqrt(8.0); // This comes from here: sqrt((r_max/2)^2 / 2)
int num_of_cells = (d_max * 2.0 / cell_side);
- std::cout << "r_max = " << r_max << std::endl;
- std::cout << "K = " << K << std::endl;
- std::cout << "d_max = " << d_max << std::endl;
- std::cout << "cell_side = " << cell_side << std::endl;
- std::cout << "num_of_cells = " << num_of_cells << std::endl;
+ //std::cout << "r_max = " << r_max << std::endl;
+ //std::cout << "K = " << K << std::endl;
+ //std::cout << "d_max = " << d_max << std::endl;
+ //std::cout << "cell_side = " << cell_side << std::endl;
+ //std::cout << "num_of_cells = " << num_of_cells << std::endl;
std::vector < std::vector < Cell >> grid(num_of_cells, std::vector < Cell > (num_of_cells));
@@ -705,7 +705,7 @@ void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float
if (abs(grid[i][j].z_min - grid[nx][ny].z_max) > h || abs(grid[i][j].z_max - grid[nx][ny].z_min) > h)
{
grid[i][j].label = 1;
- dx = dy = 2; // abortar los bucles
+ dx = dy = 2; // break the loop
}
}
}
@@ -752,7 +752,7 @@ void GroundSegmentationAlgNode::mainNodeThread(void)
ground_dense_reconstruction_pcl_cloud_, roadmap_MarkerArray_msg_);
this->detectLowObstacles(this->alg_.filtering_configuration_.low_obstacle_min_height,
- this->alg_.filtering_configuration_.sensor_vertical_resolution_deg,
+ this->alg_.filtering_configuration_.sensor_best_resolution_deg,
this->alg_.filtering_configuration_.max_navigation_slope);
//std::cout << "Ground segmentation finished!" << std::endl;
@@ -852,7 +852,7 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
// Low obstacles detection parameters
this->alg_.filtering_configuration_.low_obstacle_min_height = config_.low_obstacle_min_height;
- this->alg_.filtering_configuration_.sensor_vertical_resolution_deg = config_.sensor_vertical_resolution_deg;
+ this->alg_.filtering_configuration_.sensor_best_resolution_deg = config_.sensor_best_resolution_deg;
this->alg_.filtering_configuration_.max_navigation_slope = config_.max_navigation_slope;
// visualization and debug parameters