fixed depth odom overflow, added params to manage num depth images ad…

…ded at a time

astrobee/config/localization/depth_odometry.config

@@ -32,6 +32,13 @@ orientation_covariance_threshold = 100
 -- icp or image_feature
 depth_odometry_method = "image_feature"
 
+-- Buffer size for images and point clouds
+max_buffer_size = 5
+
+-- Max number of depth images to create in a single iteration
+-- Limits depth odometry runtime 
+max_depth_images = 2
+
 -- ICP options
 
 -- Search radius for each point in ICP 

localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper.h

@@ -21,9 +21,9 @@
 #include <depth_odometry/depth_odometry.h>
 #include <depth_odometry/depth_odometry_wrapper_params.h>
 #include <ff_msgs/DepthOdometry.h>
-#include <localization_common/measurement_buffer.h>
 #include <localization_common/time.h>
 #include <localization_common/timer.h>
+#include <localization_common/timestamped_set.h>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -48,8 +48,8 @@ class DepthOdometryWrapper {
   std::vector<ff_msgs::DepthOdometry> ProcessDepthImageIfAvailable();
   std::unique_ptr<DepthOdometry> depth_odometry_;
   DepthOdometryWrapperParams params_;
-  localization_common::MeasurementBuffer<sensor_msgs::PointCloud2ConstPtr> point_cloud_buffer_;
-  localization_common::MeasurementBuffer<sensor_msgs::ImageConstPtr> image_buffer_;
+  localization_common::TimestampedSet<sensor_msgs::PointCloud2ConstPtr> point_cloud_buffer_;
+  localization_common::TimestampedSet<sensor_msgs::ImageConstPtr> image_buffer_;
   localization_common::Timer timer_ = localization_common::Timer("Depth Odometry");
 };
 }  // namespace depth_odometry

localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper_params.h

@@ -34,6 +34,10 @@ struct DepthOdometryWrapperParams {
   Eigen::Affine3d haz_cam_A_haz_depth;
   PointToPlaneICPDepthOdometryParams icp;
   ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams image_features;
+  // Used for image and point cloud buffer
+  double max_buffer_size;
+  // Max number of depth images to generate per cycle
+  double max_depth_images;
 };
 }  // namespace depth_odometry
 

localization/depth_odometry/src/depth_odometry_wrapper.cc

@@ -58,6 +58,9 @@ void DepthOdometryWrapper::Initialize(const DepthOdometryWrapperParams& params)
   } else {
     LogFatal("DepthOdometryWrapper: Invalid depth odometry method selected.");
   }
+
+  point_cloud_buffer_ = localization_common::TimestampedSet<sensor_msgs::PointCloud2ConstPtr>(params.max_buffer_size);
+  image_buffer_ = localization_common::TimestampedSet<sensor_msgs::ImageConstPtr>(params.max_buffer_size);
 }
 
 std::vector<ff_msgs::DepthOdometry> DepthOdometryWrapper::PointCloudCallback(
@@ -75,27 +78,42 @@ std::vector<ff_msgs::DepthOdometry> DepthOdometryWrapper::ProcessDepthImageIfAva
   std::vector<lm::DepthImageMeasurement> depth_image_measurements;
   boost::optional<lc::Time> latest_added_point_cloud_msg_time;
   boost::optional<lc::Time> latest_added_image_msg_time;
+  int added_depth_images = 0;
   // Point clouds and depth images for the same measurement arrive on different topics.
   // Correlate pairs of these if possible.
-  for (const auto& image_msg : image_buffer_.measurements()) {
-    const auto image_msg_timestamp = image_msg.first;
+  // Only add up to max_depth_images per cycle.
+  for (auto image_msg_it = image_buffer_.set().rbegin();
+       image_msg_it != image_buffer_.set().rend() && added_depth_images < params_.max_depth_images; ++image_msg_it) {
+    const auto image_msg_timestamp = image_msg_it->first;
     const auto point_cloud_msg =
-      point_cloud_buffer_.GetNearby(image_msg_timestamp, params_.max_image_and_point_cloud_time_diff);
-    if (point_cloud_msg) {
+      point_cloud_buffer_.Closest(image_msg_timestamp);
+    if (point_cloud_msg &&
+        std::abs(point_cloud_msg->timestamp - image_msg_timestamp) <= params_.max_image_and_point_cloud_time_diff) {
       const auto depth_image_measurement =
-        lm::MakeDepthImageMeasurement(*point_cloud_msg, image_msg.second, params_.haz_cam_A_haz_depth);
+        lm::MakeDepthImageMeasurement(point_cloud_msg->value, image_msg_it->second, params_.haz_cam_A_haz_depth);
       if (!depth_image_measurement) {
         LogError("ProcessDepthImageIfAvailable: Failed to create depth image measurement.");
         continue;
       }
       depth_image_measurements.emplace_back(*depth_image_measurement);
-      latest_added_point_cloud_msg_time = lc::TimeFromHeader((*point_cloud_msg)->header);
+      ++added_depth_images;
+      if (!latest_added_point_cloud_msg_time)
+        latest_added_point_cloud_msg_time = point_cloud_msg->timestamp;
+      if (!latest_added_image_msg_time)
       latest_added_image_msg_time = image_msg_timestamp;
     }
   }
 
-  if (latest_added_point_cloud_msg_time) point_cloud_buffer_.EraseUpToAndIncluding(*latest_added_point_cloud_msg_time);
-  if (latest_added_image_msg_time) image_buffer_.EraseUpToAndIncluding(*latest_added_image_msg_time);
+  // Remove any measuremets older than measurements used for depth image creation
+  // Also remove measurements used for depth image creation
+  if (latest_added_point_cloud_msg_time) {
+      point_cloud_buffer_.RemoveOldValues(*latest_added_point_cloud_msg_time);
+      point_cloud_buffer_.Remove(*latest_added_point_cloud_msg_time);
+  }
+  if (latest_added_image_msg_time) {
+    image_buffer_.RemoveOldValues(*latest_added_image_msg_time);
+    image_buffer_.Remove(*latest_added_image_msg_time);
+  }
 
   std::vector<ff_msgs::DepthOdometry> depth_odometry_msgs;
   for (const auto& depth_image_measurement : depth_image_measurements) {

localization/depth_odometry/src/parameter_reader.cc

@@ -38,6 +38,8 @@ void LoadDepthOdometryWrapperParams(config_reader::ConfigReader& config, DepthOd
   params.haz_cam_A_haz_depth = Eigen::Affine3d::Identity();
   LoadPointToPlaneICPDepthOdometryParams(config, params.icp);
   LoadImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(config, params.image_features);
+  params.max_buffer_size = mc::LoadDouble(config, "max_buffer_size");
+  params.max_depth_images = mc::LoadDouble(config, "max_depth_images");
 }
 
 void LoadDepthOdometryParams(config_reader::ConfigReader& config, DepthOdometryParams& params) {