Merge branch 'develop' into devel/alice

algorithms/landmark-triangulation/include/landmark-triangulation/pose-interpolator.h

@@ -72,7 +72,8 @@ class PoseInterpolator {
       std::vector<int64_t>* vertex_timestamps_nanoseconds) const;
 
  private:
-  typedef std::pair<int64_t, StateLinearizationPoint> state_buffer_value_type;
+  typedef std::pair<const int64_t, StateLinearizationPoint>
+      state_buffer_value_type;
   typedef common::TemporalBuffer<
       StateLinearizationPoint,
       Eigen::aligned_allocator<state_buffer_value_type>>

algorithms/landmark-triangulation/src/pose-interpolator.cc

@@ -21,7 +21,7 @@ void PoseInterpolator::buildListOfAllRequiredIMUMeasurements(
   CHECK_GE(end_index, 0);
 
   // First add all imu measurements from this vertex to the buffer.
-  typedef std::pair<int64_t, IMUMeasurement> buffer_value_type;
+  typedef std::pair<const int64_t, IMUMeasurement> buffer_value_type;
   using common::TemporalBuffer;
   typedef TemporalBuffer<
       IMUMeasurement, Eigen::aligned_allocator<buffer_value_type>>

algorithms/simulation/include/mav_planning_utils/path_planning.h

@@ -60,8 +60,9 @@ class Vertex {
   static const int D = _D;
   typedef Eigen::Matrix<double, D, 1> ConstraintValueT;
   typedef std::pair<int, ConstraintValueT> Constraint;
-  typedef std::map<int, ConstraintValueT, std::less<int>,
-                   Eigen::aligned_allocator<std::pair<int, ConstraintValueT> > >
+  typedef std::map<
+      int, ConstraintValueT, std::less<int>,
+      Eigen::aligned_allocator<std::pair<const int, ConstraintValueT> > >
       Constraints;
 
   double time_to_next;
@@ -482,7 +483,7 @@ class Path4D {
   /// initializes the internal segments with the arguments, so that sample() can
   /// be used
   Path4D(
-      const SegmentVector& sx, const SegmentVector& sy, SegmentVector& sz,
+      const SegmentVector& sx, const SegmentVector& sy, const SegmentVector& sz,
       const SegmentVector& syaw)
       : sx_(sx), sy_(sy), sz_(sz), syaw_(syaw) {
     assert(sx_.size() == sy_.size());
@@ -662,9 +663,10 @@ class Path4D {
    */
   template <int n_p, int n_y>
   void sample(
-      typename Motion4D<n_p, n_y>::Vector& result, double dt,
+      typename Motion4D<n_p, n_y>::Vector* result, double dt,
       Eigen::VectorXd* timestamps_seconds) const {
     CHECK_NOTNULL(timestamps_seconds);
+    CHECK_NOTNULL(result);
     double path_time = 0;
 
     for (typename SegmentVector::const_iterator it = sx_.begin();
@@ -675,14 +677,14 @@ class Path4D {
     VLOG(3) << "path time: " << path_time;
     int n_samples = (path_time + dt * 0.5) / dt + 1;
 
-    result.resize(n_samples);
+    result->resize(n_samples);
     timestamps_seconds->resize(n_samples);
 
     VLOG(3) << "num samples: " << n_samples;
 
     double t = 0;
     for (int i = 0; i < n_samples; ++i) {
-      Motion4D<n_p, n_y>& sample = result[i];
+      Motion4D<n_p, n_y>& sample = result->at(i);
       samplePath(sample.x, sx_, t);
       samplePath(sample.y, sy_, t);
       samplePath(sample.z, sz_, t);

algorithms/simulation/src/generic-path-generator.cc

@@ -93,7 +93,7 @@ void GenericPathGenerator::generatePath() {
 
   // Downsampling of the generated trajectory by the sampling time.
   path.sample<5, 2>(
-      path_data_, settings_.sampling_time_second, &timestamps_seconds_);
+      &path_data_, settings_.sampling_time_second, &timestamps_seconds_);
   // This is randomized.
   this->motionVectorToImuData(settings_.imu_noise_bias_seed);
   is_path_generated_ = true;

applications/maplab_node/include/maplab-node/synchronizer.h

@@ -177,6 +177,10 @@ class Synchronizer {
   int64_t previous_nframe_timestamp_ns_;
   // Threshold used to throttle the consecutively published NFrames.
   const int64_t min_nframe_timestamp_diff_ns_;
+  // Tolerance factor between 0.0 and 1.0 to allow for slight variations in
+  // frame rate; relevant when camera frame rate and throttling rate nearly
+  // identical.
+  const float min_nframe_timestamp_diff_tolerance_factor_;
 
   // Number of received odometry measurements.
   int64_t odometry_measurement_counter_;

applications/maplab_node/src/synchronizer.cc

@@ -12,6 +12,12 @@ DEFINE_double(
     vio_nframe_sync_max_output_frequency_hz, 10.0,
     "Maximum output frequency of the synchronized NFrame structures "
     "from the synchronizer.");
+DEFINE_double(
+    vio_nframe_sync_max_output_frequency_tolerance_factor_, 0.95,
+    "Tolerance on the minimum timestamp differance required by throttler above "
+    "which an nframe is released. This is helpful when the desired throttling "
+    "frequency is close to the actual frame rate and the latter has slight "
+    "variations.");
 DEFINE_int32(
     vio_nframe_sync_max_queue_size, 50,
     "Maximum queue size of the synchronization pipeline trying to match images "
@@ -23,7 +29,6 @@ DEFINE_int64(
     odometry_buffer_max_forward_propagation_ns, aslam::time::milliseconds(500),
     "Determines the maximum duration the odometry buffer can "
     "forward-propagate using the IMU.");
-
 DEFINE_bool(
     enable_synchronizer_statistics, true,
     "If enable, the synchronizer will keep data about the latency and other "
@@ -58,7 +63,8 @@ Synchronizer::Synchronizer(const vi_map::SensorManager& sensor_manager)
       time_last_loop_closure_message_received_or_checked_ns_(
           aslam::time::getInvalidTime()),
       time_last_pointcloud_map_message_received_or_checked_ns_(
-          aslam::time::getInvalidTime()) {
+          aslam::time::getInvalidTime()),
+      min_nframe_timestamp_diff_tolerance_factor_(FLAGS_vio_nframe_sync_max_output_frequency_tolerance_factor_) {
   CHECK_GT(FLAGS_vio_nframe_sync_max_output_frequency_hz, 0.);
 
   if (FLAGS_enable_synchronizer_statistics) {
@@ -350,7 +356,8 @@ void Synchronizer::releaseNFrameData(
     // the following nodes are running (e.g. tracker).
     if (aslam::time::isValidTime(previous_nframe_timestamp_ns_)) {
       if (current_frame_timestamp_ns - previous_nframe_timestamp_ns_ <
-          min_nframe_timestamp_diff_ns_) {
+          min_nframe_timestamp_diff_ns_ *
+              min_nframe_timestamp_diff_tolerance_factor_) {
         ++frame_skip_counter_;
         continue;
       }

common/maplab-common/include/maplab-common/temporal-buffer.h

@@ -13,7 +13,8 @@ namespace common {
 
 template <
     typename ValueType,
-    typename AllocatorType = std::allocator<std::pair<int64_t, ValueType> > >
+    typename AllocatorType =
+        std::allocator<std::pair<const int64_t, ValueType> > >
 class TemporalBuffer {
  public:
   typedef std::map<int64_t, ValueType, std::less<int64_t>, AllocatorType>

common/vio-common/include/vio-common/imu-measurements-buffer.h

@@ -107,7 +107,7 @@ class ImuMeasurementBuffer {
       Eigen::Matrix<int64_t, 1, Eigen::Dynamic>* imu_timestamps,
       Eigen::Matrix<double, 6, Eigen::Dynamic>* imu_measurements) const;
 
-  typedef std::pair<int64_t, vio::ImuMeasurement> BufferElement;
+  typedef std::pair<const int64_t, vio::ImuMeasurement> BufferElement;
   typedef Eigen::aligned_allocator<BufferElement> BufferAllocator;
   typedef common::TemporalBuffer<vio::ImuMeasurement, BufferAllocator> Buffer;