diff --git a/include/kf_based_terrain_analysis.h b/include/kf_based_terrain_analysis.h
index 6a1f953092d3262ecbe1ee1f9b54d5e89b34c891..4903f87092e6b98dc0dbcb950a32ead846c476d7 100644
--- a/include/kf_based_terrain_analysis.h
+++ b/include/kf_based_terrain_analysis.h
@@ -49,6 +49,11 @@ private:
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud,
std::vector<std::vector<int>> &correspondence_indexes);
+ void fastGenerateElevationCloud(const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr,
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud,
+ std::vector<std::vector<int>> &correspondence_indexes);
+
void labelGroundPoints(const kf_based_terrain_analysis_lib::SensorConfiguration lidar_configuration,
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
const std::vector<std::vector<int> > &correspondence_indexes, const int reference_index,
@@ -84,16 +89,14 @@ private:
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr,
const std::vector<std::vector<int> > &correspondence_indexes,
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);
+ const std::vector<std::vector<int>> &edges, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr);
void fastLabelPointcloudUsingGroundModel(
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr,
const std::vector<std::vector<int> > &correspondence_indexes,
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);
+ const std::vector<std::vector<int>> &edges, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr);
void ensureThatConnectionsAreReflectedInBothEnds(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr,
diff --git a/include/structs_definitions.h b/include/structs_definitions.h
index 00feb6e2fe1df3fc9c68e878aa3766dd1b8c626d..6c24cb040b60caa012904294f0a793434a3d9279 100644
--- a/include/structs_definitions.h
+++ b/include/structs_definitions.h
@@ -166,5 +166,13 @@ struct Edge
int edge_class;
};
+struct ElevationCloudCell
+{
+ pcl::PointXYZRGBNormal lowest_point_in_cell;
+ int index_of_lowest_point;
+ std::vector<int> indexes_of_points_in_cell;
+ std::vector<float> z_coordinates;
+};
+
}
#endif /* INCLUDE_STRUCTS_DEFINITIONS_H_ */
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index 66baf5323daf6d2bf5f0433fcee5be78abdb40e7..978c8635a5c520503d41a3e432305a2bb29b9417 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -314,9 +314,9 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
filtering_configuration.propagation_additive_noise,
filtering_configuration.propagation_z_additive_noise);
- //std::cout << "Reference_index = " << reference_index << std::endl;
- //std::cout << "Using reference with ID = "
- // << ground_reference_cloud_ptr->points[reference_index].data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX] << std::endl;
+// std::cout << "Reference_index = " << reference_index << std::endl;
+// std::cout << "Using reference with ID = "
+// << ground_reference_cloud_ptr->points[reference_index].data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX] << std::endl;
pcl::PointXYZRGBNormal reference_in_sensor_frame = ground_reference_cloud_ptr->points[reference_index];
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr observations_for_tangent_plane_estimation(
@@ -324,17 +324,17 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
// points in ROI come from elevation pointcloud, which is a reduction of the original
// pointcloud (we have stored the indexes to be able to label the original pointcloud)
- //std::cout << "Extracting observations for local tangent plane estimation..." << std::endl;
+// std::cout << "Extracting observations for local tangent plane estimation..." << std::endl;
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = points_in_ROI->begin(); i != points_in_ROI->end(); ++i)
{
pcl::PointXYZRGBNormal point_in_sensor_frame = *i;
float prediction_std_dev = -1.0;
- //std::cout << "Computing mahalanobis distance..." << std::endl;
+// std::cout << "Computing mahalanobis distance..." << std::endl;
float mahalanobis_distance = mahalanobisDistance(reference_in_sensor_frame, point_in_sensor_frame,
prediction_std_dev);
- //std::cout << "Mahalanobis distance computed!" << std::endl;
+// std::cout << "Mahalanobis distance computed!" << std::endl;
if (std::isnan(mahalanobis_distance))
{
std::cout << "prior pred sigma = " << prediction_std_dev << " uncertainty = "
@@ -350,17 +350,17 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
// << " z = " << point_in_sensor_frame.z << std::endl << "mahalanobis_distance = " << mahalanobis_distance
// << " mahalanobis_threshold = " << filtering_configuration.mahalanobis_threshold << std::endl;
- //std::cout << "Inserting observation for plane estimation!" << std::endl;
+// std::cout << "Inserting observation for plane estimation!" << std::endl;
observations_for_tangent_plane_estimation->push_back(point_in_sensor_frame); // independently of exploration, any ground point is
// used for plane estimation, to no discard useful
// information!
if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) == CLASS_UNKNOWN) // we use unknown points to drive the exploration
{
- //std::cout << "Inserting observation to drive exploration! --> original index = "
- // << point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX] << std::endl;
+// std::cout << "Inserting observation to drive exploration! --> original index = "
+// << point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX] << std::endl;
- //std::cout << "elevation_cloud_ptr->points.size() = " << elevation_cloud_ptr->points.size() << std::endl;
+// std::cout << "elevation_cloud_ptr->points.size() = " << elevation_cloud_ptr->points.size() << std::endl;
new_references_creator.addGroundObservationInSensorFrame(
reference_in_sensor_frame,
@@ -375,7 +375,7 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
float index_of_labelling_ref =
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];
- //std::cout << "Updating elevation cloud point to represent ground data" << std::endl;
+// std::cout << "Updating elevation cloud point to represent ground data" << std::endl;
if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) != CLASS_GROUND
|| std::floor((int)index_of_labelling_ref) == INDEX_UNKNOWN
|| index_of_labelling_ref - std::floor((int)index_of_labelling_ref) < score) // if the point was seen as obstacle from another POV
@@ -387,7 +387,7 @@ 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;
+// 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;
@@ -405,9 +405,9 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
}
else
{
- //std::cout << "Obstacle point found! x = " << point_in_sensor_frame.x << " y = " << point_in_sensor_frame.y
- // << " z = " << point_in_sensor_frame.z << std::endl << "mahalanobis_distance = " << mahalanobis_distance
- // << " mahalanobis_threshold = " << filtering_configuration.mahalanobis_threshold << std::endl;
+// std::cout << "Obstacle point found! x = " << point_in_sensor_frame.x << " y = " << point_in_sensor_frame.y
+// << " z = " << point_in_sensor_frame.z << std::endl << "mahalanobis_distance = " << mahalanobis_distance
+// << " mahalanobis_threshold = " << filtering_configuration.mahalanobis_threshold << std::endl;
if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) != CLASS_GROUND) // we don't want to convert ground points to obstacles
// because of the POV (one thing can be seem as obstacle or ground depending on the POV)
@@ -443,7 +443,7 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
}
}
}
- //std::cout << "Observations for local tangent plane estimation extracted!" << std::endl;
+// std::cout << "Observations for local tangent plane estimation extracted!" << std::endl;
//std::cout << "Reference Prior = " << std::endl;
//std::cout << "x = " << ground_reference_cloud_ptr->points[reference_index].x << " y = "
@@ -460,10 +460,10 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
// << sqrt(ground_reference_cloud_ptr->points[reference_index].data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE]) * 180.0
// / M_PI << " )" << std::endl;
- //std::cout << "Estimating tangent plane..." << std::endl;
+// std::cout << "Estimating tangent plane..." << std::endl;
estimateTangentPlane(observations_for_tangent_plane_estimation, (double)filtering_configuration.z_observation_std_dev,
reference_in_sensor_frame);
- //std::cout << "Posterior distribution obtained!" << std::endl;
+// std::cout << "Posterior distribution obtained!" << std::endl;
ground_reference_cloud_ptr->points[reference_index] = reference_in_sensor_frame;
@@ -482,17 +482,17 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
// << sqrt(ground_reference_cloud_ptr->points[reference_index].data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE]) * 180.0
// / M_PI << " )" << std::endl;
- //std::cout << "Creating new references..." << std::endl;
+// std::cout << "Creating new references..." << std::endl;
new_references_creator.generateNewGroundReferences(reference_index, reference_in_sensor_frame,
ground_reference_cloud_ptr, edges);
- //std::cout << "New references created!" << std::endl;
+// std::cout << "New references created!" << std::endl;
- //std::cout << "Search for references in the ROI..." << std::endl;
+// std::cout << "Search for references in the ROI..." << std::endl;
extractPointsInROI(lidar_configuration, filtering_configuration, ground_reference_cloud_ptr->points[reference_index],
ground_reference_cloud_ptr, references_in_ROI);
- //std::cout << "Search concluded! Number of references in ROI = " << references_in_ROI->size() << std::endl;
+// std::cout << "Search concluded! Number of references in ROI = " << references_in_ROI->size() << std::endl;
- //std::cout << "Making edges to existing references in the ROI... " << std::endl;
+// std::cout << "Making edges to existing references in the ROI... " << std::endl;
int num_of_discarded_edges = 0;
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = references_in_ROI->begin(); i != references_in_ROI->end();
++i)
@@ -531,7 +531,7 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
std::getchar();
}
- //std::cout << "Checking connections..." << std::endl;
+// std::cout << "Checking connections..." << std::endl;
bool connect = true;
int edge_class = CLASS_EDGE_ACCEPTED;
@@ -571,11 +571,9 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
// << reference_in_ROI.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX] << std::endl;
}
}
- //std::cout << "Total edges evaluated = " << references_in_ROI->points.size() << " discarded = "
- // << num_of_discarded_edges << std::endl;
+// std::cout << "Total edges evaluated = " << references_in_ROI->points.size() << " discarded = "
+// << num_of_discarded_edges << std::endl;
//getchar();
-// new_references_creator.generateNewGroundReferences(reference_index, reference_in_sensor_frame,
-// ground_reference_cloud_ptr, edges);
}
void CKf_Based_Terrain_Analysis::labelGroundPoints(
@@ -609,7 +607,7 @@ void CKf_Based_Terrain_Analysis::labelGroundPoints(
labelLocalGroundPoints(lidar_configuration, filtering_configuration, correspondence_indexes, reference_index,
points_in_ROI, references_in_ROI, ground_reference_cloud_ptr, elevation_cloud_ptr, cloud_ptr,
edges, discarded_edges);
- //std::cout << "Local ground points labeled!" << std::endl;
+// std::cout << "Local ground points labeled!" << std::endl;
// std::cout << "Number of edges after labeling = " << edges[reference_index].size() << std::endl;
// getchar();
@@ -725,6 +723,108 @@ void CKf_Based_Terrain_Analysis::generateElevationCloud(
}
}
+void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
+ const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr,
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud, std::vector<std::vector<int>> &correspondence_indexes)
+{
+ //if (filtering_configuration.measure_performance)
+ //CFunctionMonitor performance_monitor("generateElevationCloud", performance_supervisor_ptr_);
+
+ 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;
+
+ kf_based_terrain_analysis_lib::ElevationCloudCell elevation_map[number_of_rows_and_columns][number_of_rows_and_columns];
+ //std::cout << "elevation_map created! " << std::endl;
+
+ int i = 0;
+ //std::cout << "Starting to fill the elevation_map! " << std::endl;
+ for (pcl::PointCloud<pcl::PointXYZRGBNormal>::const_iterator it = pcl_cloud_ptr->begin(); it != pcl_cloud_ptr->end();
+ ++it)
+ {
+ int row = std::round((number_of_rows_and_columns / 2.0) + (it->x / filtering_configuration.elevation_grid_resolution));
+ int col = std::round((number_of_rows_and_columns / 2.0) + (it->y / filtering_configuration.elevation_grid_resolution));
+
+ if(elevation_map[row][col].indexes_of_points_in_cell.empty())
+ {
+ //std::cout << "Pointing to an empty cell, storing current point as lowest, and keeping its index! " << std::endl;
+ //std::cout << "Point number " << i << " row = " << row << " col = " << col << std::endl;
+ elevation_map[row][col].lowest_point_in_cell = *it;
+ elevation_map[row][col].index_of_lowest_point = i;
+ }
+ else
+ {
+ //std::cout << "Pointing to a NON-empty cell! checking z!" << std::endl;
+ if(it->z < elevation_map[row][col].lowest_point_in_cell.z)
+ {
+ //std::cout << "Current point is the lowest, storing it as it is!" << std::endl;
+ elevation_map[row][col].lowest_point_in_cell = *it;
+ elevation_map[row][col].index_of_lowest_point = i;
+ }
+ }
+ //std::cout << "Storing z coordinate and index in the indexes vector" << std::endl;
+ elevation_map[row][col].z_coordinates.push_back(it->z);
+ elevation_map[row][col].indexes_of_points_in_cell.push_back(i);
+ i++;
+ }
+
+ //std::cout << "Now generating the elevation cloud from the elevation map!" << std::endl;
+ //DEBUG
+ //int total_number_of_points_stored = 0;
+ int index_respect_elevation_cloud = 0;
+ for(int row = 0; row < number_of_rows_and_columns; ++row)
+ {
+ for(int col = 0; col < number_of_rows_and_columns; ++col)
+ {
+ if(!elevation_map[row][col].indexes_of_points_in_cell.empty())
+ {
+ pcl::PointXYZRGBNormal elevation_cloud_point = elevation_map[row][col].lowest_point_in_cell;
+ if (elevation_cloud_point.data_c[DATA_C_1_ID_CLASS] == OUTLIER)
+ {
+ elevation_cloud_point.r = R_CLASS_OUTLIER;
+ elevation_cloud_point.g = G_CLASS_OUTLIER;
+ elevation_cloud_point.b = B_CLASS_OUTLIER;
+ }
+ else
+ {
+ elevation_cloud_point.r = R_CLASS_UNKNOWN;
+ elevation_cloud_point.g = G_CLASS_UNKNOWN;
+ elevation_cloud_point.b = B_CLASS_UNKNOWN;
+ }
+ float sum = std::accumulate(elevation_map[row][col].z_coordinates.begin(), elevation_map[row][col].z_coordinates.end(), 0.0);
+ float z_mean = sum / (float)elevation_map[row][col].z_coordinates.size();
+ float var = 0;
+ for (int n = 0; n < elevation_map[row][col].z_coordinates.size(); ++n)
+ var += ((elevation_map[row][col].z_coordinates[n] - z_mean) * (elevation_map[row][col].z_coordinates[n] - z_mean));
+
+ var /= (float)elevation_map[row][col].z_coordinates.size();
+
+ // 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_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] = index_respect_elevation_cloud++; //storing the index for later in labelling step
+
+ //std::cout << "var z = " << it->data_n[DATA_N_3_Z_VARIANCE] << std::endl;
+ //std::cout << "z_coordinates.size() = " << z_coordinates.size() << std::endl;
+
+ //DEBUG!
+ //total_number_of_points_stored += elevation_map[row][col].indexes_of_points_in_cell.size();
+
+ correspondence_indexes.push_back(elevation_map[row][col].indexes_of_points_in_cell);
+ //std::cout << "Storing point into elevation cloud!" << std::endl;
+ elevation_cloud->push_back(elevation_cloud_point);
+ }
+ }
+ }
+// std::cout << "elevation_cloud generated!!" << std::endl;
+// //std::cout << "total_number_of_points_stored = " << total_number_of_points_stored << std::endl;
+// //std::cout << "original number of points = " << pcl_cloud_ptr->points.size() << std::endl;
+}
+
void CKf_Based_Terrain_Analysis::generateRootVertex(
const kf_based_terrain_analysis_lib::SensorConfiguration lidar_configuration,
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
@@ -1448,11 +1548,17 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
std::vector < std::vector<int> > correspondence_indexes;
- generateElevationCloud(filtering_configuration, pcl_cloud_ptr, elevation_cloud_ptr, correspondence_indexes);
+ fastGenerateElevationCloud(filtering_configuration, pcl_cloud_ptr, elevation_cloud_ptr, correspondence_indexes);
//std::cout << "Elevation cloud generated! Npoints = " << elevation_cloud_ptr->points.size() << std::endl;
+// pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr fast_elevation_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+// std::vector < std::vector<int> > fast_correspondence_indexes;
+//
+// fastGenerateElevationCloud(filtering_configuration, pcl_cloud_ptr, fast_elevation_cloud_ptr, fast_correspondence_indexes);
+// std::cout << "Fast Elevation cloud generated! Npoints = " << fast_elevation_cloud_ptr->points.size() << std::endl;
+
generateRootVertex(lidar_configuration, filtering_configuration, ground_reference_cloud_ptr);
- //std::cout << "Root vertex generated! Num of points = " << ground_reference_cloud_ptr->points.size() << std::endl
+ //std::cout << "Root vertex generated! Num of points = " << ground_reference_cloud_ptr->points.size() << std::endl;
// << "Number of edges = " << edges[0].size() << std::endl;
//getchar();