discard repeating or not increasing img/imu msgs

applications/maplab_node/include/maplab-node/datasource-rosbag.h

@@ -56,6 +56,10 @@ class DataSourceRosbag : public DataSource {
 
   std::unique_ptr<rosbag::Bag> bag_;
   std::unique_ptr<rosbag::View> bag_view_;
+
+  int64_t last_imu_timestamp_ns_;
+  std::vector<int64_t> last_image_timestamp_ns_;
+
 };
 
 }  // namespace maplab

applications/maplab_node/include/maplab-node/datasource-rostopic.h

@@ -111,6 +111,9 @@ class DataSourceRostopic : public DataSource {
   ros::Subscriber sub_absolute_6dof_;
   ros::Subscriber sub_wheel_odometry_;
   ros::Subscriber sub_pointcloud_map_;
+
+  int64_t last_imu_timestamp_ns_;
+  std::vector<int64_t> last_image_timestamp_ns_;
 };
 
 }  // namespace maplab

applications/maplab_node/src/datasource-rosbag.cc

@@ -50,7 +50,13 @@ DataSourceRosbag::DataSourceRosbag(
       shutdown_requested_(false),
       all_data_streamed_(false),
       rosbag_path_filename_(rosbag_path_filename),
-      ros_topics_(ros_topics) {
+      ros_topics_(ros_topics),
+      last_imu_timestamp_ns_(aslam::time::getInvalidTime()) {
+  const uint32_t num_cameras = ros_topics_.camera_topic_cam_index_map.size();
+  if (num_cameras > 0u) {
+    last_image_timestamp_ns_.resize(num_cameras, aslam::time::getInvalidTime());
+  }
+
   initialize();
 }
 
@@ -217,6 +223,22 @@ void DataSourceRosbag::streamingWorker() {
       // Shift timestamps to start at 0.
       if (!FLAGS_zero_initial_timestamps ||
           shiftByFirstTimestamp(&(image_measurement->timestamp))) {
+        // Check for strictly increasing image timestamps.
+        CHECK_LT(camera_idx, last_image_timestamp_ns_.size());
+        if (aslam::time::isValidTime(last_image_timestamp_ns_[camera_idx]) &&
+            last_image_timestamp_ns_[camera_idx] >=
+                image_measurement->timestamp) {
+          LOG(WARNING) << "[MaplabNode-DataSource] Image message (cam "
+                       << camera_idx << ") is not strictly "
+                       << "increasing! Current timestamp: "
+                       << image_measurement->timestamp
+                       << "ns vs last timestamp: "
+                       << last_image_timestamp_ns_[camera_idx] << "ns.";
+          return;
+        } else {
+          last_image_timestamp_ns_[camera_idx] = image_measurement->timestamp;
+        }
+
         VLOG(3) << "Publish Image measurement...";
         invokeImageCallbacks(image_measurement);
       }
@@ -235,6 +257,19 @@ void DataSourceRosbag::streamingWorker() {
       // Shift timestamps to start at 0.
       if (!FLAGS_zero_initial_timestamps ||
           shiftByFirstTimestamp(&(imu_measurement->timestamp))) {
+        // Check for strictly increasing imu timestamps.
+        if (aslam::time::isValidTime(last_imu_timestamp_ns_) &&
+            last_imu_timestamp_ns_ >= imu_measurement->timestamp) {
+          LOG(WARNING) << "[MaplabNode-DataSource] IMU message is not strictly "
+                       << "increasing! Current timestamp: "
+                       << imu_measurement->timestamp
+                       << "ns vs last timestamp: " << last_imu_timestamp_ns_
+                       << "ns.";
+          return;
+        } else {
+          last_imu_timestamp_ns_ = imu_measurement->timestamp;
+        }
+
         VLOG(3) << "Publish IMU measurement...";
         invokeImuCallbacks(imu_measurement);
       }

applications/maplab_node/src/datasource-rostopic.cc

@@ -8,8 +8,8 @@
 #include <maplab-common/accessors.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <sensors/lidar.h>
-#include <vio-common/rostopic-settings.h>
 #include <vi-map/sensor-utils.h>
+#include <vio-common/rostopic-settings.h>
 
 #include "maplab-node/ros-helpers.h"
 
@@ -23,7 +23,13 @@ DataSourceRostopic::DataSourceRostopic(
     : DataSource(sensor_manager),
       shutdown_requested_(false),
       ros_topics_(ros_topics),
-      image_transport_(node_handle_) {} 
+      image_transport_(node_handle_),
+      last_imu_timestamp_ns_(aslam::time::getInvalidTime()) {
+  const uint32_t num_cameras = ros_topics_.camera_topic_cam_index_map.size();
+  if (num_cameras > 0u) {
+    last_image_timestamp_ns_.resize(num_cameras, aslam::time::getInvalidTime());
+  }
+}
 
 DataSourceRostopic::~DataSourceRostopic() {}
 
@@ -47,12 +53,12 @@ void DataSourceRostopic::registerSubscribers(
                                        << " is subscribed to an empty topic!";
 
     boost::function<void(const sensor_msgs::ImageConstPtr&)> image_callback =
-      boost::bind(
-        &DataSourceRostopic::imageCallback, this, _1, topic_camidx.second);
+        boost::bind(
+            &DataSourceRostopic::imageCallback, this, _1, topic_camidx.second);
 
     constexpr size_t kRosSubscriberQueueSizeImage = 20u;
     image_transport::Subscriber image_sub = image_transport_.subscribe(
-      topic_camidx.first, kRosSubscriberQueueSizeImage, image_callback);
+        topic_camidx.first, kRosSubscriberQueueSizeImage, image_callback);
     sub_images_.push_back(image_sub);
     VLOG(1) << "[MaplabNode-DataSource] Camera " << topic_camidx.second
             << " is subscribed to topic: '" << topic_camidx.first << "'";
@@ -213,7 +219,7 @@ void DataSourceRostopic::imageCallback(
   }
 
   vio::ImageMeasurement::Ptr image_measurement =
-      convertRosImageToMaplabImage(image_message, camera_idx);  
+      convertRosImageToMaplabImage(image_message, camera_idx);
   CHECK(image_measurement);
 
   // Apply the IMU to camera time shift.
@@ -224,6 +230,20 @@ void DataSourceRostopic::imageCallback(
   // Shift timestamps to start at 0.
   if (!FLAGS_zero_initial_timestamps ||
       shiftByFirstTimestamp(&(image_measurement->timestamp))) {
+    // Check for strictly increasing image timestamps.
+    CHECK_LT(camera_idx, last_image_timestamp_ns_.size());
+    if (aslam::time::isValidTime(last_image_timestamp_ns_[camera_idx]) &&
+        last_image_timestamp_ns_[camera_idx] >= image_measurement->timestamp) {
+      LOG(WARNING) << "[MaplabNode-DataSource] Image message (cam "
+                   << camera_idx << ") is not strictly "
+                   << "increasing! Current timestamp: "
+                   << image_measurement->timestamp << "ns vs last timestamp: "
+                   << last_image_timestamp_ns_[camera_idx] << "ns.";
+      return;
+    } else {
+      last_image_timestamp_ns_[camera_idx] = image_measurement->timestamp;
+    }
+
     invokeImageCallbacks(image_measurement);
   }
 }
@@ -240,6 +260,19 @@ void DataSourceRostopic::imuMeasurementCallback(
   // Shift timestamps to start at 0.
   if (!FLAGS_zero_initial_timestamps ||
       shiftByFirstTimestamp(&(imu_measurement->timestamp))) {
+    // Check for strictly increasing imu timestamps.
+    if (aslam::time::isValidTime(last_imu_timestamp_ns_) &&
+        last_imu_timestamp_ns_ >= imu_measurement->timestamp) {
+      LOG(WARNING) << "[MaplabNode-DataSource] IMU message is not strictly "
+                   << "increasing! Current timestamp: "
+                   << imu_measurement->timestamp
+                   << "ns vs last timestamp: " << last_imu_timestamp_ns_
+                   << "ns.";
+      return;
+    } else {
+      last_imu_timestamp_ns_ = imu_measurement->timestamp;
+    }
+
     invokeImuCallbacks(imu_measurement);
   }
 }

applications/rovioli/include/rovioli/datasource-rosbag.h

@@ -55,6 +55,9 @@ class DataSourceRosbag : public DataSource {
 
   std::unique_ptr<rosbag::Bag> bag_;
   std::unique_ptr<rosbag::View> bag_view_;
+
+  int64_t last_imu_timestamp_ns_;
+  std::vector<int64_t> last_image_timestamp_ns_;
 };
 
 }  // namespace rovioli

applications/rovioli/include/rovioli/datasource-rostopic.h

@@ -57,6 +57,9 @@ class DataSourceRostopic : public DataSource {
   std::vector<image_transport::Subscriber> sub_images_;
   ros::Subscriber sub_imu_;
   std::vector<ros::Subscriber> sub_odometry_;
+
+  int64_t last_imu_timestamp_ns_;
+  std::vector<int64_t> last_image_timestamp_ns_;
 };
 
 }  // namespace rovioli

applications/rovioli/src/datasource-rosbag.cc

@@ -45,7 +45,12 @@ DataSourceRosbag::DataSourceRosbag(
     : shutdown_requested_(false),
       all_data_streamed_(false),
       rosbag_path_filename_(rosbag_path_filename),
-      ros_topics_(ros_topics) {
+      ros_topics_(ros_topics),
+      last_imu_timestamp_ns_(aslam::time::getInvalidTime()) {
+  const uint32_t num_cameras = ros_topics_.camera_topic_cam_index_map.size();
+  if (num_cameras > 0u) {
+    last_image_timestamp_ns_.resize(num_cameras, aslam::time::getInvalidTime());
+  }
   initialize();
   if (FLAGS_imu_to_camera_time_offset_ns != 0) {
     LOG(WARNING) << "You are applying a time offset between IMU and camera, be "
@@ -169,6 +174,22 @@ void DataSourceRosbag::streamingWorker() {
       // Shift timestamps to start at 0.
       if (!FLAGS_rovioli_zero_initial_timestamps ||
           shiftByFirstTimestamp(&(image_measurement->timestamp))) {
+        // Check for strictly increasing image timestamps.
+        CHECK_LT(camera_idx, last_image_timestamp_ns_.size());
+        if (aslam::time::isValidTime(last_image_timestamp_ns_[camera_idx]) &&
+            last_image_timestamp_ns_[camera_idx] >=
+                image_measurement->timestamp) {
+          LOG(WARNING) << "[ROVIOLI-DataSource] Image message (cam "
+                       << camera_idx << ") is not strictly "
+                       << "increasing! Current timestamp: "
+                       << image_measurement->timestamp
+                       << "ns vs last timestamp: "
+                       << last_image_timestamp_ns_[camera_idx] << "ns.";
+          return;
+        } else {
+          last_image_timestamp_ns_[camera_idx] = image_measurement->timestamp;
+        }
+
         VLOG(3) << "Publish Image measurement...";
         invokeImageCallbacks(image_measurement);
       }
@@ -184,6 +205,19 @@ void DataSourceRosbag::streamingWorker() {
       // Shift timestamps to start at 0.
       if (!FLAGS_rovioli_zero_initial_timestamps ||
           shiftByFirstTimestamp(&(imu_measurement->timestamp))) {
+        // Check for strictly increasing imu timestamps.
+        if (aslam::time::isValidTime(last_imu_timestamp_ns_) &&
+            last_imu_timestamp_ns_ >= imu_measurement->timestamp) {
+          LOG(WARNING) << "[ROVIOLI-DataSource] IMU message is not strictly "
+                       << "increasing! Current timestamp: "
+                       << imu_measurement->timestamp
+                       << "ns vs last timestamp: " << last_imu_timestamp_ns_
+                       << "ns.";
+          return;
+        } else {
+          last_imu_timestamp_ns_ = imu_measurement->timestamp;
+        }
+
         VLOG(3) << "Publish IMU measurement...";
         invokeImuCallbacks(imu_measurement);
       }

applications/rovioli/src/datasource-rostopic.cc

@@ -17,7 +17,12 @@ DataSourceRostopic::DataSourceRostopic(
     const vio_common::RosTopicSettings& ros_topics)
     : shutdown_requested_(false),
       ros_topics_(ros_topics),
-      image_transport_(node_handle_) {
+      image_transport_(node_handle_),
+      last_imu_timestamp_ns_(aslam::time::getInvalidTime()) {
+  const uint32_t num_cameras = ros_topics_.camera_topic_cam_index_map.size();
+  if (num_cameras > 0u) {
+    last_image_timestamp_ns_.resize(num_cameras, aslam::time::getInvalidTime());
+  }
   if (FLAGS_imu_to_camera_time_offset_ns != 0) {
     LOG(WARNING) << "You are applying a time offset between IMU and camera, be "
                  << "aware that this will shift the image timestamps, which "
@@ -84,14 +89,28 @@ void DataSourceRostopic::imageCallback(
   vio::ImageMeasurement::Ptr image_measurement =
       convertRosImageToMaplabImage(image_message, camera_idx);
   CHECK(image_measurement);
-    // Apply the IMU to camera time shift.
+  // Apply the IMU to camera time shift.
   if (FLAGS_imu_to_camera_time_offset_ns != 0) {
     image_measurement->timestamp += FLAGS_imu_to_camera_time_offset_ns;
   }
 
   // Shift timestamps to start at 0.
   if (!FLAGS_rovioli_zero_initial_timestamps ||
       shiftByFirstTimestamp(&(image_measurement->timestamp))) {
+    // Check for strictly increasing image timestamps.
+    CHECK_LT(camera_idx, last_image_timestamp_ns_.size());
+    if (aslam::time::isValidTime(last_image_timestamp_ns_[camera_idx]) &&
+        last_image_timestamp_ns_[camera_idx] >= image_measurement->timestamp) {
+      LOG(WARNING) << "[ROVIOLI-DataSource] Image message (cam " << camera_idx
+                   << ") is not strictly "
+                   << "increasing! Current timestamp: "
+                   << image_measurement->timestamp << "ns vs last timestamp: "
+                   << last_image_timestamp_ns_[camera_idx] << "ns.";
+      return;
+    } else {
+      last_image_timestamp_ns_[camera_idx] = image_measurement->timestamp;
+    }
+
     invokeImageCallbacks(image_measurement);
   }
 }
@@ -107,6 +126,19 @@ void DataSourceRostopic::imuMeasurementCallback(
   // Shift timestamps to start at 0.
   if (!FLAGS_rovioli_zero_initial_timestamps ||
       shiftByFirstTimestamp(&(imu_measurement->timestamp))) {
+    // Check for strictly increasing imu timestamps.
+    if (aslam::time::isValidTime(last_imu_timestamp_ns_) &&
+        last_imu_timestamp_ns_ >= imu_measurement->timestamp) {
+      LOG(WARNING) << "[ROVIOLI-DataSource] IMU message is not strictly "
+                   << "increasing! Current timestamp: "
+                   << imu_measurement->timestamp
+                   << "ns vs last timestamp: " << last_imu_timestamp_ns_
+                   << "ns.";
+      return;
+    } else {
+      last_imu_timestamp_ns_ = imu_measurement->timestamp;
+    }
+
     invokeImuCallbacks(imu_measurement);
   }
 }