From cd4d5e64b0a904daf921bdcc9aad8f24b8f1d962 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Iv=C3=A1n=20del=20Pino?= <idelpino@iri.upc.edu>
Date: Thu, 6 Oct 2022 15:21:05 +0200
Subject: [PATCH] documenting some functions
---
src/kf_based_terrain_analysis.cpp | 54 +++++++++++++++++++++++++++----
1 file changed, 48 insertions(+), 6 deletions(-)
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index 78c9e9d..388ea54 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -363,6 +363,23 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr, std::vector<std::vector<int>> &edges,
std::vector<std::vector<kf_based_terrain_analysis_lib::Edge>> &discarded_edges)
{
+ // In this function we will check by Mahalanobis distance test (using the prior) each elevation_cloud point in the ROI of the reference being
+ // evaluated. Those who pass, are ground points, and will be used to update the prior. Those who pass and that are not yet labeled (have
+ // the class value as UNKNOWN)are also used to drive the exploration, being selected to create new references (vertices in the graph)
+ //
+ // Points classified as GROUND are changed to GROUND independently of their previous class (because points can be seen as obstacles from
+ // a POV, but seen as ground from another). Points classified as OBSTACLE are changed to OBSTACLE only if they were not of GROUND class.
+ // Either way, if a change of class is produced or if the new classification has a better score than the previous one, we mark the current reference
+ // (vertex in the graph) as the one which made the label. This is done to avoid searching during the whole point cloud labeling.
+ //
+ // After this, we use the GROUND classified points in the elevation cloud to update the prior
+ // and store the posterior in the graph
+ //
+ // Once we get the posterior, we use the points that were UNKNOWN at the beggining of this function to generate new references (vertices)
+ // then, we analize the vertices in the ROI and make edges to those that are seen as ground from the currently processed vertex. The CONNECTED
+ // _TO_ROOT flag is set in those to which we connect (it is unset by default)
+
+
//std::cout << "Starting labelLocalGroundPoints!!" << std::endl;
// if (filtering_configuration.measure_performance)
// CFunctionMonitor performance_monitor("labelLocalGroundPoints", performance_supervisor_ptr_);
@@ -657,6 +674,7 @@ void CKf_Based_Terrain_Analysis::labelGroundPoints(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr references_in_ROI(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
//std::cout << "Extracting points in ROI..." << std::endl;
+ // We extract points from the elevation_cloud in the Region of Interest of the reference currently being processed
extractPointsInROI(lidar_configuration, filtering_configuration, ground_reference_cloud_ptr->points[reference_index],
elevation_cloud_ptr, points_in_ROI);
@@ -793,12 +811,22 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud, std::vector<std::vector<int>> &correspondence_indexes)
{
+ // In this function we want to create a reduced point cloud using a grid. Each point in the reduced point cloud
+ // will contain the lowest point of its grid cell, the values of z mean and variance, intensity mean and variance,
+ // and a vector with the indices of the rest of points in the cell w.r.t. the original point cloud, also it will
+ // be colored as OUTLIER or UNKNOWN (since ground obstacle classification is done later on the algorithm)
+
//if (filtering_configuration.measure_performance)
//CFunctionMonitor performance_monitor("generateElevationCloud", performance_supervisor_ptr_);
+ // We will use a integer matrix with sufficient size to discretize the whole point cloud (2*MAX_RANGE/resolution)^2
+ // This matrix is required to save memory, because it contains only one integer by cell, while the structure required
+ // to store the actual data is far more complex (see the struct ElevationCloudCell), so we only create ElevationCloudCell
+ // objects in cells that actually contain data (which are a lot less than the size of the grid)
int number_of_rows_and_columns = std::ceil(2.0 * MAX_RANGE / filtering_configuration.elevation_grid_resolution);
//std::cout << "number_of_rows_and_columns = " << number_of_rows_and_columns << std::endl;
+ // We initialize the grid to unknown values
int elevation_map[number_of_rows_and_columns][number_of_rows_and_columns];
for (int i = 0; i < number_of_rows_and_columns; ++i)
for (int j = 0; j < number_of_rows_and_columns; ++j)
@@ -864,7 +892,7 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
elevation_cloud_point.g = G_CLASS_UNKNOWN;
elevation_cloud_point.b = B_CLASS_UNKNOWN;
}
- // TODO:
+ // TODO: check this!
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();
@@ -919,6 +947,16 @@ void CKf_Based_Terrain_Analysis::generateRootVertex(
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr)
{
+ // In this function we generate the root vertex using the z value as -1.0 * LiDAR height, the slope values are set to 0.0
+ // since we assume that the sensor is normal to the plane in which it lies. The uncertainties (sigma_z, sigma_roll, sigma_pitch)
+ // are set by parameter (roll means not the angle, but the tangent of the angle, in other words the slope in y axis, and pitch
+ // is the slope in x axis).
+ // We also set the flag "VERTEX_CONNECTED_TO_ROOT" to true, because the root is obviously connected to root! --> this flag will be passed
+ // through the graph building process in such a way that once it is built we can avoid to search paths to root using nodes that we already know
+ // that are not connected with the root (the root position is same of the sensor but at ground level, so not connected to root means the same
+ // as unreachable for the robot
+
+
//std::cout << "Generating root vertex" << std::endl;
// The first reference point (root vertex) is the ground in (0,0) approximately known by sensor extrinsic calibration
pcl::PointXYZRGBNormal root_vertex;
@@ -948,11 +986,10 @@ void CKf_Based_Terrain_Analysis::generateRootVertex(
root_vertex.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] = VERTEX_CONNECTED_TO_ROOT;
ground_reference_cloud_ptr->points.push_back(root_vertex);
- Eigen::Matrix3d covariance_matrix;
- covariance_matrix << filtering_configuration.z_initial_std_dev * filtering_configuration.z_initial_std_dev, 0.0, 0.0, 0.0, filtering_configuration.pitch_initial_std_dev
- * filtering_configuration.pitch_initial_std_dev, 0.0, 0.0, 0.0, filtering_configuration.roll_initial_std_dev
- * filtering_configuration.roll_initial_std_dev;
-
+// Eigen::Matrix3d covariance_matrix;
+// covariance_matrix << filtering_configuration.z_initial_std_dev * filtering_configuration.z_initial_std_dev, 0.0, 0.0, 0.0, filtering_configuration.pitch_initial_std_dev
+// * filtering_configuration.pitch_initial_std_dev, 0.0, 0.0, 0.0, filtering_configuration.roll_initial_std_dev
+// * filtering_configuration.roll_initial_std_dev;
// references_covariance_matrix_vector_.push_back(covariance_matrix);
}
@@ -1062,6 +1099,11 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr,
const std::vector<std::vector<int>> &edges, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr)
{
+ // In this function, we iterate over all points in the elevation cloud (the grid-based reduction of the original point cloud)
+ // for each elevation cloud point, we extract the best reference (encoded in the integer part of the float in the field
+ // DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL), and then using the indexes vector we check the mahalanobis distance to every
+ // point represented by the elevation point and set the score.
+
//if (filtering_configuration.measure_performance)
//CFunctionMonitor performance_monitor("labelPointcloudUsingGroundModel", performance_supervisor_ptr_);
//std::cout << "Labeling points in the original pointcloud using the posterior and the ROI indexes vectors..."
--
GitLab