creating ground references in median points instead of centroids

include/observation_downsampler.h

@@ -29,6 +29,7 @@ private:
   float propagation_additive_noise_;
   float propagation_z_additive_noise_;
   std::vector<std::vector<float>> centroids_;
+  std::vector<std::vector<pcl::PointXYZRGBNormal>> observations_;
   std::vector<int> num_points_;
   std::vector<accumulator_set<float, stats<tag::mean, tag::variance> > > acc_;
 public:

src/observation_downsampler.cpp

@@ -17,6 +17,8 @@ CObservationDownsampler::CObservationDownsampler(float angular_resolution, float
   const int NUMBER_OF_REGIONS = round(2.0 * M_PI / angular_resolution_);
   //std::cout << "NUMBER_OF_REGIONS = " << NUMBER_OF_REGIONS << std::endl;
 
+  observations_.reserve(NUMBER_OF_REGIONS);
+
   for (int i = 0; i < NUMBER_OF_REGIONS; ++i)
   {
     num_points_.push_back(0);
@@ -28,6 +30,8 @@ CObservationDownsampler::CObservationDownsampler(float angular_resolution, float
     accumulator_set<float, stats<tag::mean, tag::variance> > accum_region; // but we want another more complex accumulator
                                                                            // to compute z variance
     acc_.push_back(accum_region);
+    std::vector <pcl::PointXYZRGBNormal> default_pcl_point_vector;
+    observations_.push_back(default_pcl_point_vector);
   }
 
   //std::cout << "CObservationDownsampler object created!!" << std::endl;
@@ -92,6 +96,28 @@ void CObservationDownsampler::addGroundObservationInSensorFrame(
   acc_[index](ground_observation_in_sensor_frame.z);
 
   num_points_[index]++;
+
+  pcl::PointXYZRGBNormal aux = ground_observation_in_sensor_frame;
+  aux.data_c[GRND_REF_DATA_C_3_Z_VARIANCE] = azimuth_angle;
+  observations_[index].push_back(aux);
+}
+
+struct less_than_azimuth
+{
+    inline bool operator() (const pcl::PointXYZRGBNormal& p1, const pcl::PointXYZRGBNormal& p2)
+    {
+        return (p1.data_c[GRND_REF_DATA_C_3_Z_VARIANCE] < p2.data_c[GRND_REF_DATA_C_3_Z_VARIANCE]);
+    }
+};
+
+pcl::PointXYZRGBNormal median(std::vector<pcl::PointXYZRGBNormal> observations)
+{
+  size_t size = observations.size();
+
+  assert(size > 0 && "Error computing median (observation_downsampler.cpp), size = 0");
+
+  sort(observations.begin(), observations.end(), less_than_azimuth());
+  return observations[size / 2];
 }
 
 void CObservationDownsampler::generateNewGroundReferences(
@@ -107,11 +133,16 @@ void CObservationDownsampler::generateNewGroundReferences(
     if (num_points_[i] != 0)
     {
       pcl::PointXYZRGBNormal point;
-      point.x = centroids_[i][0] /= (float)num_points_[i];
-      point.y = centroids_[i][1] /= (float)num_points_[i];
+//      point.x = centroids_[i][0] /= (float)num_points_[i];
+//      point.y = centroids_[i][1] /= (float)num_points_[i];
+//
+//      float z_centroid = mean(acc_[i]);
+//      point.z = z_centroid;
 
-      float z_centroid = mean(acc_[i]);
-      point.z = z_centroid;
+      pcl::PointXYZRGBNormal median_point = median(observations_[i]);
+      point.x = median_point.x;
+      point.y = median_point.y;
+      point.z = median_point.z;
 
       //std::cout << "Region = " << i << "    Num points = " << num_points_[i] << std::endl
       //    << "Centroid old version x, y, z = " << centroids_[i][0] << "    " << centroids_[i][1] << "    " << centroids_[i][2]