odometry_fusion: one error-state filter for every source, with or without an IMU

README.md

@@ -16,12 +16,15 @@ the per-platform SDK packaging live in
   set to this machine's default.
 
 ```rust
-use dim_slam::{FusionCore, OdometryFusionConfig, SourceConfig};
+use dim_slam::{FusionCore, OdometryFusionConfig, SourceConfig, SourceKind};
 
+// Sources are `pose` (a non-drifting world frame), `twist` (body velocity) or `pose_delta`
+// (a drifting pose, fused by its motion only).
 let mut fusion = FusionCore::new(OdometryFusionConfig {
     odom_sources: vec![SourceConfig {
         parent_frame_id: "visual_odom".into(),
         child_frame_id: "base_link".into(),
+        kind: SourceKind::PoseDelta,
         pose_variances: [1e-4; 6],
         ..Default::default()
     }],

python/Cargo.toml

@@ -5,7 +5,7 @@
 name = "dim_odom"
 # Tracks the repository's release tag, so a consumer pinning the rust crate at a
 # tag and the wheel at a version can see at a glance that they agree.
-version = "0.4.2"
+version = "0.5.0-beta.3"
 edition = "2021"
 license = "Apache-2.0"
 description = "Python bindings for dimSLAM: cuVSLAM visual odometry plus error-state Kalman fusion"

python/README.md

@@ -2,8 +2,8 @@
 
 Python bindings for [dimSLAM](https://github.com/dimensionalOS/dimSLAM):
 `CuvslamOdometry` wraps the cuVSLAM visual(-inertial) odometry front end,
-`OdometryFusion` the error-state Kalman filter that fuses odometry sources with an
-IMU. Everything runs in-process and on the caller's data stamps, so replaying a
+`OdometryFusion` the error-state Kalman filter that fuses odometry sources, with or
+without an IMU. Everything runs in-process and on the caller's data stamps, so replaying a
 recording is deterministic.
 
 ```python
@@ -16,7 +16,9 @@ tracker = dim_odom.CuvslamOdometry(
 tracker.handle_camera_info(dim_odom.CameraModel(...))
 estimate = tracker.handle_image(dim_odom.ImageFrame(...))
 
-fusion = dim_odom.OdometryFusion({"odom_sources": [{"parent_frame_id": "odom", "child_frame_id": "base_link"}]})
+fusion = dim_odom.OdometryFusion({"odom_sources": [
+    {"parent_frame_id": "odom", "child_frame_id": "base_link", "kind": "pose_delta", "pose_variances": [1e-4] * 6},
+]})
 if estimate is not None:
     fusion.handle_source(estimate)
 fused = fusion.maybe_publish()

python/build_fallback_wheel.sh

@@ -52,5 +52,6 @@ mkdir -p "$output"
 python3 -m pip wheel --no-deps --wheel-dir "$output" "$build"
 
 # A setuptools older than 61 ignores the [project] table and silently emits a
-# UNKNOWN-0.0.0 wheel, which uploads to PyPI just as happily as a real one.
-test -f "$output/dim_odom-$version-py3-none-any.whl"
+# UNKNOWN-0.0.0 wheel, which uploads to PyPI just as happily as a real one. The
+# wheel spells the version normalized (0.5.0-beta.1 -> 0.5.0b1), hence the glob.
+compgen -G "$output/dim_odom-[0-9]*-py3-none-any.whl" >/dev/null

python/src/lib.rs

@@ -448,7 +448,8 @@ impl CuvslamOdometry {
 }
 
 /// The error-state Kalman fuser. `config` mirrors the Rust `OdometryFusionConfig`
-/// as a dict; `odom_sources` entries name their parent_frame_id and child_frame_id.
+/// as a dict; `odom_sources` entries name their parent_frame_id, child_frame_id and
+/// kind ("pose", "twist" or "pose_delta"). Unknown keys are refused.
 #[pyclass]
 struct OdometryFusion {
     core: FusionCore,
@@ -474,12 +475,16 @@ impl OdometryFusion {
         self.core.handle_source(&estimate.inner);
     }
 
-    /// The fused estimate once the publish period has elapsed on the data clock,
-    /// else None. Call after each input.
-    fn maybe_publish(&mut self) -> Option<OdometryEstimate> {
-        self.core
-            .maybe_publish()
-            .map(|inner| OdometryEstimate { inner })
+    /// The fused estimate, else None. Without `now_ns`: at the newest input's stamp once
+    /// the publish period has elapsed (call after each input). With it: predicted to that
+    /// time, whenever it is past the last output (call on a timer).
+    #[pyo3(signature = (now_ns=None))]
+    fn maybe_publish(&mut self, now_ns: Option<i64>) -> Option<OdometryEstimate> {
+        match now_ns {
+            Some(now_ns) => self.core.maybe_publish_at(now_ns),
+            None => self.core.maybe_publish(),
+        }
+        .map(|inner| OdometryEstimate { inner })
     }
 
     fn report(&self) {

rust/src/dim_slam_core.rs

@@ -9,7 +9,8 @@ use serde::{Deserialize, Serialize};
 
 use crate::cuvslam_odometry::{CameraConfig, CuvslamCore, CuvslamOdometryConfig};
 use crate::odometry_fusion::{
-    FusionCore, ImuConfig, InitialStds, OdometryFusionConfig, SourceConfig,
+    FusionCore, ImuConfig, InitialStds, OdometryFusionConfig, ProcessNoise, SourceConfig,
+    SourceKind,
 };
 use crate::types::{CameraModel, ImageFrame, ImuSample, OdometryEstimate, PointCloud, TfLookup};
 use crate::warn_throttled;
@@ -41,35 +42,39 @@ pub struct DimSlamCoreConfig {
     pub speed_gate_max_angular: f64,
     /// Stamp spread one frame set may span, milliseconds; 0 keeps cuVSLAM's 1 ms contract.
     pub max_skew_ms: f64,
-    /// Trust in the visual odometry, [x y z roll pitch yaw]. Negative takes the message
-    /// covariance, zero drops the dimension, positive is a fixed variance. It drifts, so its
-    /// covariance describes accumulated drift rather than the delta: prefer a fixed value.
+    /// Noise of the tracker's frame-to-frame motion, [x y z roll pitch yaw]: it drifts, so
+    /// it is fused as a `pose_delta` source. Negative takes the message covariance, zero drops
+    /// the dimension, positive is a fixed variance. Its covariance describes accumulated
+    /// drift rather than one frame's motion: prefer a fixed value.
     pub visual_odom_pose_variances: [f64; 6],
 
     /// Frame the fused output is stamped in.
     pub odom_frame_id: String,
     /// The body frame everything is resolved into: the tracker's output and the frame the
     /// filter estimates.
     pub output_frame_id: String,
-    /// Output cadence; the filter itself runs at the IMU rate.
+    /// Output cadence; each output is the filter predicted to its own stamp.
     pub publish_rate: f64,
     /// How far back a late measurement can reach before it is dropped.
     pub replay_buffer_seconds: f64,
-    /// Outlier gate in variance units; smaller is more aggressive, 0 is no gate.
+    /// Outlier gate: squared Mahalanobis distance per measurement dimension; 0 is no gate.
     pub outlier_rejection_allowed_variance: f64,
     /// Gate on the filter's own state, which can run away with no bad measurement; 0 disables.
     pub max_position_m: f64,
-    /// Empty disables inertial propagation; each entry keeps its own noise figures and biases.
+    /// Empty predicts by constant velocity instead; each entry keeps its own noise figures
+    /// and biases.
     pub imus: Vec<ImuConfig>,
     /// m/s^2, seeding the filter's gravity estimate.
     pub initial_gravity_estimate: f64,
     pub initial_stds: InitialStds,
-    /// External sources, each identified by the transform its estimates carry. The visual
-    /// odometry is prepended to this list, so a no-IMU blend prefers it.
+    /// The constant-velocity model's noise; only used without IMUs.
+    pub process_noise: ProcessNoise,
+    /// External sources, each identified by the transform its estimates carry.
     pub odom_sources: Vec<SourceConfig>,
-    /// Virtual zero-twist [vx vy vz wx wy wz] fused after each source update (needs an IMU):
-    /// a positive entry pins that dimension to zero with this variance, 0 frees it.
-    pub per_dimension_error_variance: [f64; 6],
+    /// A ground body: z, roll, pitch and their rates held at zero.
+    pub planar: bool,
+    /// See [`OdometryFusionConfig::zero_velocity_variances`].
+    pub zero_velocity_variances: [f64; 6],
 }
 
 /// Defers to the two cores so a field's default cannot drift from the core that reads it.
@@ -97,13 +102,15 @@ impl Default for DimSlamCoreConfig {
             imus: fusion.imus,
             initial_gravity_estimate: fusion.initial_gravity_estimate,
             initial_stds: fusion.initial_stds,
+            process_noise: fusion.process_noise,
             odom_sources: fusion.odom_sources,
-            per_dimension_error_variance: fusion.per_dimension_error_variance,
+            planar: fusion.planar,
+            zero_velocity_variances: fusion.zero_velocity_variances,
         }
     }
 }
 
-/// Visual-inertial odometry: cuVSLAM tracking blended with any other odometry source.
+/// Visual-inertial odometry: cuVSLAM tracking fused with any other odometry source.
 ///
 /// Feed it frames, IMU samples and other sources' estimates, then ask for output. The tracker's
 /// pose is fused in-process and never leaves the struct.
@@ -134,8 +141,9 @@ impl DimSlamCore {
         let mut odom_sources = vec![SourceConfig {
             parent_frame_id: VISUAL_ODOM_FRAME_ID.to_string(),
             child_frame_id: config.output_frame_id.clone(),
+            kind: SourceKind::PoseDelta,
             pose_variances: config.visual_odom_pose_variances,
-            twist_variances: [0.0; 6],
+            ..Default::default()
         }];
         odom_sources.extend(config.odom_sources);
         let fusion_config = OdometryFusionConfig {
@@ -148,8 +156,10 @@ impl DimSlamCore {
             imus: config.imus,
             initial_gravity_estimate: config.initial_gravity_estimate,
             initial_stds: config.initial_stds,
+            process_noise: config.process_noise,
             odom_sources,
-            per_dimension_error_variance: config.per_dimension_error_variance,
+            planar: config.planar,
+            zero_velocity_variances: config.zero_velocity_variances,
         };
         Ok(Self {
             visual_odometry,
@@ -206,6 +216,11 @@ impl DimSlamCore {
         self.fusion.maybe_publish()
     }
 
+    /// The fused pose predicted to `now_ns`, for a caller publishing on its own timer.
+    pub fn maybe_publish_at(&mut self, now_ns: i64) -> Option<OdometryEstimate> {
+        self.fusion.maybe_publish_at(now_ns)
+    }
+
     pub fn report(&self) {
         self.visual_odometry.report();
         self.fusion.report();

rust/src/lib.rs

@@ -7,39 +7,43 @@
 //! source, then ask for the fused pose. It owns the tracker's wiring into the filter.
 //!
 //! The two halves are also public on their own. [`CuvslamCore`] turns camera, depth and IMU
-//! frames into a pose; [`FusionCore`] blends a pose with any other odometry source against IMU
-//! propagation. Reach for them only when the wiring itself has to differ.
+//! frames into a pose; [`FusionCore`] is one error-state Kalman filter over any odometry
+//! sources, predicting by IMU or, without one, by constant velocity. Reach for them only when
+//! the wiring itself has to differ.
 //!
 //! All are synchronous and own no threads or transport.
 //!
 //! ```
 //! use dim_slam::nalgebra::Isometry3;
-//! use dim_slam::{DimSlamCore, DimSlamCoreConfig, OdometryEstimate, SourceConfig};
+//! use dim_slam::nalgebra::Vector3;
+//! use dim_slam::{DimSlamCore, DimSlamCoreConfig, OdometryEstimate, SourceConfig, SourceKind, Twist};
 //!
 //! // An odometry source is identified by the transform its estimates carry:
-//! // the message's frame_id -> child_frame_id.
+//! // the message's frame_id -> child_frame_id. Wheels measure the body's velocity.
 //! let mut slam = DimSlamCore::new(DimSlamCoreConfig {
 //!     visual_odom_pose_variances: [1e-4; 6],
 //!     odom_sources: vec![SourceConfig {
 //!         parent_frame_id: "wheel_odom".into(),
 //!         child_frame_id: "base_link".into(),
-//!         pose_variances: [1e-4; 6],
-//!         twist_variances: [0.0; 6],
+//!         kind: SourceKind::Twist,
+//!         twist_variances: [1e-4, 1e-4, 0.0, 0.0, 0.0, 1e-4],
+//!         ..Default::default()
 //!     }],
+//!     planar: true,
 //!     ..Default::default()
 //! })
 //! .expect("a usable config");
-//! for (step, timestamp_ns) in [0, 20_000_000, 40_000_000].into_iter().enumerate() {
+//! for step in 0..=50 {
 //!     slam.handle_source(&OdometryEstimate {
-//!         timestamp_ns,
+//!         timestamp_ns: step * 20_000_000,
 //!         frame_id: "wheel_odom".into(),
 //!         child_frame_id: "base_link".into(),
-//!         pose: Isometry3::translation(step as f64 * 0.1, 0.0, 0.0),
+//!         twist: Twist { linear: Vector3::new(0.5, 0.0, 0.0), angular: Vector3::zeros() },
 //!         ..Default::default()
 //!     });
 //! }
 //! let estimate = slam.maybe_publish().expect("a publish period has elapsed");
-//! assert!((estimate.pose.translation.x - 0.2).abs() < 1e-9);
+//! assert!((estimate.pose.translation.x - 0.5).abs() < 0.05);
 //! ```
 
 pub mod cuvslam_odometry;
@@ -51,7 +55,10 @@ pub mod types;
 
 pub use cuvslam_odometry::{CameraConfig, CuvslamCore, CuvslamOdometryConfig};
 pub use dim_slam_core::{DimSlamCore, DimSlamCoreConfig};
-pub use odometry_fusion::{FusionCore, ImuConfig, InitialStds, OdometryFusionConfig, SourceConfig};
+pub use odometry_fusion::{
+    FusionCore, ImuConfig, InitialStds, OdometryFusionConfig, ProcessNoise, SourceConfig,
+    SourceKind,
+};
 pub use types::{
     CameraModel, ImageFrame, ImuNoiseModel, ImuSample, OdometryEstimate, PointCloud, TfLookup,
     Twist,