From e179f81f4280957d52626fc1e1760257f4ea80a4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Iv=C3=A1n=20del=20Pino?= <idelpino@iri.upc.edu>
Date: Fri, 7 Oct 2022 16:48:08 +0200
Subject: [PATCH] more cleaning
---
include/structs_definitions.h | 19 +++++++----------
src/kf_based_terrain_analysis.cpp | 34 ++++++++-----------------------
2 files changed, 15 insertions(+), 38 deletions(-)
diff --git a/include/structs_definitions.h b/include/structs_definitions.h
index d772605..9ade8a8 100644
--- a/include/structs_definitions.h
+++ b/include/structs_definitions.h
@@ -14,10 +14,8 @@ const float OUT_OF_RANGE = 1000.0;
const float MAX_RANGE = 100.0;
const int DATA_N_0_INTENSITY = 0;
-
-const int DATA_N_1_Z_GROUND = 1;
-const int DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE = 1;
-
+const int DATA_C_2_MEAN_INTENSITY = 2;
+const int DATA_N_1_INTENSITY_VARIANCE = 1;
const int DATA_N_2_Z_MEAN = 2;
const int DATA_N_3_Z_VARIANCE = 3;
@@ -25,12 +23,9 @@ const int DATA_C_0_RGB_CAST_INTO_FLOAT = 0;
const int DATA_C_1_ID_CLASS = 1;
const int DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL = 2;
-const int DATA_C_2_MEAN_INTENSITY = 2;
const int DATA_C_3_ORIGINAL_INDEX = 3;
-const int DATA_C_2_CAR_PROB = 2; // TODO: remove this
-
const int GRND_REF_DATA_C_0_RGB_CAST_INTO_FLOAT = 0;
const int GRND_REF_DATA_C_1_ROLL = 1;
const int GRND_REF_DATA_C_2_PITCH = 2;
@@ -47,12 +42,12 @@ const int INDEX_UNKNOWN = -1;
const float VERTEX_CONNECTED_TO_ROOT = 0.0;
const float VERTEX_NOT_CONNECTED_TO_ROOT = 100.0;
-const float Z_VARIANCE_UNKNOWN = -1.0;
-const float Z_MEAN_UNKNOWN = -1.0;
+const float UNKNOWN_MEAN = -1000000.0;
+const float UNKOWN_VARIANCE = -1.0;
-const float INTENSITY_UNKNOWN = -1.0;
-const float PROB_UNKNOWN = -1.0;
-const float Z_GROUND_UNKNOWN = -1000000.0;
+const float UNKNOWN_INTENSITY = -1.0;
+const float UNKNOWN_PROB = -1.0;
+const float UNKNOWN_Z_GROUND = -1000000.0;
const int KITTI_NUMBER_OF_PRECISION_RECALL_INTERVALES = 40;
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index 6303bd1..9a05e53 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -462,13 +462,6 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) != CLASS_GROUND)
{
-// std::cout << "Storing z point as z ground" << std::endl;
- // as now the point is classified as ground, we store its own z value as z ground
-
- // this is redundant, so we now use the data_n_1 channel to store intensity variance.
-// elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_n[DATA_N_1_Z_GROUND] =
-// elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].z;
-
// set the class as ground
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_c[DATA_C_1_ID_CLASS] =
(float)CLASS_GROUND + score;
@@ -503,13 +496,6 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL] =
reference_index + score;
- //Deprecated, we now use the data_n_1 channel to store intensity variance
-// // if it is an obstacle point we use the ground reference z prediction as z ground
-// float z_ground = predictZ(ground_reference_cloud_ptr->points[reference_index], point_in_sensor_frame.x,
-// point_in_sensor_frame.y);
-// elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_n[DATA_N_1_Z_GROUND] =
-// z_ground;
-
// set the class to OBSTACLE
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_c[DATA_C_1_ID_CLASS] =
(float)CLASS_OBSTACLE + score; //0.99;
@@ -806,14 +792,14 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
// Filling remaining fields
elevation_cloud_point.data_n[DATA_N_0_INTENSITY] = intensity_mean;
- elevation_cloud_point.data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE] = var_intensity;
+ elevation_cloud_point.data_n[DATA_N_1_INTENSITY_VARIANCE] = var_intensity;
elevation_cloud_point.data_n[DATA_N_2_Z_MEAN] = z_mean;
elevation_cloud_point.data_n[DATA_N_3_Z_VARIANCE] = var;
elevation_cloud_point.data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL] = INDEX_UNKNOWN;
elevation_cloud_point.data_c[DATA_C_3_ORIGINAL_INDEX] = i; //storing the index for later in labelling step
- //std::cout << "Intensity of elevation point AFTER storing it: mean = " << elevation_cloud_point.data_n[DATA_N_0_INTENSITY] << " std = " << sqrt( elevation_cloud_point.data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE]) << std::endl;
+ //std::cout << "Intensity of elevation point AFTER storing it: mean = " << elevation_cloud_point.data_n[DATA_N_0_INTENSITY] << " std = " << sqrt( elevation_cloud_point.data_n[DATA_N_1_INTENSITY_VARIANCE]) << std::endl;
//std::cout << "var z = " << it->data_n[DATA_N_3_Z_VARIANCE] << std::endl;
//std::cout << "elevation_cell_vector[i].z_coordinates.size() = " << z_coordinates.size() << std::endl;
@@ -1028,18 +1014,18 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_2_MEAN_INTENSITY] =
point_in_sensor_frame.data_n[DATA_N_0_INTENSITY];
- pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE] =
- point_in_sensor_frame.data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE];
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_INTENSITY_VARIANCE] =
+ point_in_sensor_frame.data_n[DATA_N_1_INTENSITY_VARIANCE];
// std::cout << "Intensity in elevation cloud: mean = " << point_in_sensor_frame.data_n[DATA_N_0_INTENSITY]
// << " var = "
-// << point_in_sensor_frame.data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE]
+// << point_in_sensor_frame.data_n[DATA_N_1_INTENSITY_VARIANCE]
// << std::endl;
//
// std::cout << "Intensity in pcl cloud: point = "
// << pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_0_INTENSITY] << " mean = "
// << pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_2_MEAN_INTENSITY] << " var = "
-// << pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE] << std::endl;
+// << pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_INTENSITY_VARIANCE] << std::endl;
pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] = point_in_sensor_frame.data_n[DATA_N_2_Z_MEAN];
pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] =
@@ -1061,10 +1047,6 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
}
else
{
- //std::cout << "obstacle z distance from ground = " << pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z << std::endl;
- //pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_Z_GROUND] = point_in_sensor_frame.z;
-
- //std::cout << "obstacle z distance from ground = " << pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z << std::endl;
if ((pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z)
< filtering_configuration.robot_height)
{
@@ -1102,8 +1084,8 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
{
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_2_MEAN_INTENSITY] =
point_in_sensor_frame.data_n[DATA_N_0_INTENSITY];
- pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE] =
- point_in_sensor_frame.data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE];
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_INTENSITY_VARIANCE] =
+ point_in_sensor_frame.data_n[DATA_N_1_INTENSITY_VARIANCE];
pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] =
point_in_sensor_frame.data_n[DATA_N_2_Z_MEAN];
--
GitLab