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

added intensity mean and variance in elevation pointcloudf

parent 2b226128
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/structs_definitions.h
+ 4
− 0
View file @ d260c86f
...@@ -15,6 +15,9 @@ const float MAX_RANGE = 100.0; ...@@ -15,6 +15,9 @@ const float MAX_RANGE = 100.0;
const int DATA_N_0_INTENSITY = 0; const int DATA_N_0_INTENSITY = 0;
const int DATA_N_1_Z_GROUND = 1; 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_2_Z_MEAN = 2;
const int DATA_N_3_Z_VARIANCE = 3; const int DATA_N_3_Z_VARIANCE = 3;
...@@ -173,6 +176,7 @@ struct ElevationCloudCell ...@@ -173,6 +176,7 @@ struct ElevationCloudCell
int index_of_lowest_point; int index_of_lowest_point;
std::vector<int> indexes_of_points_in_cell; std::vector<int> indexes_of_points_in_cell;
std::vector<float> z_coordinates; std::vector<float> z_coordinates;
std::vector<float> intensities;
}; };
} }
......
src/kf_based_terrain_analysis.cpp
+ 52
− 13
View file @ d260c86f
...@@ -369,7 +369,6 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints( ...@@ -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) if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) != OUTLIER)
{ {
float prediction_std_dev = -1.0; 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, float mahalanobis_distance = mahalanobisDistance(reference_in_sensor_frame, point_in_sensor_frame,
...@@ -430,8 +429,10 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints( ...@@ -430,8 +429,10 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
{ {
// 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 // 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 // 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] = 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( ...@@ -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] = 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; reference_index + score;
// if it is an obstacle point we use the ground reference z prediction as z ground //Deprecated, we now use the data_n_1 channel to store intensity variance
float z_ground = predictZ(ground_reference_cloud_ptr->points[reference_index], point_in_sensor_frame.x, // // if it is an obstacle point we use the ground reference z prediction as z ground
point_in_sensor_frame.y); // float z_ground = predictZ(ground_reference_cloud_ptr->points[reference_index], point_in_sensor_frame.x,
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_n[DATA_N_1_Z_GROUND] = // point_in_sensor_frame.y);
z_ground; // 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 // 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] = 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( ...@@ -816,6 +818,8 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
} }
//std::cout << "Storing z coordinate and index in the indexes vector" << std::endl; //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]].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); elevation_cell_vector[elevation_map[row][col]].indexes_of_points_in_cell.push_back(i);
i++; i++;
} }
...@@ -839,6 +843,7 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud( ...@@ -839,6 +843,7 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
elevation_cloud_point.g = G_CLASS_UNKNOWN; elevation_cloud_point.g = G_CLASS_UNKNOWN;
elevation_cloud_point.b = B_CLASS_UNKNOWN; elevation_cloud_point.b = B_CLASS_UNKNOWN;
} }
// TODO:
float sum = std::accumulate(elevation_cell_vector[i].z_coordinates.begin(), float sum = std::accumulate(elevation_cell_vector[i].z_coordinates.begin(),
elevation_cell_vector[i].z_coordinates.end(), 0.0); elevation_cell_vector[i].z_coordinates.end(), 0.0);
float z_mean = sum / (float)elevation_cell_vector[i].z_coordinates.size(); float z_mean = sum / (float)elevation_cell_vector[i].z_coordinates.size();
...@@ -849,15 +854,29 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud( ...@@ -849,15 +854,29 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
var /= (float)elevation_cell_vector[i].z_coordinates.size(); 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 // Filling remaining fields
elevation_cloud_point.data_n[DATA_N_0_INTENSITY] = INTENSITY_UNKNOWN; elevation_cloud_point.data_n[DATA_N_0_INTENSITY] = intensity_mean;
elevation_cloud_point.data_n[DATA_N_1_Z_GROUND] = Z_GROUND_UNKNOWN; 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_2_Z_MEAN] = z_mean;
elevation_cloud_point.data_n[DATA_N_3_Z_VARIANCE] = var; 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_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 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 << "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; //std::cout << "elevation_cell_vector[i].z_coordinates.size() = " << z_coordinates.size() << std::endl;
...@@ -1049,6 +1068,19 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel( ...@@ -1049,6 +1068,19 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// //
// std::getchar(); // 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_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] = pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] =
...@@ -1056,6 +1088,10 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel( ...@@ -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? //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 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) if (score < 0.0)
score = 0.0; score = 0.0;
...@@ -1063,7 +1099,7 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel( ...@@ -1063,7 +1099,7 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
if (score > filtering_configuration.score_threshold) 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].r = R_CLASS_GROUND;
pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_GROUND; pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_GROUND;
...@@ -1074,7 +1110,7 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel( ...@@ -1074,7 +1110,7 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
else else
{ {
//std::cout << "obstacle z distance from ground = " << pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z << std::endl; //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; //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) if ((pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z)
...@@ -1109,7 +1145,10 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel( ...@@ -1109,7 +1145,10 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
for (std::vector<int>::const_iterator point_iterator = index_iterator->begin(); for (std::vector<int>::const_iterator point_iterator = index_iterator->begin();
point_iterator != index_iterator->end(); ++point_iterator) 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] = pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] =
point_in_sensor_frame.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] = pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] =
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment