From 02b794359673dbd4aec22ea934b8e88a2739e0b9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Iv=C3=A1n=20del=20Pino?= <idelpino@iri.upc.edu>
Date: Fri, 8 Jul 2022 14:17:58 +0200
Subject: [PATCH] added semantic kitti classes
---
include/structs_definitions.h | 20 +++++++++++++++++---
src/kf_based_terrain_analysis.cpp | 26 ++++++++++++++++++--------
2 files changed, 35 insertions(+), 11 deletions(-)
diff --git a/include/structs_definitions.h b/include/structs_definitions.h
index 763c5bb..b8571d4 100644
--- a/include/structs_definitions.h
+++ b/include/structs_definitions.h
@@ -47,18 +47,31 @@ const float PROB_UNKNOWN = -1.0;
const float Z_GROUND_UNKNOWN = -1000000.0;
const int KITTI_NUMBER_OF_PRECISION_RECALL_INTERVALES = 40;
+
+// EVALUATION EXCLUDED CLASSES ///////
const int UNLABELED = 0;
const int OUTLIER = 1;
+const int OTHER_STRUCTURE = 52;
+const int OTHER_OBJECT = 99;
+
+// GROUND CLASSES /////
const int ROAD = 40;
const int PARKING = 44;
const int SIDEWALK = 48;
const int OTHER_GROUND = 49;
-const int OTHER_STRUCTURE = 52;
const int LANE_MARKING = 60;
+const int TERRAIN = 72;
+
+// ORDINARY OBSTACLES //////
+const int BUILDING = 50;
+const int FENCE = 51;
const int VEGETATION = 70;
+const int TRUNK = 71;
+const int POLE = 80;
const int TRAFFIC_SIGN = 81;
-const int OTHER_OBJECT = 99;
-const int TERRAIN = 72;
+
+// Key obstacles like cars, pedestrians and so on are the rest of the classes so we will not explicitly write them here
+
//Custom labels not present in Kitti semantic
const int CLASS_GROUND = 46;
@@ -115,6 +128,7 @@ struct FilteringConfiguration
float robot_height;
float ROI_delta_x_and_y;
+ float ROI_shadow_area;
float ground_reference_search_resolution;
float elevation_grid_resolution;
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index 8e01a98..699be79 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -27,10 +27,18 @@ void CKf_Based_Terrain_Analysis::extractPointsInROI(
//std::cout << "Extracting points in ROI: reference.x = " << reference.x << " reference.y = " << reference.y
// << " reference.z = " << reference.z << std::endl;
- float x_min, x_max, y_min, y_max;
+ float x_min, x_max, y_min, y_max, x_search_range, y_search_range;
- float x_search_range = filtering_configuration.ROI_delta_x_and_y;
- float y_search_range = filtering_configuration.ROI_delta_x_and_y;
+ if (reference.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX] > 0.5)
+ {
+ x_search_range = filtering_configuration.ROI_delta_x_and_y;
+ y_search_range = filtering_configuration.ROI_delta_x_and_y;
+ }
+ else
+ {
+ x_search_range = filtering_configuration.ROI_shadow_area;
+ y_search_range = filtering_configuration.ROI_shadow_area;
+ }
x_min = reference.x - x_search_range;
y_min = reference.y - y_search_range;
@@ -846,7 +854,8 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
std::vector<int> pointIdxKNNSearch(K);
std::vector<float> pointKNNSquaredDistance(K);
- int num_of_references_found = kdtree.nearestKSearch(point_in_sensor_frame, K, pointIdxKNNSearch, pointKNNSquaredDistance);
+ int num_of_references_found = kdtree.nearestKSearch(point_in_sensor_frame, K, pointIdxKNNSearch,
+ pointKNNSquaredDistance);
int best_reference_index = -1;
int best_reference_index_predicting_ground = -1;
@@ -861,9 +870,9 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
pcl::PointXYZRGBNormal reference_in_sensor_frame =
ground_reference_cloud_ptr->points[pointIdxKNNSearch[reference_iterator]];
- if (!filtering_configuration.discard_not_connected_references_for_labelling ||
- ( reference_in_sensor_frame.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT &&
- edges[reference_in_sensor_frame.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].size() > 0))
+ if (!filtering_configuration.discard_not_connected_references_for_labelling
+ || (reference_in_sensor_frame.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT
+ && edges[reference_in_sensor_frame.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].size() > 0))
{
bool observation_z_is_below_prediction = false;
float prediction_std_dev = -1.0;
@@ -917,8 +926,9 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
{
pcl::PointXYZRGBNormal reference_in_sensor_frame =
ground_reference_cloud_ptr->points[pointIdxKNNSearch[best_reference_index]];
- if (elevation_cloud_point_is_ground)
+ if (elevation_cloud_point_is_ground)//&& sqrt(reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE]) < 0.06) //TODO!!!
{
+ //std::cout << "std_dev_z = " << sqrt(reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE]) << std::endl;
// We classify the elevation cloud point as ground
i->data_n[DATA_N_1_Z_GROUND] = i->z;
--
GitLab