Merge pull request #185 from ethz-asl/feature/discard_non_strictly_in…

…creasing_imu_and_img_msgs

Discard IMU/Img msgs if not strictly increasing timestamps

applications/maplab-console-full/package.xml

@@ -21,6 +21,7 @@
   <exec_depend>maplab_console</exec_depend>
   <exec_depend>mapping_workflows_plugin</exec_depend>
   <exec_depend>pose_graph_manipulation_plugin</exec_depend>
+  <exec_depend>rosbag_plugin</exec_depend>
   <exec_depend>statistics_plugin</exec_depend>
   <exec_depend>vi_map_basic_plugin</exec_depend>
   <exec_depend>vi_map_data_import_export_plugin</exec_depend>

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

@@ -56,6 +56,11 @@ 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_;
+  int64_t last_wheel_odometry_timestamp_ns_;
+
 };
 
 }  // namespace maplab

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

@@ -111,6 +111,10 @@ 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_;
+  int64_t last_wheel_odometry_timestamp_ns_;
 };
 
 }  // namespace maplab

applications/maplab_node/launch/maplab_node_w_rovioli/maplab_node_rovioli_rgbdi.launch

@@ -1,13 +1,13 @@
 <?xml version="1.0"?>
 
 <launch>
-	<node pkg="rosbag" type="play" name="rosbag" args="/home/mfehr/experimental_ws/modelify/bags/outdoor1.bag -q -d 5 -s 0 -r 2.0"/>
+	<node pkg="rosbag" type="play" name="rosbag" args="/home/mfehr/experimental_ws/maplab/rgbdi_bag/original5_2019-11-28-13-18-04.bag -q -d 5 -s 0 -r 1.0"/>
 
 	<node pkg="rviz" type="rviz" name="maplab_rviz" args="-d $(find maplab_node)/share/maplab.rviz -f map"/>
 
 	<include file="$(find maplab_node)/launch/maplab_node_w_rovioli/maplab_node_rovioli.launch">
 		<arg name="robot_name" default=""/>
-		<arg name="maplab_sensor_calibration_file" default="$(find maplab_node)/share/example_calibrations/rgbdi-vi-complete.yaml"/>
+		<arg name="maplab_sensor_calibration_file" default="$(find maplab_node)/share/example_calibrations/rgbdi-sensor_vi.yaml"/>
 		<arg name="maplab_imu_to_camera_time_offset_ns" default="19638586"/>
 
 		<arg name="maplab_selected_ncamera_sensor_id" default="8af1bacaaac5fad79f09e49e3b96d87c"/>
@@ -28,6 +28,6 @@
 		<arg name="maplab_point_cloud_to_depth_map_conversion_include_intensity_image" default="true"/>
 	  <arg name="maplab_attach_point_cloud_maps" default="false"/>
 
-	  <arg name="maplab_map_folder" default="/tmp/"/>
+	  <arg name="maplab_map_folder" default="/tmp/rgbdi_lee_j/"/>
 	</include>
 </launch>

applications/maplab_node/src/datasource-rosbag.cc

@@ -50,7 +50,14 @@ 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()),
+      last_wheel_odometry_timestamp_ns_(aslam::time::getInvalidTime()) {
+  const uint8_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,8 +224,23 @@ void DataSourceRosbag::streamingWorker() {
       // Shift timestamps to start at 0.
       if (!FLAGS_zero_initial_timestamps ||
           shiftByFirstTimestamp(&(image_measurement->timestamp))) {
-        VLOG(3) << "Publish Image measurement...";
-        invokeImageCallbacks(image_measurement);
+        // 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.";
+        } else {
+          last_image_timestamp_ns_[camera_idx] = image_measurement->timestamp;
+
+          VLOG(3) << "Publish Image measurement...";
+          invokeImageCallbacks(image_measurement);
+        }
       }
     }
 
@@ -235,8 +257,20 @@ void DataSourceRosbag::streamingWorker() {
       // Shift timestamps to start at 0.
       if (!FLAGS_zero_initial_timestamps ||
           shiftByFirstTimestamp(&(imu_measurement->timestamp))) {
-        VLOG(3) << "Publish IMU measurement...";
-        invokeImuCallbacks(imu_measurement);
+        // 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.";
+        } else {
+          last_imu_timestamp_ns_ = imu_measurement->timestamp;
+
+          VLOG(3) << "Publish IMU measurement...";
+          invokeImuCallbacks(imu_measurement);
+        }
       }
     }
 
@@ -317,14 +351,25 @@ void DataSourceRosbag::streamingWorker() {
       vi_map::WheelOdometryMeasurement::Ptr wheel_odometry_measurement =
           convertRosOdometryToMaplabWheelOdometry(
               wheel_odometry_msg, sensor_id);
-      if (!wheel_odometry_measurement) {
-        LOG(ERROR) << "Received INVALID wheel odometry constraint!";
-        return;
-      } else {
-        // Shift timestamps to start at 0.
-        if (!FLAGS_zero_initial_timestamps ||
-            shiftByFirstTimestamp(
-                wheel_odometry_measurement->getTimestampNanosecondsMutable())) {
+      CHECK(wheel_odometry_measurement);
+
+      // Shift timestamps to start at 0.
+      if (!FLAGS_zero_initial_timestamps ||
+          shiftByFirstTimestamp(
+              wheel_odometry_measurement->getTimestampNanosecondsMutable())) {
+        // Check for strictly increasing wheel odometry timestamps.
+        if (aslam::time::isValidTime(last_wheel_odometry_timestamp_ns_) &&
+            last_wheel_odometry_timestamp_ns_ >=
+                wheel_odometry_measurement->getTimestampNanoseconds()) {
+          LOG(WARNING) << "[MaplabNode-DataSource] Wheel odometry message is "
+                       << "not strictly increasing! Current timestamp: "
+                       << wheel_odometry_measurement->getTimestampNanoseconds()
+                       << "ns vs last timestamp: "
+                       << last_wheel_odometry_timestamp_ns_ << "ns.";
+        } else {
+          last_wheel_odometry_timestamp_ns_ =
+              wheel_odometry_measurement->getTimestampNanoseconds();
+
           VLOG(3) << "Publish wheel odometry constraint...";
           invokeWheelOdometryConstraintCallbacks(wheel_odometry_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,14 @@ 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()),
+      last_wheel_odometry_timestamp_ns_(aslam::time::getInvalidTime()) {
+  const uint8_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 +54,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 +220,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 +231,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 +261,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);
   }
 }
@@ -401,6 +435,21 @@ void DataSourceRostopic::wheelOdometryConstraintCallback(
   if (!FLAGS_zero_initial_timestamps ||
       (shiftByFirstTimestamp(
           wheel_odometry_measurement->getTimestampNanosecondsMutable()))) {
+    // Check for strictly increasing wheel odometry timestamps.
+    if (aslam::time::isValidTime(last_wheel_odometry_timestamp_ns_) &&
+        last_wheel_odometry_timestamp_ns_ >=
+            wheel_odometry_measurement->getTimestampNanoseconds()) {
+      LOG(WARNING) << "[MaplabNode-DataSource] Wheel odometry message is "
+                   << "not strictly increasing! Current timestamp: "
+                   << wheel_odometry_measurement->getTimestampNanoseconds()
+                   << "ns vs last timestamp: "
+                   << last_wheel_odometry_timestamp_ns_ << "ns.";
+      return;
+    } else {
+      last_wheel_odometry_timestamp_ns_ =
+          wheel_odometry_measurement->getTimestampNanoseconds();
+    }
+
     invokeWheelOdometryConstraintCallbacks(wheel_odometry_measurement);
   }
 }

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

@@ -55,6 +55,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_;
+  int64_t last_odometry_timestamp_ns_;
 };
 
 }  // namespace rovioli

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

@@ -57,6 +57,10 @@ 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_;
+  int64_t last_odometry_timestamp_ns_;
 };
 
 }  // namespace rovioli