From d260c86f8261776af620171775d0b627248f3bdf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Iv=C3=A1n=20del=20Pino?= <idelpino@iri.upc.edu>
Date: Tue, 19 Jul 2022 16:17:24 +0200
Subject: [PATCH] added intensity mean and variance in elevation pointcloudf
---
include/structs_definitions.h | 4 ++
src/kf_based_terrain_analysis.cpp | 65 ++++++++++++++++++++++++-------
2 files changed, 56 insertions(+), 13 deletions(-)
diff --git a/include/structs_definitions.h b/include/structs_definitions.h
index 33ab2e6..8a540f6 100644
--- a/include/structs_definitions.h
+++ b/include/structs_definitions.h
@@ -15,6 +15,9 @@ 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_N_2_Z_MEAN = 2;
const int DATA_N_3_Z_VARIANCE = 3;
@@ -173,6 +176,7 @@ struct ElevationCloudCell
int index_of_lowest_point;
std::vector<int> indexes_of_points_in_cell;
std::vector<float> z_coordinates;
+ std::vector<float> intensities;
};
}
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index dc48dc0..e1ed95f 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -369,7 +369,6 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) != OUTLIER)
{
-
float prediction_std_dev = -1.0;
// std::cout << "Computing mahalanobis distance..." << std::endl;
float mahalanobis_distance = mahalanobisDistance(reference_in_sensor_frame, point_in_sensor_frame,
@@ -430,8 +429,10 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
{
// 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
- 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;
+
+ // 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] =
@@ -467,11 +468,12 @@ 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;
- // 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;
+ //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] =
@@ -816,6 +818,8 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
}
//std::cout << "Storing z coordinate and index in the indexes vector" << std::endl;
elevation_cell_vector[elevation_map[row][col]].z_coordinates.push_back(it->z);
+ elevation_cell_vector[elevation_map[row][col]].intensities.push_back(it->data_n[DATA_N_0_INTENSITY]);
+ //std::cout << "Storing intensity value: " << it->data_n[DATA_N_0_INTENSITY] << std::endl;
elevation_cell_vector[elevation_map[row][col]].indexes_of_points_in_cell.push_back(i);
i++;
}
@@ -839,6 +843,7 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
elevation_cloud_point.g = G_CLASS_UNKNOWN;
elevation_cloud_point.b = B_CLASS_UNKNOWN;
}
+ // TODO:
float sum = std::accumulate(elevation_cell_vector[i].z_coordinates.begin(),
elevation_cell_vector[i].z_coordinates.end(), 0.0);
float z_mean = sum / (float)elevation_cell_vector[i].z_coordinates.size();
@@ -849,15 +854,29 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
var /= (float)elevation_cell_vector[i].z_coordinates.size();
+ float sum_intensity = std::accumulate(elevation_cell_vector[i].intensities.begin(),
+ elevation_cell_vector[i].intensities.end(), 0.0);
+ float intensity_mean = sum_intensity / (float)elevation_cell_vector[i].intensities.size();
+ float var_intensity = 0;
+ for (int n = 0; n < elevation_cell_vector[i].intensities.size(); ++n)
+ var_intensity += ((elevation_cell_vector[i].intensities[n] - intensity_mean)
+ * (elevation_cell_vector[i].intensities[n] - intensity_mean));
+
+ var_intensity /= (float)elevation_cell_vector[i].intensities.size();
+
+ //std::cout << "Intensity of elevation point: mean = " << intensity_mean << " std = " << sqrt(var_intensity) << std::endl;
+
// Filling remaining fields
- elevation_cloud_point.data_n[DATA_N_0_INTENSITY] = INTENSITY_UNKNOWN;
- elevation_cloud_point.data_n[DATA_N_1_Z_GROUND] = Z_GROUND_UNKNOWN;
+ 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_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 << "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;
@@ -1049,6 +1068,19 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
//
// std::getchar();
// }
+ // TODO: check this!
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_0_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];
+
+// 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]
+// << std::endl;
+//
+// std::cout << "Intensity in pcl cloud: mean = " << pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_0_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_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] =
@@ -1056,6 +1088,10 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
//sqrt(reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE]) < 0.06) TODO: Could we use this variance to penalize uneven terrain?
float score = 0.995 - (mahalanobis_distance / filtering_configuration.mahalanobis_threshold); // we use 0.995 instead of 1.0 to avoid overflowing the class field
+
+ //if (sqrt(reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE]) > 0.05)
+ // score = 0.0;
+
if (score < 0.0)
score = 0.0;
@@ -1063,7 +1099,7 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
if (score > filtering_configuration.score_threshold)
{
- pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_Z_GROUND] = pcl_cloud_ptr->points[*point_iterator].z;
+ //pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_Z_GROUND] = pcl_cloud_ptr->points[*point_iterator].z;
pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_GROUND;
pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_GROUND;
@@ -1074,7 +1110,7 @@ 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;
+ //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)
@@ -1109,7 +1145,10 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
for (std::vector<int>::const_iterator point_iterator = index_iterator->begin();
point_iterator != index_iterator->end(); ++point_iterator)
{
- pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_Z_GROUND] = point_in_sensor_frame.z;
+ //pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_Z_GROUND] = point_in_sensor_frame.z;
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_0_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_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] =
--
GitLab