From 15cf0efdde7ad9bfe2c7f91ece8f2b6e142c5aa9 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Mon, 10 Feb 2020 12:14:55 +0100
Subject: [PATCH] Used the maximum allowed inclination as a parameter. Solved a
bug with the use of abs() instead of fabs().
---
cfg/ObstacleDetectionNormals.cfg | 6 ++----
config/default_config.yaml | 4 +---
src/obstacle_detection_normals_alg.cpp | 8 ++------
3 files changed, 5 insertions(+), 13 deletions(-)
diff --git a/cfg/ObstacleDetectionNormals.cfg b/cfg/ObstacleDetectionNormals.cfg
index c9c1054..f1f1d57 100755
--- a/cfg/ObstacleDetectionNormals.cfg
+++ b/cfg/ObstacleDetectionNormals.cfg
@@ -39,9 +39,7 @@ gen = ParameterGenerator()
# Name Type Reconfiguration level Description Default Min Max
#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
-gen.add("filter_min_dist", double_t, 0, "Min distance threshold", 0.3, 0, 100)
+gen.add("filter_min_dist", double_t, 0, "Min distance threshold", 0.3, 0, 100)
gen.add("normal_KSearch", int_t, 0, "No of neighbors to estimate the surface",100,10, 200)
-gen.add("normal_x", double_t, 0, "Accepted Normal X component", 1,)
-gen.add("normal_y", double_t, 0, "Accepted Normal Y component", -0.2,)
-gen.add("normal_z", double_t, 0, "Accepted Normal Z component", 1,)
+gen.add("max_inc", double_t, 0, "maximum inclination in rad", 0.25, 0.0, 1.0)
exit(gen.generate(PACKAGE, "ObstacleDetectionNormalsAlgorithm", "ObstacleDetectionNormals"))
diff --git a/config/default_config.yaml b/config/default_config.yaml
index 9fab8e5..1ce6dbe 100755
--- a/config/default_config.yaml
+++ b/config/default_config.yaml
@@ -1,5 +1,3 @@
filter_min_dist: 0.3
normal_KSearch: 30
-normal_x: 1.001
-normal_y: 1.001
-normal_z: 0.95
+max_inc: 0.25
diff --git a/src/obstacle_detection_normals_alg.cpp b/src/obstacle_detection_normals_alg.cpp
index 7854b2d..33061ad 100644
--- a/src/obstacle_detection_normals_alg.cpp
+++ b/src/obstacle_detection_normals_alg.cpp
@@ -58,18 +58,14 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(const pcl::PointCloud<pcl::Poi
*cloud_obs=*cloud_out;
- std::cout << &this->config_ << std::endl;
- std::cout << this->config_.normal_z << "," << this->config_.normal_y << "," << this->config_.normal_x << std::endl;
for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_out->height; ++rowIndex)
{
for (size_t colIndex=0; colIndex<cloud_out->width; ++colIndex, ++pointIndex)
{
if (cloud_out->points[pointIndex].x > this->config_.filter_min_dist)
{
-// std::cout << "(" << rowIndex << "," << pointIndex << "): " << cloud_out->points[pointIndex].normal_z << "," << cloud_out->points[pointIndex].normal_y << "," << cloud_out->points[pointIndex].normal_x << std::endl;
- if (abs(cloud_out->points[pointIndex].normal_z) > this->config_.normal_z &&
- cloud_out->points[pointIndex].normal_y < this->config_.normal_y &&
- cloud_out->points[pointIndex].normal_x < this->config_.normal_x )
+ std::cout << "(" << rowIndex << "," << pointIndex << "): " << cloud_out->points[pointIndex].normal_z << "," << this->config_.max_inc << std::endl;
+ if (fabs(cloud_out->points[pointIndex].normal_z) > cos(this->config_.max_inc))
{
cloud_out->points[pointIndex].r=0;
cloud_out->points[pointIndex].g=255;
--
GitLab