mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 22:41:13 +00:00
[AZ-333] C1 VINS-Mono strategy — research-only comparative VIO
VinsMonoStrategy: Python facade conforming to AZ-331 Protocol; mirrors the AZ-332 OKVIS2 facade so the AZ-331 factory + IT-12 comparative harness can treat both as drop-in substitutable. Native binding is a pybind11 skeleton compiled behind BUILD_VINS_MONO=ON (default OFF for airborne / operator-tooling / replay-cli per module-layout.md Build-Time Exclusion Map). Real vins_estimator wiring is the Tier-2 follow-up. VinsMonoConfig added to c1_vio/config.py with sliding-window / feature-tracker / marginalisation / opt-iteration knobs plus __post_init__ validation; exported through the package __init__. cpp/vins_mono/CMakeLists.txt replaces the AZ-263 placeholder with full pybind11 wiring: Risk-1 mitigation forces VINS_MONO_USE_ROS=OFF; Risk-2 mitigation links Eigen from the same cpp/_third_party/eigen pin as OKVIS2; Risk-3 mitigation enforces BUILD_VINS_MONO=OFF in deployment binaries via the gate at the top of the file. Tests: 17 new in test_vins_mono_strategy.py (15 pass + 2 tier2 skip); fake_vins_mono_binding fixture added to conftest.py mirroring the fake_okvis2_binding pattern; test_protocol_conformance updated to drop vins_mono from _STRATEGIES_WITHOUT_PY_MODULE so the existing parametrised factory tests route through the new strategy. Focused c1_vio suite: 72 passed, 4 skipped. Full suite: 1788 passed, 1 unrelated pre-existing flake (c12 cold-start perf, env-bound). Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -25,7 +25,11 @@ from gps_denied_onboard._types.nav import (
|
||||
VioState,
|
||||
WarmStartPose,
|
||||
)
|
||||
from gps_denied_onboard.components.c1_vio.config import C1VioConfig, Okvis2Config
|
||||
from gps_denied_onboard.components.c1_vio.config import (
|
||||
C1VioConfig,
|
||||
Okvis2Config,
|
||||
VinsMonoConfig,
|
||||
)
|
||||
from gps_denied_onboard.components.c1_vio.errors import (
|
||||
VioDegradedError,
|
||||
VioError,
|
||||
@@ -41,6 +45,7 @@ __all__ = [
|
||||
"C1VioConfig",
|
||||
"FeatureQuality",
|
||||
"Okvis2Config",
|
||||
"VinsMonoConfig",
|
||||
"VioDegradedError",
|
||||
"VioError",
|
||||
"VioFatalError",
|
||||
|
||||
@@ -0,0 +1,338 @@
|
||||
// AZ-333 — pybind11 binding for VINS-Mono (research-only C1 VIO).
|
||||
//
|
||||
// Exposes a narrow surface that mirrors what the Python facade
|
||||
// (`gps_denied_onboard.components.c1_vio.vins_mono.VinsMonoStrategy`)
|
||||
// needs — NOT the full VINS-Mono estimator API. The surface mirrors
|
||||
// the AZ-332 OKVIS2 binding 1:1 so the AZ-331 factory can treat both
|
||||
// strategies as drop-in substitutable for the IT-12 comparative-study
|
||||
// research binary:
|
||||
//
|
||||
// VinsMonoBackend
|
||||
// ctor(yaml_config: str, camera_intrinsics_3x3: ndarray[float64, 3, 3])
|
||||
// add_frame(frame_id: str, ts_ns: int, image: ndarray[uint8, H, W, C]) -> bool
|
||||
// add_imu(ts_ns: int, accel: ndarray[float64, 3], gyro: ndarray[float64, 3]) -> None
|
||||
// get_latest_output() -> dict | None
|
||||
// reset(body_T_world: ndarray[float64, 4, 4], velocity: ndarray[float64, 3],
|
||||
// accel_bias: ndarray[float64, 3], gyro_bias: ndarray[float64, 3]) -> None
|
||||
// health() -> dict
|
||||
//
|
||||
// Frame buffers cross the FFI boundary as `py::array_t<uint8_t,
|
||||
// c_style|forcecast>` so the camera-ingest path (AZ-265
|
||||
// LiveCameraFrameSource) can hand off a contiguous numpy array without
|
||||
// a copy — Risk-2 mitigation per the AZ-333 task spec.
|
||||
//
|
||||
// Exception envelope: every VINS-Mono / Ceres / Eigen / std::runtime_error
|
||||
// inside a binding method is caught and rethrown as one of three
|
||||
// Python-side exceptions registered via `py::register_exception`. The
|
||||
// Python facade then rewraps those into the VioError family.
|
||||
//
|
||||
// Risk-1 mitigation (ROS leak): this binding compiles against the
|
||||
// de-ROSified VINS-Mono pin only — `cpp/vins_mono/CMakeLists.txt`
|
||||
// strips upstream's `roscpp` / `rosbag` deps before the
|
||||
// `vins_estimator` core is exposed here.
|
||||
|
||||
#include <pybind11/pybind11.h>
|
||||
#include <pybind11/numpy.h>
|
||||
#include <pybind11/stl.h>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
// VINS-Mono estimator headers. The exact include path is determined by
|
||||
// the de-ROSified upstream pin's CMake export. The skeleton compiles
|
||||
// without these headers because the actual `vins_estimator::Estimator`
|
||||
// wiring lives in _build_estimator() / _drive_estimator(), which today
|
||||
// STUB and surface a runtime error if invoked. Wiring them in is the
|
||||
// follow-up task within AZ-333's tier2 deliverable bundle.
|
||||
//
|
||||
// #include <vins_estimator/estimator.h>
|
||||
// #include <vins_estimator/feature_tracker.h>
|
||||
// #include <vins_estimator/parameters.h>
|
||||
|
||||
namespace py = pybind11;
|
||||
|
||||
namespace {
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Exception types — registered as Python-side classes via
|
||||
// `py::register_exception` in PYBIND11_MODULE below. The Python facade
|
||||
// catches these and rewraps into the VioError family.
|
||||
|
||||
class VinsMonoInitException : public std::runtime_error {
|
||||
public:
|
||||
using std::runtime_error::runtime_error;
|
||||
};
|
||||
|
||||
class VinsMonoFatalException : public std::runtime_error {
|
||||
public:
|
||||
using std::runtime_error::runtime_error;
|
||||
};
|
||||
|
||||
class VinsMonoOptimizationException : public std::runtime_error {
|
||||
public:
|
||||
using std::runtime_error::runtime_error;
|
||||
};
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Pose / output struct produced by the estimator step.
|
||||
struct EstimatorOutput {
|
||||
std::string frame_id;
|
||||
Eigen::Matrix4d pose_T_world_body;
|
||||
Eigen::Matrix<double, 6, 6> pose_covariance_6x6;
|
||||
Eigen::Vector3d accel_bias;
|
||||
Eigen::Vector3d gyro_bias;
|
||||
int tracked_features = 0;
|
||||
int new_features = 0;
|
||||
int lost_features = 0;
|
||||
double mean_parallax = 0.0;
|
||||
double mre_px = 0.0;
|
||||
std::int64_t emitted_at_ns = 0;
|
||||
};
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Internal estimator state machine — INIT until the SfM bootstrap
|
||||
// converges (VINS-Mono's `solve_initial` flips), TRACKING during nominal
|
||||
// optimisation, DEGRADED on feature-count drop, LOST after consecutive
|
||||
// failed updates.
|
||||
enum class HealthState : int { Init = 0, Tracking = 1, Degraded = 2, Lost = 3 };
|
||||
|
||||
const char* state_to_str(HealthState s) {
|
||||
switch (s) {
|
||||
case HealthState::Init:
|
||||
return "init";
|
||||
case HealthState::Tracking:
|
||||
return "tracking";
|
||||
case HealthState::Degraded:
|
||||
return "degraded";
|
||||
case HealthState::Lost:
|
||||
return "lost";
|
||||
}
|
||||
return "init";
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// VinsMonoBackend — the C++ surface exposed to Python.
|
||||
class VinsMonoBackend {
|
||||
public:
|
||||
VinsMonoBackend(const std::string& yaml_config,
|
||||
py::array_t<double, py::array::c_style | py::array::forcecast>
|
||||
camera_intrinsics_3x3)
|
||||
: yaml_config_(yaml_config) {
|
||||
if (camera_intrinsics_3x3.ndim() != 2 ||
|
||||
camera_intrinsics_3x3.shape(0) != 3 ||
|
||||
camera_intrinsics_3x3.shape(1) != 3) {
|
||||
throw VinsMonoInitException(
|
||||
"VinsMonoBackend: camera_intrinsics_3x3 must be a 3x3 float64 array");
|
||||
}
|
||||
auto buf = camera_intrinsics_3x3.unchecked<2>();
|
||||
for (py::ssize_t i = 0; i < 3; ++i) {
|
||||
for (py::ssize_t j = 0; j < 3; ++j) {
|
||||
K_(i, j) = buf(i, j);
|
||||
}
|
||||
}
|
||||
_build_estimator();
|
||||
}
|
||||
|
||||
// Push a nav-camera frame into the estimator.
|
||||
// Returns true if the estimator produced a new output for this frame
|
||||
// (caller then calls `get_latest_output()`); false if the frame was
|
||||
// consumed but did not yield a new output (e.g. dropped as
|
||||
// non-keyframe by the parallax-driven keyframe selector).
|
||||
bool add_frame(
|
||||
const std::string& frame_id, std::int64_t ts_ns,
|
||||
py::array_t<std::uint8_t,
|
||||
py::array::c_style | py::array::forcecast> image) {
|
||||
if (image.ndim() < 2 || image.ndim() > 3) {
|
||||
throw VinsMonoOptimizationException(
|
||||
"VinsMonoBackend.add_frame: image must be 2-D (grayscale) or 3-D "
|
||||
"(HxWxC)");
|
||||
}
|
||||
pending_frame_id_ = frame_id;
|
||||
pending_ts_ns_ = ts_ns;
|
||||
return _drive_estimator(image);
|
||||
}
|
||||
|
||||
void add_imu(std::int64_t ts_ns,
|
||||
py::array_t<double,
|
||||
py::array::c_style | py::array::forcecast> accel,
|
||||
py::array_t<double,
|
||||
py::array::c_style | py::array::forcecast> gyro) {
|
||||
if (accel.size() != 3 || gyro.size() != 3) {
|
||||
throw VinsMonoOptimizationException(
|
||||
"VinsMonoBackend.add_imu: accel and gyro must be length-3 float64 "
|
||||
"arrays");
|
||||
}
|
||||
if (ts_ns <= last_imu_ts_ns_) {
|
||||
throw VinsMonoOptimizationException(
|
||||
"VinsMonoBackend.add_imu: ts_ns must be strict-monotonic");
|
||||
}
|
||||
last_imu_ts_ns_ = ts_ns;
|
||||
// Real VINS-Mono IMU push lands here once the estimator is wired
|
||||
// in. For the skeleton we just record the most recent sample — the
|
||||
// estimator's IMU pre-integration is performed inside
|
||||
// `vins_estimator::Estimator::processIMU`.
|
||||
auto a = accel.unchecked<1>();
|
||||
auto g = gyro.unchecked<1>();
|
||||
last_accel_ = Eigen::Vector3d(a(0), a(1), a(2));
|
||||
last_gyro_ = Eigen::Vector3d(g(0), g(1), g(2));
|
||||
}
|
||||
|
||||
std::optional<py::dict> get_latest_output() const {
|
||||
std::lock_guard<std::mutex> lk(output_mtx_);
|
||||
if (!latest_output_.has_value()) {
|
||||
return std::nullopt;
|
||||
}
|
||||
const auto& o = *latest_output_;
|
||||
py::dict d;
|
||||
d["frame_id"] = o.frame_id;
|
||||
d["pose_T_world_body"] = py::array_t<double>(
|
||||
{4, 4}, {sizeof(double) * 4, sizeof(double)},
|
||||
o.pose_T_world_body.data());
|
||||
d["pose_covariance_6x6"] = py::array_t<double>(
|
||||
{6, 6}, {sizeof(double) * 6, sizeof(double)},
|
||||
o.pose_covariance_6x6.data());
|
||||
d["accel_bias"] = py::array_t<double>(
|
||||
{3}, {sizeof(double)}, o.accel_bias.data());
|
||||
d["gyro_bias"] = py::array_t<double>(
|
||||
{3}, {sizeof(double)}, o.gyro_bias.data());
|
||||
d["tracked_features"] = o.tracked_features;
|
||||
d["new_features"] = o.new_features;
|
||||
d["lost_features"] = o.lost_features;
|
||||
d["mean_parallax"] = o.mean_parallax;
|
||||
d["mre_px"] = o.mre_px;
|
||||
d["emitted_at_ns"] = o.emitted_at_ns;
|
||||
return d;
|
||||
}
|
||||
|
||||
void reset(py::array_t<double,
|
||||
py::array::c_style | py::array::forcecast> body_T_world,
|
||||
py::array_t<double,
|
||||
py::array::c_style | py::array::forcecast> velocity,
|
||||
py::array_t<double,
|
||||
py::array::c_style | py::array::forcecast> accel_bias,
|
||||
py::array_t<double,
|
||||
py::array::c_style | py::array::forcecast> gyro_bias) {
|
||||
if (body_T_world.ndim() != 2 || body_T_world.shape(0) != 4 ||
|
||||
body_T_world.shape(1) != 4) {
|
||||
throw VinsMonoInitException(
|
||||
"VinsMonoBackend.reset: body_T_world must be a 4x4 float64 array");
|
||||
}
|
||||
if (velocity.size() != 3 || accel_bias.size() != 3 ||
|
||||
gyro_bias.size() != 3) {
|
||||
throw VinsMonoInitException(
|
||||
"VinsMonoBackend.reset: velocity / *_bias must be length-3 float64 "
|
||||
"arrays");
|
||||
}
|
||||
auto T = body_T_world.unchecked<2>();
|
||||
for (py::ssize_t i = 0; i < 4; ++i) {
|
||||
for (py::ssize_t j = 0; j < 4; ++j) {
|
||||
seed_body_T_world_(i, j) = T(i, j);
|
||||
}
|
||||
}
|
||||
auto v = velocity.unchecked<1>();
|
||||
auto ab = accel_bias.unchecked<1>();
|
||||
auto gb = gyro_bias.unchecked<1>();
|
||||
seed_velocity_ = Eigen::Vector3d(v(0), v(1), v(2));
|
||||
seed_accel_bias_ = Eigen::Vector3d(ab(0), ab(1), ab(2));
|
||||
seed_gyro_bias_ = Eigen::Vector3d(gb(0), gb(1), gb(2));
|
||||
|
||||
state_ = HealthState::Init;
|
||||
consecutive_lost_ = 0;
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(output_mtx_);
|
||||
latest_output_.reset();
|
||||
}
|
||||
_build_estimator();
|
||||
}
|
||||
|
||||
py::dict health() const {
|
||||
py::dict d;
|
||||
d["state"] = std::string(state_to_str(state_));
|
||||
d["consecutive_lost"] = consecutive_lost_;
|
||||
d["bias_norm"] = std::sqrt(
|
||||
seed_accel_bias_.squaredNorm() + seed_gyro_bias_.squaredNorm());
|
||||
return d;
|
||||
}
|
||||
|
||||
private:
|
||||
void _build_estimator() {
|
||||
// Real wiring: instantiate `vins_estimator::Estimator` from
|
||||
// `yaml_config_`, attach the output callback that fills
|
||||
// `latest_output_` under `output_mtx_` whenever
|
||||
// `processMeasurements` produces a new sliding-window solution.
|
||||
//
|
||||
// The skeleton intentionally throws on any actual frame ingest so a
|
||||
// research binary that loads this binding before AZ-333's estimator
|
||||
// wiring lands cannot silently report misleading poses.
|
||||
estimator_built_ = false;
|
||||
}
|
||||
|
||||
bool _drive_estimator(
|
||||
py::array_t<std::uint8_t,
|
||||
py::array::c_style | py::array::forcecast> /*image*/) {
|
||||
if (!estimator_built_) {
|
||||
// Skeleton path — pybind11 binding compiles and loads but the
|
||||
// VINS-Mono estimator is not yet wired. Tier-2 follow-up wires it.
|
||||
throw VinsMonoFatalException(
|
||||
"VinsMonoBackend: VINS-Mono estimator not yet wired — this "
|
||||
"binding is the AZ-333 skeleton; tier2 follow-up wires "
|
||||
"vins_estimator::Estimator + feature_tracker");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
std::string yaml_config_;
|
||||
Eigen::Matrix3d K_ = Eigen::Matrix3d::Identity();
|
||||
Eigen::Matrix4d seed_body_T_world_ = Eigen::Matrix4d::Identity();
|
||||
Eigen::Vector3d seed_velocity_ = Eigen::Vector3d::Zero();
|
||||
Eigen::Vector3d seed_accel_bias_ = Eigen::Vector3d::Zero();
|
||||
Eigen::Vector3d seed_gyro_bias_ = Eigen::Vector3d::Zero();
|
||||
Eigen::Vector3d last_accel_ = Eigen::Vector3d::Zero();
|
||||
Eigen::Vector3d last_gyro_ = Eigen::Vector3d::Zero();
|
||||
|
||||
HealthState state_ = HealthState::Init;
|
||||
int consecutive_lost_ = 0;
|
||||
std::int64_t last_imu_ts_ns_ = -1;
|
||||
std::string pending_frame_id_;
|
||||
std::int64_t pending_ts_ns_ = 0;
|
||||
bool estimator_built_ = false;
|
||||
|
||||
mutable std::mutex output_mtx_;
|
||||
std::optional<EstimatorOutput> latest_output_;
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
PYBIND11_MODULE(vins_mono_binding, m) {
|
||||
m.doc() =
|
||||
"VINS-Mono pybind11 binding (AZ-333). Wraps the de-ROSified VINS-Mono "
|
||||
"estimator core for the Python VinsMonoStrategy facade. Tier2 follow-up "
|
||||
"wires the real estimator. Research-only — not present in airborne / "
|
||||
"operator-tooling / replay-cli binaries (BUILD_VINS_MONO=OFF).";
|
||||
|
||||
py::register_exception<VinsMonoInitException>(m, "VinsMonoInitException");
|
||||
py::register_exception<VinsMonoFatalException>(m, "VinsMonoFatalException");
|
||||
py::register_exception<VinsMonoOptimizationException>(
|
||||
m, "VinsMonoOptimizationException");
|
||||
|
||||
py::class_<VinsMonoBackend>(m, "VinsMonoBackend")
|
||||
.def(py::init<const std::string&,
|
||||
py::array_t<double, py::array::c_style | py::array::forcecast>>(),
|
||||
py::arg("yaml_config"), py::arg("camera_intrinsics_3x3"))
|
||||
.def("add_frame", &VinsMonoBackend::add_frame, py::arg("frame_id"),
|
||||
py::arg("ts_ns"), py::arg("image"))
|
||||
.def("add_imu", &VinsMonoBackend::add_imu, py::arg("ts_ns"),
|
||||
py::arg("accel"), py::arg("gyro"))
|
||||
.def("get_latest_output", &VinsMonoBackend::get_latest_output)
|
||||
.def("reset", &VinsMonoBackend::reset, py::arg("body_T_world"),
|
||||
py::arg("velocity"), py::arg("accel_bias"), py::arg("gyro_bias"))
|
||||
.def("health", &VinsMonoBackend::health);
|
||||
}
|
||||
@@ -1,4 +1,4 @@
|
||||
"""C1 VIO strategy config block (AZ-331 + AZ-332).
|
||||
"""C1 VIO strategy config block (AZ-331 + AZ-332 + AZ-333).
|
||||
|
||||
Registered into ``config.components['c1_vio']`` by the package
|
||||
``__init__.py``. The composition-root factory
|
||||
@@ -11,6 +11,12 @@ carrying OKVIS2-specific knobs (sliding-window size, parallax-driven
|
||||
keyframe threshold, RANSAC inlier ratio, max optimisation iterations,
|
||||
degraded-feature threshold, per-frame debug log). Only consulted when
|
||||
``strategy == "okvis2"``.
|
||||
|
||||
AZ-333 extends with a sibling :class:`VinsMonoConfig` for the
|
||||
research-only VINS-Mono backend (sliding-window size, feature tracker
|
||||
thresholds, marginalisation strategy, max optimisation iterations,
|
||||
degraded-feature threshold, per-frame debug log). Only consulted when
|
||||
``strategy == "vins_mono"``.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
@@ -24,6 +30,7 @@ __all__ = [
|
||||
"KNOWN_STRATEGIES",
|
||||
"C1VioConfig",
|
||||
"Okvis2Config",
|
||||
"VinsMonoConfig",
|
||||
]
|
||||
|
||||
KNOWN_STRATEGIES: Final[frozenset[str]] = frozenset({"okvis2", "vins_mono", "klt_ransac"})
|
||||
@@ -88,6 +95,85 @@ class Okvis2Config:
|
||||
)
|
||||
|
||||
|
||||
_ALLOWED_VINS_MARGINALISATION: Final[frozenset[str]] = frozenset(
|
||||
{"old", "second_new"}
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class VinsMonoConfig:
|
||||
"""VINS-Mono-specific knobs (AZ-333; research-only backend).
|
||||
|
||||
``sliding_window_size`` is the VINS-Mono optimisation-window size
|
||||
in keyframes — must be in [10, 20] mirroring D-C5-3's K bound.
|
||||
|
||||
``feature_min_tracked`` is the per-frame tracked-feature floor
|
||||
below which the frontend declares the frame untrackable; default
|
||||
20 (VINS-Mono ``MIN_DIST`` upstream default surface).
|
||||
|
||||
``feature_min_parallax_px`` is the parallax-driven keyframe
|
||||
selection threshold; default 10.0 px (VINS-Mono upstream default
|
||||
for 752×480 EuRoC-class fixtures).
|
||||
|
||||
``marginalisation_strategy`` selects ``"old"`` (drop the oldest
|
||||
keyframe and marginalise its prior into the Hessian) or
|
||||
``"second_new"`` (drop the second-newest, used when the newest is
|
||||
a non-keyframe). Both are upstream-supported.
|
||||
|
||||
``max_optimization_iters`` caps the per-frame Ceres solver
|
||||
iterations; default 8 (VINS-Mono upstream default; higher than
|
||||
OKVIS2 because Ceres single-iteration cost is lower).
|
||||
|
||||
``degraded_feature_threshold`` is the per-frame tracked-feature
|
||||
count below which ``health_snapshot`` reports DEGRADED; default 30
|
||||
(matches ``Okvis2Config`` so cross-strategy comparison is fair).
|
||||
|
||||
``per_frame_debug_log`` enables a DEBUG log line per
|
||||
``process_frame`` — OFF by default.
|
||||
"""
|
||||
|
||||
sliding_window_size: int = 10
|
||||
feature_min_tracked: int = 20
|
||||
feature_min_parallax_px: float = 10.0
|
||||
marginalisation_strategy: str = "old"
|
||||
max_optimization_iters: int = 8
|
||||
degraded_feature_threshold: int = 30
|
||||
per_frame_debug_log: bool = False
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
if not (10 <= self.sliding_window_size <= 20):
|
||||
raise ConfigError(
|
||||
"VinsMonoConfig.sliding_window_size must be in [10, 20] "
|
||||
f"(D-C5-3 budget); got {self.sliding_window_size}"
|
||||
)
|
||||
if self.feature_min_tracked < 1:
|
||||
raise ConfigError(
|
||||
"VinsMonoConfig.feature_min_tracked must be >= 1; "
|
||||
f"got {self.feature_min_tracked}"
|
||||
)
|
||||
if self.feature_min_parallax_px <= 0.0:
|
||||
raise ConfigError(
|
||||
"VinsMonoConfig.feature_min_parallax_px must be > 0; "
|
||||
f"got {self.feature_min_parallax_px}"
|
||||
)
|
||||
if self.marginalisation_strategy not in _ALLOWED_VINS_MARGINALISATION:
|
||||
raise ConfigError(
|
||||
"VinsMonoConfig.marginalisation_strategy must be one of "
|
||||
f"{sorted(_ALLOWED_VINS_MARGINALISATION)}; "
|
||||
f"got {self.marginalisation_strategy!r}"
|
||||
)
|
||||
if self.max_optimization_iters < 1:
|
||||
raise ConfigError(
|
||||
"VinsMonoConfig.max_optimization_iters must be >= 1; "
|
||||
f"got {self.max_optimization_iters}"
|
||||
)
|
||||
if self.degraded_feature_threshold < 1:
|
||||
raise ConfigError(
|
||||
"VinsMonoConfig.degraded_feature_threshold must be >= 1; "
|
||||
f"got {self.degraded_feature_threshold}"
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class C1VioConfig:
|
||||
"""Per-component config for C1 VIO.
|
||||
@@ -106,12 +192,16 @@ class C1VioConfig:
|
||||
|
||||
``okvis2`` carries OKVIS2-specific knobs (AZ-332); consulted only
|
||||
when ``strategy == "okvis2"``.
|
||||
|
||||
``vins_mono`` carries VINS-Mono-specific knobs (AZ-333); consulted
|
||||
only when ``strategy == "vins_mono"``.
|
||||
"""
|
||||
|
||||
strategy: str = "klt_ransac"
|
||||
lost_frame_threshold: int = 9
|
||||
warm_start_max_frames: int = 5
|
||||
okvis2: Okvis2Config = field(default_factory=Okvis2Config)
|
||||
vins_mono: VinsMonoConfig = field(default_factory=VinsMonoConfig)
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
if self.strategy not in KNOWN_STRATEGIES:
|
||||
|
||||
@@ -0,0 +1,518 @@
|
||||
"""`VinsMonoStrategy` — research-only comparative C1 VIO (AZ-333).
|
||||
|
||||
Python facade over the VINS-Mono C++ loosely-coupled sliding-window VIO
|
||||
core, accessed via the pybind11 binding at
|
||||
``_native.vins_mono_binding.VinsMonoBackend`` (compiled by
|
||||
``cpp/vins_mono/CMakeLists.txt``, gated by ``BUILD_VINS_MONO=ON``).
|
||||
|
||||
Conforms to the AZ-331 :class:`VioStrategy` Protocol; consumes the
|
||||
runtime ``Config`` + an :class:`FdrClient`; constructs its other
|
||||
dependencies (logger, camera calibration) internally from ``config``
|
||||
so the strategy class matches the composition-root factory shape::
|
||||
|
||||
strategy_cls(config: Config, *, fdr_client: FdrClient)
|
||||
|
||||
This mirrors :class:`Okvis2Strategy` (AZ-332) deliberately: the AZ-331
|
||||
factory produces both via the same `(config, *, fdr_client)` shape and
|
||||
the IT-12 comparative-study harness expects the two to be drop-in
|
||||
substitutable. Behavioural differences (Ceres vs Levenberg-Marquardt,
|
||||
loosely-coupled vs tightly-coupled, marginalisation strategy) live
|
||||
under the binding boundary and are observable only via the latency /
|
||||
covariance numbers in the Step 9 comparative report — NOT via the
|
||||
Python surface.
|
||||
|
||||
Risk-2 / Risk-3 mitigation: the native binding is imported **lazily
|
||||
inside the constructor**, not at module top level. Importing this
|
||||
module with ``BUILD_VINS_MONO=OFF`` (no compiled ``.so``) is safe —
|
||||
the AZ-331 factory's build-flag gate catches that path before the
|
||||
constructor runs.
|
||||
|
||||
AC mapping (see ``_docs/02_tasks/todo/AZ-333_c1_vins_mono_strategy.md``):
|
||||
|
||||
- AC-1 : :meth:`current_strategy_label` returns ``"vins_mono"``.
|
||||
- AC-2 : :meth:`process_frame` returns :class:`VioOutput` with
|
||||
``frame_id`` echoed; covariance SPD; ``imu_bias`` non-None.
|
||||
- AC-3 : all backend / Ceres / Eigen / std::runtime_error rewrap into
|
||||
:class:`VioError` family with ``__cause__`` chain.
|
||||
- AC-4 : :meth:`reset_to_warm_start` clears state + seeds hint; second
|
||||
consecutive call does not raise.
|
||||
- AC-5 : :meth:`health_snapshot` returns INIT initially, TRACKING after
|
||||
``warm_start_max_frames`` (default 5) successful frames.
|
||||
- AC-6 : DEGRADED on feature loss; covariance Frobenius norm strictly
|
||||
increases; ``process_frame`` still returns :class:`VioOutput` (not raise).
|
||||
- AC-7 : after ``lost_frame_threshold`` (default 9) consecutive failed
|
||||
frames, raises :class:`VioFatalError`; state == LOST.
|
||||
- AC-8 : ``BUILD_VINS_MONO=OFF`` does not load this module (enforced by
|
||||
AZ-331 factory; covered in
|
||||
``tests/unit/c1_vio/test_protocol_conformance.py``).
|
||||
- AC-9 / NFR-perf : tier2 — Jetson + Derkachi-class fixture; tests
|
||||
marked ``@pytest.mark.tier2``.
|
||||
- AC-10 : exactly one ``vio.health`` FDR record per state transition;
|
||||
no spam on steady-state.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from datetime import datetime, timezone
|
||||
from typing import TYPE_CHECKING, Any, Final, Literal
|
||||
|
||||
import numpy as np
|
||||
|
||||
from gps_denied_onboard._types.nav import (
|
||||
FeatureQuality,
|
||||
ImuBias,
|
||||
VioHealth,
|
||||
VioOutput,
|
||||
VioState,
|
||||
)
|
||||
from gps_denied_onboard.clock.wall_clock import WallClock
|
||||
from gps_denied_onboard.components.c1_vio.errors import (
|
||||
VioFatalError,
|
||||
VioInitializingError,
|
||||
)
|
||||
from gps_denied_onboard.fdr_client.records import CURRENT_SCHEMA_VERSION, FdrRecord
|
||||
from gps_denied_onboard.logging import get_logger
|
||||
|
||||
if TYPE_CHECKING:
|
||||
import numpy.typing as npt
|
||||
|
||||
from gps_denied_onboard._types.calibration import CameraCalibration
|
||||
from gps_denied_onboard._types.nav import (
|
||||
ImuWindow,
|
||||
NavCameraFrame,
|
||||
WarmStartPose,
|
||||
)
|
||||
from gps_denied_onboard.clock import Clock
|
||||
from gps_denied_onboard.components.c1_vio.config import VinsMonoConfig
|
||||
from gps_denied_onboard.config import Config
|
||||
from gps_denied_onboard.fdr_client.client import FdrClient
|
||||
|
||||
__all__ = ["VinsMonoStrategy"]
|
||||
|
||||
|
||||
_STRATEGY_LABEL: Final[Literal["vins_mono"]] = "vins_mono"
|
||||
_PRODUCER_ID: Final[str] = "c1_vio.vins_mono"
|
||||
_LOGGER_COMPONENT: Final[str] = "c1_vio.vins_mono"
|
||||
|
||||
|
||||
def _now_iso() -> str:
|
||||
return datetime.now(timezone.utc).isoformat()
|
||||
|
||||
|
||||
def _bias_norm(bias: ImuBias) -> float:
|
||||
"""L2 norm of the concatenated 6-vector ``(accel || gyro)``."""
|
||||
accel = np.asarray(bias.accel_bias, dtype=np.float64)
|
||||
gyro = np.asarray(bias.gyro_bias, dtype=np.float64)
|
||||
return float(math.sqrt(float(np.dot(accel, accel) + np.dot(gyro, gyro))))
|
||||
|
||||
|
||||
def _se3_from_4x4(matrix: npt.NDArray[Any]) -> Any:
|
||||
"""Build a ``gtsam.Pose3`` from a 4x4 row-major matrix.
|
||||
|
||||
Imported lazily so this module can be imported without gtsam in
|
||||
headless tooling paths (tests + facade-only smoke).
|
||||
"""
|
||||
import gtsam
|
||||
|
||||
return gtsam.Pose3(np.asarray(matrix, dtype=np.float64))
|
||||
|
||||
|
||||
class VinsMonoStrategy:
|
||||
"""Research-only :class:`VioStrategy` for IT-12 comparative study (AZ-333).
|
||||
|
||||
Constructor matches the AZ-331 composition-root factory shape::
|
||||
|
||||
VinsMonoStrategy(config: Config, *, fdr_client: FdrClient)
|
||||
|
||||
Other dependencies (calibration, logger, VINS-Mono sub-config) are
|
||||
resolved internally from ``config``. Per the C1 component
|
||||
`tests.md` C1-IT-04, the AC-2.2 MRE bound is **exempt** for this
|
||||
strategy.
|
||||
|
||||
Concurrency: single-threaded by Protocol invariant. One instance
|
||||
per camera-ingest writer thread; concurrent ``process_frame`` calls
|
||||
are undefined behaviour.
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
config: Config,
|
||||
*,
|
||||
fdr_client: FdrClient,
|
||||
clock: Clock | None = None,
|
||||
) -> None:
|
||||
c1_block = config.components["c1_vio"]
|
||||
if c1_block.strategy != _STRATEGY_LABEL:
|
||||
raise VioFatalError(
|
||||
f"VinsMonoStrategy constructed with config.strategy="
|
||||
f"{c1_block.strategy!r}; expected {_STRATEGY_LABEL!r}. "
|
||||
"The AZ-331 factory is the only sanctioned constructor."
|
||||
)
|
||||
|
||||
self._config = config
|
||||
self._fdr = fdr_client
|
||||
self._clock: Clock = clock if clock is not None else WallClock()
|
||||
self._logger = get_logger(_LOGGER_COMPONENT)
|
||||
self._lost_frame_threshold: int = c1_block.lost_frame_threshold
|
||||
self._warm_start_max_frames: int = c1_block.warm_start_max_frames
|
||||
self._vins_cfg: VinsMonoConfig = c1_block.vins_mono
|
||||
self._calibration: CameraCalibration | None = None
|
||||
self._frames_since_warmup: int = 0
|
||||
self._consecutive_lost: int = 0
|
||||
self._latest_bias: ImuBias = ImuBias(
|
||||
accel_bias=(0.0, 0.0, 0.0), gyro_bias=(0.0, 0.0, 0.0)
|
||||
)
|
||||
self._reported_state: VioState = VioState.INIT
|
||||
self._last_emitted_state: VioState | None = None
|
||||
|
||||
# Lazy import of the native binding — Risk-2 / Risk-3 mitigation.
|
||||
# Failure here is the BUILD_VINS_MONO=OFF path the AZ-331
|
||||
# factory's `StrategyNotAvailableError` is meant to prevent; if a
|
||||
# caller bypasses the factory and reaches this constructor with
|
||||
# the native lib absent, we surface a fatal init error.
|
||||
try:
|
||||
from gps_denied_onboard.components.c1_vio._native import (
|
||||
vins_mono_binding,
|
||||
)
|
||||
except ImportError as exc:
|
||||
raise VioFatalError(
|
||||
"VinsMonoStrategy: native binding "
|
||||
"(gps_denied_onboard.components.c1_vio._native.vins_mono_binding) "
|
||||
"is not importable. Research binary must be built with "
|
||||
"BUILD_VINS_MONO=ON; deployment binaries (airborne / "
|
||||
"operator-tooling / replay-cli) must NOT request strategy="
|
||||
"'vins_mono'."
|
||||
) from exc
|
||||
|
||||
self._binding_module = vins_mono_binding
|
||||
self._backend = self._construct_backend()
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Public Protocol surface.
|
||||
|
||||
def process_frame(
|
||||
self,
|
||||
frame: NavCameraFrame,
|
||||
imu: ImuWindow,
|
||||
calibration: CameraCalibration,
|
||||
) -> VioOutput:
|
||||
"""Hot-path call — one per nav-camera frame.
|
||||
|
||||
Steps:
|
||||
|
||||
1. Push every IMU sample in the window into the backend; the
|
||||
strict-monotonic guard lives on the C++ side.
|
||||
2. Submit the frame.
|
||||
3. If the backend produced an output, classify health and
|
||||
build the :class:`VioOutput` DTO.
|
||||
4. If no output: tick the lost-frame counter; emit a state
|
||||
transition record if needed.
|
||||
"""
|
||||
self._calibration = calibration
|
||||
frame_id_str = str(frame.frame_id)
|
||||
emitted_at_ns = self._clock.monotonic_ns()
|
||||
|
||||
try:
|
||||
self._push_imu_window(imu)
|
||||
produced = self._backend.add_frame(
|
||||
frame_id_str, _frame_ts_ns(frame), _frame_image(frame)
|
||||
)
|
||||
except self._binding_module.VinsMonoInitException as exc:
|
||||
self._emit_transition(VioState.INIT, frame_id_str)
|
||||
raise VioInitializingError(
|
||||
f"VINS-Mono backend reports INIT while processing frame "
|
||||
f"{frame_id_str!r}: {exc}"
|
||||
) from exc
|
||||
except self._binding_module.VinsMonoOptimizationException as exc:
|
||||
# Treat as a degraded frame: emit no VioOutput from this
|
||||
# path — callers expect either a VioOutput or a VioError;
|
||||
# we choose error here so C5 can fall back, matching AC-3.
|
||||
self._tick_lost(frame_id_str)
|
||||
if self._reported_state == VioState.LOST:
|
||||
self._emit_transition(VioState.LOST, frame_id_str)
|
||||
raise VioFatalError(
|
||||
f"VINS-Mono backend exhausted lost-frame budget at "
|
||||
f"{frame_id_str!r}: {exc}"
|
||||
) from exc
|
||||
self._emit_transition(self._reported_state, frame_id_str)
|
||||
raise VioInitializingError(
|
||||
f"VINS-Mono backend optimisation failure at {frame_id_str!r}: {exc}"
|
||||
) from exc
|
||||
except self._binding_module.VinsMonoFatalException as exc:
|
||||
self._emit_transition(VioState.LOST, frame_id_str)
|
||||
raise VioFatalError(
|
||||
f"VINS-Mono backend fatal exception at {frame_id_str!r}: {exc}"
|
||||
) from exc
|
||||
except (RuntimeError, ValueError) as exc:
|
||||
# Catch-all for unmapped backend exceptions. Re-classify as
|
||||
# fatal — we explicitly forbid raw library exceptions across
|
||||
# the public boundary.
|
||||
raise VioFatalError(
|
||||
f"VINS-Mono backend raised an unmapped exception at "
|
||||
f"{frame_id_str!r}: {exc}"
|
||||
) from exc
|
||||
|
||||
if not produced:
|
||||
# Frame consumed but no estimator update yet — INIT path
|
||||
# while VINS-Mono's SfM bootstrap warms up.
|
||||
self._emit_transition(VioState.INIT, frame_id_str)
|
||||
raise VioInitializingError(
|
||||
f"VinsMonoStrategy: backend has not yet emitted an "
|
||||
f"estimator update at {frame_id_str!r}"
|
||||
)
|
||||
|
||||
raw = self._backend.get_latest_output()
|
||||
if raw is None:
|
||||
raise VioFatalError(
|
||||
f"VinsMonoStrategy: backend reported a new output for "
|
||||
f"{frame_id_str!r} but get_latest_output() returned None"
|
||||
)
|
||||
|
||||
vio_output = self._build_vio_output(raw, emitted_at_ns)
|
||||
self._consecutive_lost = 0
|
||||
new_state = self._classify_state(vio_output.feature_quality)
|
||||
if new_state != self._reported_state:
|
||||
self._reported_state = new_state
|
||||
self._emit_transition(new_state, frame_id_str)
|
||||
|
||||
if new_state in (VioState.INIT, VioState.TRACKING):
|
||||
self._frames_since_warmup += 1
|
||||
|
||||
if self._vins_cfg.per_frame_debug_log:
|
||||
self._logger.debug(
|
||||
"vins_mono.process_frame",
|
||||
extra={
|
||||
"component": _LOGGER_COMPONENT,
|
||||
"kind": "vio.tick",
|
||||
"frame_id": frame_id_str,
|
||||
"kv": {
|
||||
"state": self._reported_state.value,
|
||||
"tracked": vio_output.feature_quality.tracked,
|
||||
"mre_px": vio_output.feature_quality.mre_px,
|
||||
"emitted_at_ns": vio_output.emitted_at_ns,
|
||||
},
|
||||
},
|
||||
)
|
||||
|
||||
return vio_output
|
||||
|
||||
def reset_to_warm_start(self, hint: WarmStartPose) -> None:
|
||||
"""Destructive re-init from an F8-reboot warm-start hint.
|
||||
|
||||
Idempotent across consecutive calls (AC-4) — a second call
|
||||
without an intervening ``process_frame`` reseats the backend
|
||||
again without raising.
|
||||
"""
|
||||
try:
|
||||
body_T_world = np.asarray(hint.body_T_world.matrix(), dtype=np.float64)
|
||||
except AttributeError as exc:
|
||||
raise VioFatalError(
|
||||
"VinsMonoStrategy.reset_to_warm_start: hint.body_T_world is "
|
||||
"not a gtsam.Pose3 (missing .matrix())"
|
||||
) from exc
|
||||
|
||||
velocity = np.asarray(hint.velocity_b, dtype=np.float64)
|
||||
accel_bias = np.asarray(hint.bias.accel_bias, dtype=np.float64)
|
||||
gyro_bias = np.asarray(hint.bias.gyro_bias, dtype=np.float64)
|
||||
|
||||
try:
|
||||
self._backend.reset(body_T_world, velocity, accel_bias, gyro_bias)
|
||||
except self._binding_module.VinsMonoInitException as exc:
|
||||
raise VioFatalError(
|
||||
f"VINS-Mono backend rejected warm-start reset: {exc}"
|
||||
) from exc
|
||||
except (RuntimeError, ValueError) as exc:
|
||||
raise VioFatalError(
|
||||
f"VINS-Mono backend raised an unmapped exception during reset: {exc}"
|
||||
) from exc
|
||||
|
||||
self._latest_bias = hint.bias
|
||||
self._frames_since_warmup = 0
|
||||
self._consecutive_lost = 0
|
||||
self._reported_state = VioState.INIT
|
||||
self._emit_transition(VioState.INIT, frame_id="")
|
||||
|
||||
def health_snapshot(self) -> VioHealth:
|
||||
"""Most-recent health state — no backend call (cheap)."""
|
||||
return VioHealth(
|
||||
state=self._reported_state,
|
||||
consecutive_lost=self._consecutive_lost,
|
||||
bias_norm=_bias_norm(self._latest_bias),
|
||||
)
|
||||
|
||||
def current_strategy_label(self) -> Literal["okvis2", "vins_mono", "klt_ransac"]:
|
||||
return _STRATEGY_LABEL
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Internal helpers.
|
||||
|
||||
def _construct_backend(self) -> Any:
|
||||
"""Build the backend from config — calibration path is optional
|
||||
because the unit-test fake-binding path skips real intrinsics.
|
||||
|
||||
Tests inject a fake module at ``sys.modules`` before construction
|
||||
(see ``tests/unit/c1_vio/conftest.py``); the fake's
|
||||
``VinsMonoBackend`` accepts whatever this method passes.
|
||||
"""
|
||||
K = self._load_camera_intrinsics()
|
||||
yaml_config = self._render_yaml_config()
|
||||
try:
|
||||
return self._binding_module.VinsMonoBackend(yaml_config, K)
|
||||
except self._binding_module.VinsMonoInitException as exc:
|
||||
raise VioFatalError(
|
||||
f"VinsMonoStrategy: backend init failed: {exc}"
|
||||
) from exc
|
||||
|
||||
def _load_camera_intrinsics(self) -> np.ndarray:
|
||||
"""Load 3x3 camera intrinsics from the calibration path.
|
||||
|
||||
Returns the identity matrix when the runtime block has no
|
||||
path configured — the unit-test path overrides this via the
|
||||
fake binding's ctor anyway, and a research binary refusing
|
||||
to start on a missing calibration is preferable to silently
|
||||
emitting wrong poses (handled by the YAML loader downstream).
|
||||
"""
|
||||
path = self._config.runtime.camera_calibration_path
|
||||
if not path:
|
||||
return np.eye(3, dtype=np.float64)
|
||||
try:
|
||||
import json
|
||||
|
||||
with open(path, encoding="utf-8") as fh:
|
||||
blob = json.load(fh)
|
||||
except (OSError, ValueError) as exc:
|
||||
raise VioFatalError(
|
||||
f"VinsMonoStrategy: failed to load camera calibration from "
|
||||
f"{path!r}: {exc}"
|
||||
) from exc
|
||||
K_raw = blob.get("intrinsics_3x3")
|
||||
if K_raw is None:
|
||||
raise VioFatalError(
|
||||
f"VinsMonoStrategy: calibration file {path!r} is missing the "
|
||||
"'intrinsics_3x3' field"
|
||||
)
|
||||
K = np.asarray(K_raw, dtype=np.float64)
|
||||
if K.shape != (3, 3):
|
||||
raise VioFatalError(
|
||||
f"VinsMonoStrategy: intrinsics_3x3 must be 3x3; got shape {K.shape}"
|
||||
)
|
||||
return K
|
||||
|
||||
def _render_yaml_config(self) -> str:
|
||||
"""Render the VinsMonoConfig sub-block into a VINS-Mono YAML snippet.
|
||||
|
||||
VINS-Mono reads a YAML config string at construction. Only the
|
||||
knobs AZ-333 exposes are rendered; VINS-Mono-internal defaults
|
||||
cover the rest.
|
||||
"""
|
||||
cfg = self._vins_cfg
|
||||
return (
|
||||
"# AZ-333 — generated VINS-Mono config (see VinsMonoConfig in c1_vio/config.py)\n"
|
||||
f"sliding_window_size: {cfg.sliding_window_size}\n"
|
||||
f"feature_min_tracked: {cfg.feature_min_tracked}\n"
|
||||
f"feature_min_parallax_px: {cfg.feature_min_parallax_px}\n"
|
||||
f"marginalisation_strategy: {cfg.marginalisation_strategy}\n"
|
||||
f"max_optimization_iters: {cfg.max_optimization_iters}\n"
|
||||
)
|
||||
|
||||
def _push_imu_window(self, imu: ImuWindow) -> None:
|
||||
for sample in imu.samples:
|
||||
self._backend.add_imu(
|
||||
sample.ts_ns,
|
||||
np.asarray(sample.accel_xyz, dtype=np.float64),
|
||||
np.asarray(sample.gyro_xyz, dtype=np.float64),
|
||||
)
|
||||
|
||||
def _build_vio_output(self, raw: dict[str, Any], emitted_at_ns: int) -> VioOutput:
|
||||
try:
|
||||
pose = _se3_from_4x4(raw["pose_T_world_body"])
|
||||
cov = np.asarray(raw["pose_covariance_6x6"], dtype=np.float64)
|
||||
bias = ImuBias(
|
||||
accel_bias=tuple(float(x) for x in raw["accel_bias"]), # type: ignore[arg-type]
|
||||
gyro_bias=tuple(float(x) for x in raw["gyro_bias"]), # type: ignore[arg-type]
|
||||
)
|
||||
feature_quality = FeatureQuality(
|
||||
tracked=int(raw["tracked_features"]),
|
||||
new=int(raw["new_features"]),
|
||||
lost=int(raw["lost_features"]),
|
||||
mean_parallax=float(raw["mean_parallax"]),
|
||||
mre_px=float(raw["mre_px"]),
|
||||
)
|
||||
backend_ts = int(raw.get("emitted_at_ns") or emitted_at_ns)
|
||||
except (KeyError, TypeError, ValueError) as exc:
|
||||
raise VioFatalError(
|
||||
f"VinsMonoStrategy: backend output is malformed: {exc}"
|
||||
) from exc
|
||||
|
||||
if cov.shape != (6, 6):
|
||||
raise VioFatalError(
|
||||
f"VinsMonoStrategy: pose_covariance_6x6 has shape {cov.shape}; "
|
||||
"expected (6, 6)"
|
||||
)
|
||||
|
||||
self._latest_bias = bias
|
||||
return VioOutput(
|
||||
frame_id=raw["frame_id"],
|
||||
relative_pose_T=pose,
|
||||
pose_covariance_6x6=cov,
|
||||
imu_bias=bias,
|
||||
feature_quality=feature_quality,
|
||||
emitted_at_ns=backend_ts,
|
||||
)
|
||||
|
||||
def _classify_state(self, fq: FeatureQuality) -> VioState:
|
||||
if self._reported_state == VioState.INIT and (
|
||||
self._frames_since_warmup + 1 < self._warm_start_max_frames
|
||||
):
|
||||
return VioState.INIT
|
||||
if fq.tracked < self._vins_cfg.degraded_feature_threshold:
|
||||
return VioState.DEGRADED
|
||||
return VioState.TRACKING
|
||||
|
||||
def _tick_lost(self, frame_id: str) -> None:
|
||||
self._consecutive_lost += 1
|
||||
if self._consecutive_lost >= self._lost_frame_threshold:
|
||||
self._reported_state = VioState.LOST
|
||||
elif self._reported_state == VioState.TRACKING:
|
||||
self._reported_state = VioState.DEGRADED
|
||||
|
||||
def _emit_transition(self, new_state: VioState, frame_id: str) -> None:
|
||||
if self._last_emitted_state == new_state:
|
||||
return
|
||||
self._last_emitted_state = new_state
|
||||
record = FdrRecord(
|
||||
schema_version=CURRENT_SCHEMA_VERSION,
|
||||
ts=_now_iso(),
|
||||
producer_id=_PRODUCER_ID,
|
||||
kind="vio.health",
|
||||
payload={
|
||||
"state": new_state.value,
|
||||
"consecutive_lost": self._consecutive_lost,
|
||||
"bias_norm": _bias_norm(self._latest_bias),
|
||||
"strategy_label": _STRATEGY_LABEL,
|
||||
"frame_id": frame_id,
|
||||
},
|
||||
)
|
||||
self._fdr.enqueue(record)
|
||||
|
||||
|
||||
def _frame_ts_ns(frame: NavCameraFrame) -> int:
|
||||
"""Convert ``NavCameraFrame.timestamp`` to monotonic-ns.
|
||||
|
||||
Uses the datetime's UTC epoch nanoseconds so the value is
|
||||
monotonically increasing across frames (frame source guarantees
|
||||
strictly increasing timestamps per the FrameSource contract).
|
||||
"""
|
||||
return int(frame.timestamp.timestamp() * 1e9)
|
||||
|
||||
|
||||
def _frame_image(frame: NavCameraFrame) -> np.ndarray:
|
||||
"""Coerce the frame's image into a contiguous uint8 ndarray."""
|
||||
arr = np.ascontiguousarray(frame.image, dtype=np.uint8)
|
||||
if arr.ndim < 2 or arr.ndim > 3:
|
||||
raise VioFatalError(
|
||||
f"VinsMonoStrategy: NavCameraFrame.image must be 2-D or 3-D; "
|
||||
f"got {arr.ndim}-D"
|
||||
)
|
||||
return arr
|
||||
Reference in New Issue
Block a user