Skip to content
Snippets Groups Projects
Commit cd4d5e64 authored by Iván del Pino's avatar Iván del Pino
Browse files

documenting some functions

parent 91deb9ad
No related branches found
No related tags found
No related merge requests found
......@@ -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..."
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment