[Bug] Bypass SOF on blackout images

libs/odometry/multi_visual_odometry_base.cpp

@@ -25,6 +25,51 @@
 
 namespace cuvslam::odom {
 
+namespace {
+
+bool IsAllZeroHostU8(const ImageSource& source, const ImageShape& shape) {
+  if (source.data == nullptr || source.memory_type != ImageSource::Host || source.type != ImageSource::U8) {
+    return false;
+  }
+
+  const size_t num_channels = source.image_encoding == ImageEncoding::RGB8 ? 3 : 1;
+  const auto* data = static_cast<const uint8_t*>(source.data);
+  const size_t num_values = static_cast<size_t>(shape.width) * static_cast<size_t>(shape.height) * num_channels;
+  return std::all_of(data, data + num_values, [](uint8_t value) { return value == 0; });
+}
+
+bool AreAllAvailableImagesBlack(const Sources& sources, const sof::Images& images) {
+  bool saw_image = false;
+  for (size_t cam_id = 0; cam_id < images.size(); ++cam_id) {
+    if (images[cam_id] == nullptr || cam_id >= sources.size() || sources[cam_id].data == nullptr) {
+      continue;
+    }
+    saw_image = true;
+    if (!IsAllZeroHostU8(sources[cam_id], images[cam_id]->get_image_meta().shape)) {
+      return false;
+    }
+  }
+  return saw_image;
+}
+
+void DropCurrentImages(sof::Images& images) {
+  for (auto& image : images) {
+    image = nullptr;
+  }
+}
+
+void ClearFrameStat(IVisualOdometry::VOFrameStat* stat) {
+  if (!stat) {
+    return;
+  }
+  stat->keyframe = false;
+  stat->heating = false;
+  stat->tracks2d.clear();
+  stat->tracks3d.clear();
+}
+
+}  // namespace
+
 MultiVisualOdometryBase::MultiVisualOdometryBase(const camera::Rig& rig, const camera::FrustumIntersectionGraph& fig,
                                                  const Settings& settings, bool use_gpu)
 
@@ -73,12 +118,38 @@ bool MultiVisualOdometryBase::track(const Sources& curr_sources, [[maybe_unused]
   TRACE_EVENT ev = profiler_domain_.trace_event("MultiVisualOdometryBase::track()", profiler_color_);
   const int64_t timestamp = (*first_image)->get_image_meta().timestamp;  // current frame timestamp
   Isometry3T predicted_world_from_rig = prev_world_from_rig_;
+  Isometry3T world_from_rig;
   pipelines::ISFMSolver& solver = get_solver();
 
   if (settings_.use_prediction) {
     do_predict(&prediction_model_, timestamp, predicted_world_from_rig);
   }
 
+  if (can_track_visual_blackout() && AreAllAvailableImagesBlack(curr_sources, curr_images)) {
+    for (auto& cam_observations : observations_) {
+      cam_observations.clear();
+    }
+
+    const bool have_pose = solver.solveNextFrame(
+        timestamp, sof::FrameState::None, observations_, world_from_rig, static_info_exp,
+        {per_frame_setting.sba, per_frame_setting.sm, per_frame_setting.vo_pnp, per_frame_setting.inertial_stereo_pnp,
+         per_frame_setting.imu_pnp, per_frame_setting.icp});
+
+    DropCurrentImages(curr_images);
+    if (!have_pose) {
+      ClearFrameStat(last_frame_stat_.get());
+      delta = Isometry3T::Identity();
+      static_info_exp.setZero();
+      return false;
+    }
+
+    ClearFrameStat(last_frame_stat_.get());
+    prediction_model_.add_known_pose(world_from_rig, timestamp);
+    delta = prev_world_from_rig_.inverse() * world_from_rig;
+    prev_world_from_rig_ = world_from_rig;
+    return true;
+  }
+
   sof::FrameState frame_type;
   for (auto& cam_observations : observations_) {
     cam_observations.clear();
@@ -98,7 +169,6 @@ bool MultiVisualOdometryBase::track(const Sources& curr_sources, [[maybe_unused]
   IVisualOdometry::VOFrameStat* stat = last_frame_stat_.get();
   std::vector<Track2D>* tracks2d = stat ? &(stat->tracks2d) : nullptr;
   Tracks3DMap* tracks3d = stat ? &(stat->tracks3d) : nullptr;
-  Isometry3T world_from_rig;
 
   const bool have_pose =
       solver.solveNextFrame(timestamp, frame_type, observations_, world_from_rig, static_info_exp,

libs/odometry/multi_visual_odometry_base.h

@@ -50,6 +50,8 @@ class MultiVisualOdometryBase : public IVisualOdometry {
   virtual pipelines::ISFMSolver& get_solver() = 0;
 
 protected:
+  virtual bool can_track_visual_blackout() const { return false; }
+
   void reset();
   camera::Rig rig_;
   camera::FrustumIntersectionGraph fig_;

libs/odometry/stereo_inertial_odometry.cpp

@@ -55,4 +55,6 @@ std::optional<pipelines::SolverSfMInertial::ImuState> StereoInertialOdometry::Ge
   return solver_.GetImuState();
 }
 
+bool StereoInertialOdometry::can_track_visual_blackout() const { return solver_.get_gravity().has_value(); }
+
 }  // namespace cuvslam::odom

libs/odometry/stereo_inertial_odometry.h

@@ -47,6 +47,8 @@ class StereoInertialOdometry : public MultiVisualOdometryBase {
   std::optional<pipelines::SolverSfMInertial::ImuState> GetImuState() const;
 
 private:
+  bool can_track_visual_blackout() const override;
+
   imu::ImuCalibration calib_;
   pipelines::SolverSfMInertial solver_;
 };

libs/pipelines/track_online_inertial.cpp

@@ -634,7 +634,10 @@ bool SolverSfMInertial::solveNextFrame(int64_t time_ns, const sof::FrameState& f
     last_valid_pose.preintegration = sba_imu::IMUPreintegration(curr_pose.gyro_bias, curr_pose.acc_bias);
   }
 
-  integrated = !pnp_result && imu_state == StateMachine::State::Ok;
+  const bool no_observations = obs_vector_.empty();
+  const bool integrated_from_blackout =
+      !pnp_result && no_observations && no_drops && !is_first_run && maybe_gravity.has_value();
+  integrated = !pnp_result && (imu_state == StateMachine::State::Ok || integrated_from_blackout);
   TraceMessage(
       "Frame: pnp=%d integrated=%d imu_state=%d obs=%d vel=[%.3f,%.3f,%.3f] gbias=[%.4f,%.4f,%.4f] "
       "abias=[%.4f,%.4f,%.4f]",
@@ -657,10 +660,14 @@ bool SolverSfMInertial::solveNextFrame(int64_t time_ns, const sof::FrameState& f
   }
 
   if (integrated) {
-    integ_kf.predict_pose(*maybe_gravity, integ_kf.preintegration, curr_pose);
+    if (integrated_from_blackout) {
+      prev_pose.predict_pose(*maybe_gravity, prev_pose.preintegration, curr_pose);
+    } else {
+      integ_kf.predict_pose(*maybe_gravity, integ_kf.preintegration, curr_pose);
+    }
     TraceDebug("Pose was integrated!");
   }
-  if (pnp_result || imu_state == StateMachine::State::Ok) {
+  if (pnp_result || integrated) {
     // either we successfully converged, or successfully integrated the pose
     world_from_rig = curr_pose.w_from_imu * imu_from_rig;
     rig_from_w = world_from_rig.inverse();