Skip frames with no trajectory pose available for a nearby timestamp

README.md

@@ -184,6 +184,7 @@ A list of optional program arguments follows, grouped by category:
 #### Dataset playback ####
 
 * `--depth_scaling` (default 5000): Input depth scaling: input_depth = depth_scaling * depth_in_meters. The default is for TUM RGB-D benchmark datasets.
+* `--max_pose_interpolation_time_extent` (default 0.05): The maximum time (in seconds) between the timestamp of a frame, and the preceding respectively succeeding trajectory pose timestamp, to interpolate the frame's pose. If this threshold is exceeded, the frame will be dropped since no close-enough pose information is available.
 * `--start_frame` (default 0): First frame of the video to process.
 * `--end_frame` (default: 2147483647): If the video is longer, processing stops after end_frame.
 * `--pyramid_level` (default: 0): Specify the scale-space pyramid level to use. 0 uses the original sized images, 1 uses half the original resolution, etc.

applications/surfel_meshing/src/surfel_meshing/main.cc

@@ -281,6 +281,11 @@ int LIBVIS_MAIN(int argc, char** argv) {
       "--depth_scaling", &depth_scaling, /*required*/ false,
       "Input depth scaling: input_depth = depth_scaling * depth_in_meters. The default is for TUM RGB-D benchmark datasets.");
 
+  float max_pose_interpolation_time_extent = 0.05f;
+  cmd_parser.NamedParameter(
+      "--max_pose_interpolation_time_extent", &max_pose_interpolation_time_extent, /*required*/ false,
+      "The maximum time (in seconds) between the timestamp of a frame, and the preceding respectively succeeding trajectory pose timestamp, to interpolate the frame's pose. If this threshold is exceeded, the frame will be dropped since no close-enough pose information is available.");
+
   int start_frame = 0;
   cmd_parser.NamedParameter(
       "--start_frame", &start_frame, /*required*/ false,
@@ -617,7 +622,7 @@ int LIBVIS_MAIN(int argc, char** argv) {
   // Load dataset.
   RGBDVideo<Vec3u8, u16> rgbd_video;
 
-  if (!ReadTUMRGBDDatasetAssociatedAndCalibrated(dataset_folder_path.c_str(), trajectory_filename.c_str(), &rgbd_video)) {
+  if (!ReadTUMRGBDDatasetAssociatedAndCalibrated(dataset_folder_path.c_str(), trajectory_filename.c_str(), &rgbd_video, max_pose_interpolation_time_extent)) {
     LOG(FATAL) << "Could not read dataset.";
   } else {
     CHECK_EQ(rgbd_video.depth_frames_mutable()->size(), rgbd_video.color_frames_mutable()->size());

libvis/src/libvis/rgbd_video_io_tum_dataset.h

@@ -41,7 +41,12 @@
 namespace vis {
 
 template <typename PoseScalar>
-bool InterpolatePose(double timestamp, const vector<double>& pose_timestamps, const vector<Sophus::SE3<PoseScalar>>& poses, Sophus::SE3<PoseScalar>* pose) {
+bool InterpolatePose(
+    double timestamp,
+    const vector<double>& pose_timestamps,
+    const vector<Sophus::SE3<PoseScalar>>& poses,
+    Sophus::SE3<PoseScalar>* pose,
+    double max_interpolation_time_extent = numeric_limits<double>::infinity()) {
   CHECK_EQ(pose_timestamps.size(), poses.size());
   CHECK_GE(pose_timestamps.size(), 2u);
 
@@ -56,6 +61,11 @@ bool InterpolatePose(double timestamp, const vector<double>& pose_timestamps, co
   // TODO: Binary search should be faster (or with given starting point if having monotonically increasing query points as is the case below).
   for (usize i = 0; i < pose_timestamps.size() - 1; ++ i) {
     if (timestamp >= pose_timestamps[i] && timestamp <= pose_timestamps[i + 1]) {
+      if ((timestamp - pose_timestamps[i]) > max_interpolation_time_extent ||
+          (pose_timestamps[i + 1] - timestamp) > max_interpolation_time_extent) {
+        return false;
+      }
+
       double factor = (timestamp - pose_timestamps[i]) / (pose_timestamps[i + 1] - pose_timestamps[i]);
 
       const Sophus::SE3<PoseScalar>& pose_a = poses[i];
@@ -128,7 +138,8 @@ template<typename ColorT, typename DepthT>
 bool ReadTUMRGBDDatasetAssociatedAndCalibrated(
     const char* dataset_folder_path,
     const char* trajectory_filename,
-    RGBDVideo<ColorT, DepthT>* rgbd_video) {
+    RGBDVideo<ColorT, DepthT>* rgbd_video,
+    double max_interpolation_time_extent = numeric_limits<double>::infinity()) {
   rgbd_video->color_frames_mutable()->clear();
   rgbd_video->depth_frames_mutable()->clear();
 
@@ -187,15 +198,15 @@ bool ReadTUMRGBDDatasetAssociatedAndCalibrated(
     SE3f rgb_global_T_frame;
     double rgb_timestamp = atof(rgb_time_string);
     if (!poses_global_T_frame.empty()) {
-      if (!InterpolatePose(rgb_timestamp, pose_timestamps, poses_global_T_frame, &rgb_global_T_frame)) {
+      if (!InterpolatePose(rgb_timestamp, pose_timestamps, poses_global_T_frame, &rgb_global_T_frame, max_interpolation_time_extent)) {
         continue;
       }
     }
 
     SE3f depth_global_T_frame;
     double depth_timestamp = atof(depth_time_string);
     if (!poses_global_T_frame.empty()) {
-      if (!InterpolatePose(depth_timestamp, pose_timestamps, poses_global_T_frame, &depth_global_T_frame)) {
+      if (!InterpolatePose(depth_timestamp, pose_timestamps, poses_global_T_frame, &depth_global_T_frame, max_interpolation_time_extent)) {
         continue;
       }
     }