diff --git a/include/kf_based_terrain_analysis.h b/include/kf_based_terrain_analysis.h
index fce5ce841c60bb413d2e2e31a665a2d9cb122162..03c9f65df3e1621704c8b7d21bfd4d43ff62b30f 100644
--- a/include/kf_based_terrain_analysis.h
+++ b/include/kf_based_terrain_analysis.h
@@ -21,6 +21,10 @@ class CKf_Based_Terrain_Analysis
private:
CTimeSupervisor::Ptr performance_supervisor_ptr_;
+ bool use_full_covariance_matrices_;
+ std::vector<Eigen::Matrix3d> references_covariance_matrix_vector_;
+
+
float predictZ(const pcl::PointXYZRGBNormal reference, const float delta_x, const float delta_y);
float computeLikelihood(const pcl::PointXYZRGBNormal reference_in_sensor_frame,
diff --git a/include/observation_downsampler.h b/include/observation_downsampler.h
index 824334f66f9abdbeec2ddfb22e12852a9b9157ae..3ec2fb88dbb2cd7f9294f244df7e231ff205e72d 100644
--- a/include/observation_downsampler.h
+++ b/include/observation_downsampler.h
@@ -40,6 +40,7 @@ public:
void generateNewGroundReferences(const int reference_index, const pcl::PointXYZRGBNormal& reference,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr,
+ std::vector<Eigen::Matrix3d> & references_covariance_matrix_vector,
std::vector<std::vector<int>>& edges);
void predictZWithVariance(const pcl::PointXYZRGBNormal reference, const float x, const float y, float& z_predicted,
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index e1ed95f45ea2ef33a748363de9da54903bc599c9..8772ab3c673291b89db9743ec346333601308ceb 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -3,6 +3,7 @@
CKf_Based_Terrain_Analysis::CKf_Based_Terrain_Analysis()
{
performance_supervisor_ptr_ = boost::make_shared<CTimeSupervisor>();
+ use_full_covariance_matrices_ = false; // TODO: Extract parameter!!
}
CKf_Based_Terrain_Analysis::~CKf_Based_Terrain_Analysis()
@@ -172,25 +173,40 @@ float CKf_Based_Terrain_Analysis::mahalanobisDistance(const pcl::PointXYZRGBNorm
const pcl::PointXYZRGBNormal point_in_sensor_frame)
{
//std::cout << "<------- Exectuting CKf_Based_Terrain_Analysis::mahalanobisDistance... --------->" << std::endl;
-
float prediction_fast = reference_in_sensor_frame.z
+ (point_in_sensor_frame.x - reference_in_sensor_frame.x)
* reference_in_sensor_frame.data_c[GRND_REF_DATA_C_2_PITCH]
+ (point_in_sensor_frame.y - reference_in_sensor_frame.y)
* reference_in_sensor_frame.data_c[GRND_REF_DATA_C_1_ROLL];
- float prediction_std_fast = sqrt(
- reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE]
- + (point_in_sensor_frame.x - reference_in_sensor_frame.x)
- * (point_in_sensor_frame.x - reference_in_sensor_frame.x)
- * reference_in_sensor_frame.data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE]
- + (point_in_sensor_frame.y - reference_in_sensor_frame.y)
- * (point_in_sensor_frame.y - reference_in_sensor_frame.y)
- * reference_in_sensor_frame.data_n[GRND_REF_DATA_N_0_ROLL_VARIANCE]);
+ float prediction_std_fast = -1.0;
+
+ if (!use_full_covariance_matrices_)
+ {
+ prediction_std_fast = sqrt(
+ reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE]
+ + (point_in_sensor_frame.x - reference_in_sensor_frame.x)
+ * (point_in_sensor_frame.x - reference_in_sensor_frame.x)
+ * reference_in_sensor_frame.data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE]
+ + (point_in_sensor_frame.y - reference_in_sensor_frame.y)
+ * (point_in_sensor_frame.y - reference_in_sensor_frame.y)
+ * reference_in_sensor_frame.data_n[GRND_REF_DATA_N_0_ROLL_VARIANCE]);
+ }
+ else
+ {
+ Eigen::Vector3d partial_derivatives;
+ partial_derivatives(0) = 1.0;
+ partial_derivatives(1) = (point_in_sensor_frame.y - reference_in_sensor_frame.y);
+ partial_derivatives(2) = (point_in_sensor_frame.x - reference_in_sensor_frame.x);
+
+ prediction_std_fast = partial_derivatives.transpose()
+ * references_covariance_matrix_vector_[reference_in_sensor_frame.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]]
+ * partial_derivatives;
+ }
float mahalanobis_distance_fast = fabs(point_in_sensor_frame.z - prediction_fast) / prediction_std_fast;
- if (std::isnan(mahalanobis_distance_fast))
+ if (std::isnan (mahalanobis_distance_fast))
{
std::cout << "ref (x,y,z) = " << reference_in_sensor_frame.x << ", " << reference_in_sensor_frame.y << ", "
<< reference_in_sensor_frame.z << std::endl;
@@ -203,6 +219,7 @@ float CKf_Based_Terrain_Analysis::mahalanobisDistance(const pcl::PointXYZRGBNorm
<< reference_in_sensor_frame.data_c[GRND_REF_DATA_C_1_ROLL] << ", "
<< reference_in_sensor_frame.data_n[GRND_REF_DATA_N_0_ROLL_VARIANCE] << std::endl;
}
+
return (mahalanobis_distance_fast);
}
@@ -328,6 +345,8 @@ void CKf_Based_Terrain_Analysis::estimateTangentPlane(
reference_in_sensor_frame.data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE] = (float)P(1, 1);
reference_in_sensor_frame.data_n[GRND_REF_DATA_N_0_ROLL_VARIANCE] = (float)P(2, 2);
+// references_covariance_matrix_vector_[reference_in_sensor_frame.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]] = P;
+
//std::cout << "x posterior = " << std::endl << x << std::endl;
//std::cout << "P posterior = " << std::endl << P << std::endl;
//getchar();
@@ -528,7 +547,8 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
// std::cout << "Creating new references..." << std::endl;
new_references_creator.generateNewGroundReferences(reference_index, reference_in_sensor_frame,
- ground_reference_cloud_ptr, edges);
+ ground_reference_cloud_ptr, references_covariance_matrix_vector_,
+ edges);
// std::cout << "New references created!" << std::endl;
// std::cout << "Search for references in the ROI..." << std::endl;
@@ -563,6 +583,7 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
// std::cout << "Analizing vertex in ROI with id = " << reference_in_ROI.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]
// << std::endl;
+ // TODO: check if it would be convenient to use the whole cov matrix for this:
float prediction_std_dev = -1.0;
float mahalanobis_distance = mahalanobisDistance(reference_in_sensor_frame, reference_in_ROI, prediction_std_dev);
@@ -855,7 +876,7 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
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);
+ 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)
@@ -926,6 +947,13 @@ 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;
+
+// references_covariance_matrix_vector_.push_back(covariance_matrix);
}
void CKf_Based_Terrain_Analysis::createDenseGroundCloud(
@@ -1069,8 +1097,10 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// 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];
+ 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 = "
@@ -1146,8 +1176,10 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
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_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_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] =
point_in_sensor_frame.data_n[DATA_N_2_Z_MEAN];
@@ -1596,7 +1628,8 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
// std::cout << "reference_index = " << reference_index << std::endl;
// std::cout << "number of references = " << ground_reference_cloud_ptr->points.size() << std::endl;
}
- //std::cout << "Total number of references = " << ground_reference_cloud_ptr->points.size() << std::endl;
+// std::cout << "Total number of references = " << ground_reference_cloud_ptr->points.size() << std::endl;
+// std::cout << "Total number of covariance matrices = " << references_covariance_matrix_vector_.size() << std::endl;
//getchar();
// We can discard ground references if they are not connected with any other, to test this, we need
@@ -1614,9 +1647,9 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
fastLabelPointcloudUsingGroundModel(filtering_configuration, elevation_cloud_ptr, correspondence_indexes,
ground_reference_cloud_ptr, edges, pcl_cloud_ptr);
}
+
// Dense reconstruction aims to serve as debugging or visualizing tool, it generates an XY regular grid with predicted
// Z values using the closest reference point
-
if (filtering_configuration.show_dense_reconstruction)
{
//std::cout << "Ready to create dense reconstruction" << std::endl;
@@ -1638,6 +1671,8 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
pcl_cloud.clear();
pcl_cloud = *pcl_cloud_ptr;
+// references_covariance_matrix_vector_.clear();
+
if (filtering_configuration.measure_performance)
performance_supervisor_ptr_->printStatistics();
}
diff --git a/src/observation_downsampler.cpp b/src/observation_downsampler.cpp
index 679a8c43882c7f45929a6642079496eb3c8ae61c..50c81bdba531c6849c87fd9cf5746a5a02167f74 100644
--- a/src/observation_downsampler.cpp
+++ b/src/observation_downsampler.cpp
@@ -7,13 +7,13 @@
#include "../include/observation_downsampler.h"
-CObservationDownsampler::CObservationDownsampler(float angular_resolution, float propagation_additive_noise, float propagation_z_additive_noise)
+CObservationDownsampler::CObservationDownsampler(float angular_resolution, float propagation_additive_noise,
+ float propagation_z_additive_noise)
{
angular_resolution_ = angular_resolution;
propagation_additive_noise_ = propagation_additive_noise;
propagation_z_additive_noise_ = propagation_z_additive_noise;
-
const int NUMBER_OF_REGIONS = round(2.0 * M_PI / angular_resolution_);
//std::cout << "NUMBER_OF_REGIONS = " << NUMBER_OF_REGIONS << std::endl;
@@ -39,7 +39,7 @@ CObservationDownsampler::~CObservationDownsampler()
}
void CObservationDownsampler::predictZWithVariance(const pcl::PointXYZRGBNormal reference, const float x, const float y,
- float& z_predicted, float& var_z_predicted)
+ float &z_predicted, float &var_z_predicted)
{
z_predicted = reference.z + (x - reference.x) * reference.data_c[GRND_REF_DATA_C_2_PITCH]
+ (y - reference.y) * reference.data_c[GRND_REF_DATA_C_1_ROLL];
@@ -69,7 +69,7 @@ void CObservationDownsampler::addGroundObservationInSensorFrame(
azimuth_angle += 2.0 * M_PI;
if (azimuth_angle >= 2.0 * M_PI)
- azimuth_angle -= 2.0 * M_PI;
+ azimuth_angle -= 2.0 * M_PI;
int index = floor(azimuth_angle / angular_resolution_);
@@ -77,9 +77,11 @@ void CObservationDownsampler::addGroundObservationInSensorFrame(
const int NUMBER_OF_REGIONS = round(2.0 * M_PI / angular_resolution_);
if (index == NUMBER_OF_REGIONS)
{
- std::cout << "CObservationDownsampler::addGroundObservationInSensorFrame --> found a point escaping the angular wrapping!" << std::endl;
+ std::cout
+ << "CObservationDownsampler::addGroundObservationInSensorFrame --> found a point escaping the angular wrapping!"
+ << std::endl;
std::cout << "azimuth_angle = " << azimuth_angle * 180.0 / M_PI << std::endl;
- std::cout << "angular_resolution = " << angular_resolution_ * 180.0 / M_PI << std::endl;
+ std::cout << "angular_resolution = " << angular_resolution_ * 180.0 / M_PI << std::endl;
std::cout << "INDEX circular exploration = " << index << std::endl;
index = 0;
@@ -93,9 +95,9 @@ void CObservationDownsampler::addGroundObservationInSensorFrame(
}
void CObservationDownsampler::generateNewGroundReferences(
- const int reference_index, const pcl::PointXYZRGBNormal& reference,
+ const int reference_index, const pcl::PointXYZRGBNormal &reference,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr,
- std::vector<std::vector<int>>& edges)
+ std::vector<Eigen::Matrix3d> &references_covariance_matrix_vector, std::vector<std::vector<int>> &edges)
{
//std::cout << "Generating new vertices!" << std::endl;
const int NUMBER_OF_REGIONS = floor(2.0 * M_PI / angular_resolution_);
@@ -139,7 +141,7 @@ void CObservationDownsampler::generateNewGroundReferences(
z_aux_var += (propagation_z_additive_noise_ * range) * (propagation_z_additive_noise_ * range);
float greatest_z_var = z_aux_var;
- if(greatest_z_var < z_centroid_variance)
+ if (greatest_z_var < z_centroid_variance)
greatest_z_var = z_centroid_variance;
point.data_c[GRND_REF_DATA_C_3_Z_VARIANCE] = greatest_z_var;
@@ -166,6 +168,21 @@ void CObservationDownsampler::generateNewGroundReferences(
// and store the point
ground_reference_cloud_ptr->points.push_back(point);
+
+// //TODO: check this!
+// Eigen::Matrix3d covariance_matrix = references_covariance_matrix_vector[reference.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]];
+// covariance_matrix(0,0) = point.data_c[GRND_REF_DATA_C_3_Z_VARIANCE];
+// covariance_matrix(1,1) = point.data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE];
+// covariance_matrix(2,2) = point.data_n[GRND_REF_DATA_N_0_ROLL_VARIANCE];
+//
+//// covariance_matrix << point.data_c[GRND_REF_DATA_C_3_Z_VARIANCE], 0.0, 0.0, 0.0, point.data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE], 0.0, 0.0, 0.0, point.data_n[GRND_REF_DATA_N_0_ROLL_VARIANCE];
+//
+// references_covariance_matrix_vector.push_back(covariance_matrix);
+//
+// assert(
+// ground_reference_cloud_ptr->points[ground_reference_cloud_ptr->points.size() - 1].data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]
+// == references_covariance_matrix_vector.size() - 1
+// && "ERROR!! unxepected index in CObservationDownsampler::generateNewGroundReferences");
}
}