diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index 2ba5b779b1d3c5b88389f28a790e80594f61a663..654d29fadc7abf6ee5c02f086071cf8731ecc4e2 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -1006,6 +1006,7 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
std::vector<double> point_to_reference_squared_distances;
std::vector<double> prediction_errors;
std::vector<double> scores;
+ std::vector<double> gt_label;
// 2) We point to the vector of indices related to the elevation_cloud point under evaluation
std::vector<std::vector<int>>::const_iterator index_iterator = correspondence_indexes.begin()
@@ -1034,23 +1035,24 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
pcl::PointXYZRGBNormal point_to_evaluate = pcl_cloud_ptr->points[*point_iterator];
features_indexes_in_pcl_cloud_ptr.push_back(*point_iterator); // Index to modify the classification in a later step
+ //std::cout << "Index saved = " << *point_iterator << std::endl;
squared_distances.push_back(
(double)(point_to_evaluate.x * point_to_evaluate.x + point_to_evaluate.y * point_to_evaluate.y
- + point_to_evaluate.z * point_to_evaluate.z));
+ + point_to_evaluate.z * point_to_evaluate.z)); // Feature!
- incidence_angles.push_back(0.0); // TODO: Incidence angle w.r.t the plane defined by the ground reference
- intensities.push_back(point_to_evaluate.data_n[DATA_N_0_INTENSITY]); // Point intensity
+ incidence_angles.push_back(0.0); // TODO: Incidence angle w.r.t the plane defined by the ground reference // Feature!
+ intensities.push_back(point_to_evaluate.data_n[DATA_N_0_INTENSITY]); // Point intensity // Feature!
double delta_x = (point_to_evaluate.x - reference_in_sensor_frame.x);
double delta_y = (point_to_evaluate.y - reference_in_sensor_frame.y);
double delta_z = (point_to_evaluate.z - reference_in_sensor_frame.z);
- point_to_reference_squared_distances.push_back(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z);
+ point_to_reference_squared_distances.push_back(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z); // Feature!
- prediction_errors.push_back((double)euclideanDistance(reference_in_sensor_frame, point_to_evaluate)); // Prediction Error
+ prediction_errors.push_back((double)euclideanDistance(reference_in_sensor_frame, point_to_evaluate)); // Prediction Error // Feature!
- scores.push_back((double)score);
+ scores.push_back((double)score); // Feature!
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_GROUND + score;
}
@@ -1089,38 +1091,73 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
}
}
}
+ int num_of_ground_points = squared_distances.size();
+ //std::cout << "num_of_ground_points = " << num_of_ground_points << std::endl;
- double ratio_ground_points = features_container.size()
- / correspondence_indexes[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].size();
-
- double mean_intensity_ground_points_in_cell = 0.0;
- double std_intensity_ground_points_in_cell = 0.0;
- double mean_prediction_error = 0.0;
- double std_prediction_error = 0.0;
- double mean_squared_error_of_predictions = 0.0;
- double mean_of_scores = 0.0;
- double std_of_scores = 0.0;
-
- 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();
- float var = 0;
- for (int n = 0; n < elevation_cell_vector[i].z_coordinates.size(); ++n)
- var += ((elevation_cell_vector[i].z_coordinates[n] - z_mean)
- * (elevation_cell_vector[i].z_coordinates[n] - z_mean));
-
- var /= (float)elevation_cell_vector[i].z_coordinates.size();
-
- Eigen::VectorXd features(14, 1);
-
- features(06, 0) = ratio_ground_points;
- features(07, 0) = mean_intensity_ground_points_in_cell;
- features(08, 0) = std_intensity_ground_points_in_cell;
- features(09, 0) = mean_prediction_error;
- features(10, 0) = std_prediction_error;
- features(11, 0) = mean_squared_error_of_predictions;
- features(12, 0) = mean_of_scores;
- features(13, 0) = std_of_scores;
+ if (num_of_ground_points > 0)
+ {
+ int total_num_of_points = correspondence_indexes[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].size();
+ //std::cout << "total_num_of_points = " << total_num_of_points << std::endl;
+
+ double ratio_ground_points = (double)num_of_ground_points / (double)total_num_of_points; // Feature!
+ //std::cout << "ratio_ground_points = " << ratio_ground_points << std::endl;
+
+ double sum = std::accumulate(intensities.begin(), intensities.end(), 0.0);
+ double mean_intensity_ground_points_in_cell = sum / (double)num_of_ground_points; // Feature!
+ //std::cout << "mean_intensity_ground_points_in_cell = " << mean_intensity_ground_points_in_cell << std::endl;
+
+ double var_intensity_ground_points_in_cell = 0;
+ for (int n = 0; n < num_of_ground_points; ++n)
+ var_intensity_ground_points_in_cell += ((intensities[n] - mean_intensity_ground_points_in_cell)
+ * (intensities[n] - mean_intensity_ground_points_in_cell));
+
+ var_intensity_ground_points_in_cell /= (double)num_of_ground_points; // Feature!
+ //std::cout << "var_intensity_ground_points_in_cell = " << var_intensity_ground_points_in_cell << std::endl;
+
+ sum = std::accumulate(prediction_errors.begin(), prediction_errors.end(), 0.0);
+ double mean_prediction_error = sum / (double)num_of_ground_points; // Feature!
+ //std::cout << "mean_prediction_error = " << mean_prediction_error << std::endl;
+
+ double var_prediction_error = 0;
+ for (int n = 0; n < num_of_ground_points; ++n)
+ var_prediction_error += ((prediction_errors[n] - mean_prediction_error)
+ * (prediction_errors[n] - mean_prediction_error));
+
+ var_prediction_error /= (double)num_of_ground_points; // Feature!
+ //std::cout << "var_prediction_error = " << var_prediction_error << std::endl;
+
+ sum = std::accumulate(scores.begin(), scores.end(), 0.0);
+ double mean_of_scores = sum / (double)num_of_ground_points; // Feature!
+ //std::cout << "mean_of_scores = " << mean_of_scores << std::endl;
+
+ double var_of_scores = 0;
+ for (int n = 0; n < num_of_ground_points; ++n)
+ var_of_scores += ((scores[n] - mean_of_scores) * (scores[n] - mean_of_scores));
+
+ var_of_scores /= (double)num_of_ground_points; // Feature!
+ //std::cout << "var_of_scores = " << var_of_scores << std::endl;
+
+ int randomIndex = std::rand() % num_of_ground_points; // To save the dataset we will use only one random point of each elevation cloud
+ //std::cout << "randomIndex = " << randomIndex << std::endl;
+
+ Eigen::VectorXd features(13, 1);
+
+ features(0, 0) = squared_distances[randomIndex];
+ features(1, 0) = incidence_angles[randomIndex];
+ features(2, 0) = intensities[randomIndex];
+ features(3, 0) = point_to_reference_squared_distances[randomIndex];
+ features(4, 0) = prediction_errors[randomIndex];
+ features(5, 0) = scores[randomIndex];
+ features(6, 0) = ratio_ground_points;
+ features(7, 0) = mean_intensity_ground_points_in_cell;
+ features(8, 0) = var_intensity_ground_points_in_cell;
+ features(9, 0) = mean_prediction_error;
+ features(10, 0) = var_prediction_error;
+ features(11, 0) = mean_of_scores;
+ features(12, 0) = var_of_scores;
+
+ std::cout << "features = " << std::endl << features << std::endl;
+ }
}
else // if we don't have enough information to try to predict the ground level at this point
{