From fa60d71493de1ddafa6bbf885db344fee76635af Mon Sep 17 00:00:00 2001 From: Zheng Wang Date: Wed, 12 Aug 2026 13:56:13 -0700 Subject: [PATCH] [Bug] Bypass SOF on blackout images --- libs/odometry/multi_visual_odometry_base.cpp | 72 +++++++++++++++++++- libs/odometry/multi_visual_odometry_base.h | 2 + libs/odometry/stereo_inertial_odometry.cpp | 2 + libs/odometry/stereo_inertial_odometry.h | 2 + libs/pipelines/track_online_inertial.cpp | 13 +++- 5 files changed, 87 insertions(+), 4 deletions(-) diff --git a/libs/odometry/multi_visual_odometry_base.cpp b/libs/odometry/multi_visual_odometry_base.cpp index a241145..2d8d6b8 100644 --- a/libs/odometry/multi_visual_odometry_base.cpp +++ b/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(source.data); + const size_t num_values = static_cast(shape.width) * static_cast(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* 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, diff --git a/libs/odometry/multi_visual_odometry_base.h b/libs/odometry/multi_visual_odometry_base.h index 5c83d6c..471722f 100644 --- a/libs/odometry/multi_visual_odometry_base.h +++ b/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_; diff --git a/libs/odometry/stereo_inertial_odometry.cpp b/libs/odometry/stereo_inertial_odometry.cpp index d9f40a3..355526e 100644 --- a/libs/odometry/stereo_inertial_odometry.cpp +++ b/libs/odometry/stereo_inertial_odometry.cpp @@ -55,4 +55,6 @@ std::optional StereoInertialOdometry::Ge return solver_.GetImuState(); } +bool StereoInertialOdometry::can_track_visual_blackout() const { return solver_.get_gravity().has_value(); } + } // namespace cuvslam::odom diff --git a/libs/odometry/stereo_inertial_odometry.h b/libs/odometry/stereo_inertial_odometry.h index b513d8b..9316982 100644 --- a/libs/odometry/stereo_inertial_odometry.h +++ b/libs/odometry/stereo_inertial_odometry.h @@ -47,6 +47,8 @@ class StereoInertialOdometry : public MultiVisualOdometryBase { std::optional GetImuState() const; private: + bool can_track_visual_blackout() const override; + imu::ImuCalibration calib_; pipelines::SolverSfMInertial solver_; }; diff --git a/libs/pipelines/track_online_inertial.cpp b/libs/pipelines/track_online_inertial.cpp index 0f3417d..9043175 100644 --- a/libs/pipelines/track_online_inertial.cpp +++ b/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();