Add view_offset setting for cameras with low overlap

data/euroc_ds_calib.json

@@ -224,6 +224,7 @@
         },
         "mocap_time_offset_ns": 0,
         "mocap_to_imu_offset_ns": 140763258159875,
-        "cam_time_offset_ns": 0
+        "cam_time_offset_ns": 0,
+        "view_offset": 0
     }
 }

data/euroc_eucm_calib.json

@@ -208,6 +208,7 @@
         },
         "mocap_time_offset_ns": 0,
         "mocap_to_imu_offset_ns": 140763258159875,
-        "cam_time_offset_ns": 0
+        "cam_time_offset_ns": 0,
+        "view_offset": 0
     }
 }

data/tumvi_512_ds_calib.json

@@ -105,6 +105,7 @@
         "mocap_time_offset_ns": 0,
         "mocap_to_imu_offset_ns": 0,
         "cam_time_offset_ns": 0,
+        "view_offset": 0,
         "vignette": [
             {
                 "value0": 0,

data/tumvi_512_eucm_calib.json

@@ -105,6 +105,7 @@
         "mocap_time_offset_ns": 0,
         "mocap_to_imu_offset_ns": 0,
         "cam_time_offset_ns": 0,
+        "view_offset": 0,
         "vignette": [
             {
                 "value0": 0,

include/basalt/optical_flow/frame_to_frame_optical_flow.h

@@ -103,6 +103,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
         if (output_queue) output_queue->push(nullptr);
         break;
       }
+      input_ptr->addTime("opticalflow_received");
 
       processFrame(input_ptr->t_ns, input_ptr);
     }
@@ -172,18 +173,20 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
     }
 
     if (output_queue && frame_counter % config.optical_flow_skip_frames == 0) {
+      transforms->input_images->addTime("opticalflow_produced");
       output_queue->push(transforms);
     }
 
     frame_counter++;
   }
 
-  void trackPoints(const basalt::ManagedImagePyr<uint16_t>& pyr_1,
+  void trackPoints(
+      const basalt::ManagedImagePyr<uint16_t>& pyr_1,
       const basalt::ManagedImagePyr<uint16_t>& pyr_2,
       const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
           transform_map_1,
-                   Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
+      Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>& transform_map_2,
-                       transform_map_2) const {
+      int64_t view_offset = 0) const {
     size_t num_points = transform_map_1.size();
 
     std::vector<KeypointId> ids;
@@ -207,25 +210,29 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
 
         const Eigen::AffineCompact2f& transform_1 = init_vec[r];
         Eigen::AffineCompact2f transform_2 = transform_1;
+        transform_2.translation()(0) -= view_offset;
 
-        bool valid = trackPoint(pyr_1, pyr_2, transform_1, transform_2);
+        bool valid = transform_2.translation()(0) >= 0 &&
+                     transform_2.translation()(0) < pyr_2.lvl(0).w;
+        if (!valid) continue;
+
+        valid = trackPoint(pyr_1, pyr_2, transform_1, transform_2);
+        if (!valid) continue;
 
-        if (valid) {
         Eigen::AffineCompact2f transform_1_recovered = transform_2;
+        transform_1_recovered.translation()(0) += view_offset;
 
         valid = trackPoint(pyr_2, pyr_1, transform_2, transform_1_recovered);
+        if (!valid) continue;
 
-          if (valid) {
+        Scalar dist2 =
-            Scalar dist2 = (transform_1.translation() -
+            (transform_1.translation() - transform_1_recovered.translation())
-                            transform_1_recovered.translation())
                 .squaredNorm();
 
         if (dist2 < config.optical_flow_max_recovered_dist2) {
           result[id] = transform_2;
         }
       }
-        }
-      }
     };
 
     tbb::blocked_range<size_t> range(0, num_points);
@@ -332,7 +339,8 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
     }
 
     if (calib.intrinsics.size() > 1) {
-      trackPoints(pyramid->at(0), pyramid->at(1), new_poses0, new_poses1);
+      trackPoints(pyramid->at(0), pyramid->at(1), new_poses0, new_poses1,
+                  calib.view_offset);
 
       for (const auto& kv : new_poses1) {
         transforms->observations.at(1).emplace(kv);

scripts/basalt_convert_kitti_calib.py

@@ -111,7 +111,8 @@ calib_template = Template('''{
         "gyro_noise_std": [0.0, 0.0, 0.0],
         "accel_bias_std": [0.0, 0.0, 0.0],
         "gyro_bias_std": [0.0, 0.0, 0.0],
-        "cam_time_offset_ns": 0
+        "cam_time_offset_ns": 0,
+        "view_offset": 0
     }
 }
 ''')

thirdparty/basalt-headers

@@ -1 +1 @@
-Subproject commit 42b623e812c12422e7522a652bc0a2e3ee6924d5
+Subproject commit 8c39d10b3d5463bf257701a32293caee088b8836