From 2fcd3963ceb18b12ccb38ba874030c19c7104d82 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Fri, 23 Aug 2024 13:57:49 -0400 Subject: [PATCH 01/13] Added ImuBias type --- examples/g2o-rerun.rs | 2 +- src/variables/imu_bias.rs | 136 ++++++++++++++++++++++++++++++++++++++ src/variables/macros.rs | 16 +++-- src/variables/mod.rs | 3 + src/variables/so3.rs | 22 +++--- 5 files changed, 161 insertions(+), 18 deletions(-) create mode 100644 src/variables/imu_bias.rs diff --git a/examples/g2o-rerun.rs b/examples/g2o-rerun.rs index bb7edc5..2b574fc 100644 --- a/examples/g2o-rerun.rs +++ b/examples/g2o-rerun.rs @@ -13,7 +13,7 @@ use factrs::{ use rerun::{Arrows2D, Arrows3D, Points2D, Points3D}; fn main() -> Result<(), Box> { - // ------------------------- Parse Arguments & Load data ------------------------- // + // ---------------------- Parse Arguments & Load data ---------------------- // let args: Vec = env::args().collect(); if args.len() < 2 { println!("Usage: {} ", args[0]); diff --git a/src/variables/imu_bias.rs b/src/variables/imu_bias.rs new file mode 100644 index 0000000..b1c14ff --- /dev/null +++ b/src/variables/imu_bias.rs @@ -0,0 +1,136 @@ +use std::fmt; + +use nalgebra::Const; + +use super::Variable; +use crate::{ + dtype, + linalg::{DualVector, Numeric, Vector3, VectorDim}, + tag_variable, +}; + +tag_variable!(ImuBias); + +/// IMU bias +/// +/// The IMU bias is a 6D vector containing the gyro and accel biases. It is +/// treated as a 6D vector for optimization purposes. +#[derive(Clone)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct ImuBias { + gyro: Vector3, + accel: Vector3, +} + +impl ImuBias { + /// Create a new IMU bias + // TODO: I would love to see a way to verify these aren't put in backwards + pub fn new(gyro: Vector3, accel: Vector3) -> Self { + ImuBias { gyro, accel } + } + + /// Get the gyro bias + pub fn gyro(&self) -> &Vector3 { + &self.gyro + } + + /// Get the accel bias + pub fn accel(&self) -> &Vector3 { + &self.accel + } +} + +impl Variable for ImuBias { + type Dim = crate::linalg::Const<6>; + type Alias = ImuBias
; + + fn identity() -> Self { + ImuBias { + gyro: Vector3::zeros(), + accel: Vector3::zeros(), + } + } + + fn inverse(&self) -> Self { + ImuBias { + gyro: -self.gyro, + accel: -self.accel, + } + } + + fn compose(&self, other: &Self) -> Self { + ImuBias { + gyro: self.gyro + other.gyro, + accel: self.accel + other.accel, + } + } + + fn exp(delta: crate::linalg::VectorViewX) -> Self { + let gyro = Vector3::new(delta[0], delta[1], delta[2]); + let accel = Vector3::new(delta[3], delta[4], delta[5]); + ImuBias { gyro, accel } + } + + fn log(&self) -> crate::linalg::VectorX { + crate::linalg::vectorx![ + self.gyro.x, + self.gyro.y, + self.gyro.z, + self.accel.x, + self.accel.y, + self.accel.z + ] + } + + fn dual_convert(other: &Self::Alias) -> Self::Alias
{ + ImuBias { + gyro: other.gyro.map(|x| x.into()), + accel: other.accel.map(|x| x.into()), + } + } + + fn dual_setup(idx: usize) -> Self::Alias> + where + crate::linalg::AllocatorBuffer: Sync + Send, + nalgebra::DefaultAllocator: crate::linalg::DualAllocator, + crate::linalg::DualVector: Copy, + { + let n = VectorDim::::zeros().shape_generic().0; + + let mut gyro = Vector3::>::zeros(); + for (i, gi) in gyro.iter_mut().enumerate() { + gi.eps = num_dual::Derivative::derivative_generic(n, Const::<1>, idx + i); + } + + let mut accel = Vector3::>::zeros(); + for (i, ai) in accel.iter_mut().enumerate() { + ai.eps = num_dual::Derivative::derivative_generic(n, Const::<1>, idx + i + 3); + } + + ImuBias { gyro, accel } + } +} + +impl fmt::Display for ImuBias { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!( + f, + "ImuBias(g: ({:.3}, {:.3}, {:.3}), a: ({:.3}, {:.3}, {:.3}))", + self.gyro.x, self.gyro.y, self.gyro.z, self.accel.x, self.accel.y, self.accel.z + ) + } +} + +impl fmt::Debug for ImuBias { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + fmt::Display::fmt(self, f) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::test_variable; + + test_variable!(ImuBias); +} diff --git a/src/variables/macros.rs b/src/variables/macros.rs index 1ee3c38..306b812 100644 --- a/src/variables/macros.rs +++ b/src/variables/macros.rs @@ -5,15 +5,15 @@ #[macro_export] macro_rules! assert_variable_eq { ($x:expr, $y:expr) => { - matrixcompare::assert_matrix_eq!($x.ominus(&$y), VectorX::zeros($x.dim())); + matrixcompare::assert_matrix_eq!($x.ominus(&$y), $crate::linalg::VectorX::zeros($x.dim())); }; ($x:expr, $y:expr, comp = exact) => { - matrixcompare::assert_matrix_eq!($x.ominus(&$y), VectorX::zeros($x.dim()), comp = exact); + matrixcompare::assert_matrix_eq!($x.ominus(&$y), $crate::linalg::VectorX::zeros($x.dim()), comp = exact); }; ($x:expr, $y:expr, comp = abs, tol = $tol:expr) => { matrixcompare::assert_matrix_eq!( $x.ominus(&$y), - VectorX::zeros($crate::variables::traits::Variable::dim(&$x)), + $crate::linalg::VectorX::zeros($crate::variables::traits::Variable::dim(&$x)), comp = abs, tol = $tol ); @@ -21,18 +21,18 @@ macro_rules! assert_variable_eq { ($x:expr, $y:expr, comp = ulp, tol = $tol:expr) => { matrixcompare::assert_matrix_eq!( $x.ominus(&$y), - VectorX::zeros($x.dim()), + $crate::linalg::VectorX::zeros($x.dim()), comp = ulp, tol = $tol ); }; ($x:expr, $y:expr, comp = float) => { - matrixcompare::assert_matrix_eq!($x.ominus(&$y), VectorX::zeros($x.dim()), comp = float); + matrixcompare::assert_matrix_eq!($x.ominus(&$y), $crate::linalg::VectorX::zeros($x.dim()), comp = float); }; ($x:expr, $y:expr, comp = float, $($key:ident = $val:expr),+) => { matrixcompare::assert_matrix_eq!( $x.ominus(&$y), - VectorX::zeros($x.dim()), + $crate::linalg::VectorX::zeros($x.dim()), comp = float, $($key:ident = $val:expr),+ ); @@ -51,7 +51,9 @@ macro_rules! test_variable { ($var:ident) => { // Return a misc element for our tests fn element(scale: $crate::dtype) -> T { - let xi = VectorX::from_fn(T::DIM, |_, i| scale * ((i + 1) as $crate::dtype) / 10.0); + let xi = $crate::linalg::VectorX::from_fn(T::DIM, |_, i| { + scale * ((i + 1) as $crate::dtype) / 10.0 + }); T::exp(xi.as_view()) } diff --git a/src/variables/mod.rs b/src/variables/mod.rs index fce7965..ec61913 100644 --- a/src/variables/mod.rs +++ b/src/variables/mod.rs @@ -77,4 +77,7 @@ pub use vector::{ VectorVar6, }; +mod imu_bias; +pub use imu_bias::ImuBias; + mod macros; diff --git a/src/variables/so3.rs b/src/variables/so3.rs index d47d1ad..7b3f80f 100644 --- a/src/variables/so3.rs +++ b/src/variables/so3.rs @@ -106,20 +106,22 @@ impl Variable for SO3 { fn exp(xi: VectorViewX) -> Self { let mut xyzw = Vector4::zeros(); + let theta = xi.norm(); + xyzw.w = (theta * D::from(0.5)).cos(); + if theta < D::from(1e-3) { - xyzw.x = xi[0] * D::from(0.5); - xyzw.y = xi[1] * D::from(0.5); - xyzw.z = xi[2] * D::from(0.5); - xyzw.w = D::from(1.0); + let tmp = xyzw.w * D::from(0.5); + xyzw.x = xi[0] * tmp; + xyzw.y = xi[1] * tmp; + xyzw.z = xi[2] * tmp; } else { - let theta_half = theta / D::from(2.0); - let sin_theta = theta_half.sin(); - xyzw.x = xi[0] * sin_theta / theta; - xyzw.y = xi[1] * sin_theta / theta; - xyzw.z = xi[2] * sin_theta / theta; - xyzw.w = theta_half.cos(); + let omega = xi / theta; + let sin_theta_half = (D::from(1.0) - xyzw.w * xyzw.w).sqrt(); + xyzw.x = omega[0] * sin_theta_half; + xyzw.y = omega[1] * sin_theta_half; + xyzw.z = omega[2] * sin_theta_half; } SO3 { xyzw } From 91c493a84fa3723fa0bf314e7c5003a8efeb6a79 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Mon, 26 Aug 2024 17:10:27 -0400 Subject: [PATCH 02/13] Ditched prelude --- src/containers/symbol.rs | 2 +- src/containers/values.rs | 5 ++--- src/lib.rs | 24 ++++++++---------------- src/optimizers/mod.rs | 3 +-- 4 files changed, 12 insertions(+), 22 deletions(-) diff --git a/src/containers/symbol.rs b/src/containers/symbol.rs index 9adfc28..59d6406 100644 --- a/src/containers/symbol.rs +++ b/src/containers/symbol.rs @@ -4,7 +4,7 @@ use std::{ mem::size_of, }; -use crate::prelude::VariableUmbrella; +use crate::variables::VariableUmbrella; // Char is stored in last CHR_BITS // Value is stored in the first IDX_BITS diff --git a/src/containers/values.rs b/src/containers/values.rs index 61e5837..24b6a62 100644 --- a/src/containers/values.rs +++ b/src/containers/values.rs @@ -2,11 +2,10 @@ use std::{collections::hash_map::Entry, default::Default, fmt, iter::IntoIterato use ahash::AHashMap; -use super::{Key, Symbol, TypedSymbol}; +use super::{DefaultSymbol, Key, Symbol, TypedSymbol}; use crate::{ linear::LinearValues, - prelude::{DefaultSymbol, VariableUmbrella}, - variables::VariableSafe, + variables::{VariableSafe, VariableUmbrella}, }; // Since we won't be passing dual numbers through any of this, diff --git a/src/lib.rs b/src/lib.rs index e5f5cda..1eef114 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -89,11 +89,11 @@ pub mod robust; pub mod utils; pub mod variables; -/// Untagged symnbols if `unchecked` API is desired. +/// Untagged symbols if `unchecked` API is desired. /// /// We strongly recommend using [assign_symbols](crate::assign_symbols) to /// create and tag symbols with the appropriate types. However, we provide a -/// number of pre-defined symbols if desired. Note this objects can't be tagged +/// number of pre-defined symbols if desired. Note these objects can't be tagged /// due to the orphan rules. pub mod symbols { crate::assign_symbols!( @@ -101,23 +101,15 @@ pub mod symbols { ); } -/// Helper module to import common types +/// Helper module to import common traits /// -/// This module is meant to be glob imported to make it easier to use the -/// library. +/// This module is meant to be glob imported to make it easier to use the traits +/// and functionality in the library. /// ``` -/// use factrs::prelude::*; +/// use factrs::traits::*; /// ``` -pub mod prelude { - pub use crate::{ - assign_symbols, - containers::*, - noise::*, - optimizers::*, - residuals::*, - robust::*, - variables::*, - }; +pub mod traits { + pub use crate::{linalg::DualConvert, residuals::Residual, variables::Variable}; } #[cfg(feature = "rerun")] diff --git a/src/optimizers/mod.rs b/src/optimizers/mod.rs index ddba5c4..164d675 100644 --- a/src/optimizers/mod.rs +++ b/src/optimizers/mod.rs @@ -61,11 +61,10 @@ pub mod test { use super::*; use crate::{ - containers::{Graph, Values}, + containers::{FactorBuilder, Graph, Values}, dtype, linalg::{AllocatorBuffer, Const, DualAllocator, DualVector, VectorX}, noise::{NoiseModelSafe, UnitNoise}, - prelude::FactorBuilder, residuals::{BetweenResidual, PriorResidual, Residual, ResidualSafe}, symbols::X, variables::VariableUmbrella, From 78fa77fde4825afc05e056dfc68a4f9d04e5a061 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Mon, 26 Aug 2024 17:11:21 -0400 Subject: [PATCH 03/13] Finished writing bulk of preint --- src/linalg/dual.rs | 15 +- src/linalg/mod.rs | 2 +- src/noise/gaussian.rs | 3 + src/noise/unit.rs | 3 + src/residuals/imu_preint.rs | 299 ++++++++++++++++++++++++++++++++++++ src/residuals/macros.rs | 6 +- src/residuals/mod.rs | 3 + src/variables/imu_bias.rs | 24 ++- src/variables/so3.rs | 16 ++ 9 files changed, 365 insertions(+), 6 deletions(-) create mode 100644 src/residuals/imu_preint.rs diff --git a/src/linalg/dual.rs b/src/linalg/dual.rs index edd1467..eb1ec57 100644 --- a/src/linalg/dual.rs +++ b/src/linalg/dual.rs @@ -1,6 +1,6 @@ use nalgebra::{allocator::Allocator, Const}; -use super::{Dim, RealField}; +use super::{Dim, Matrix, RealField}; use crate::dtype; /// Wrapper for all properties needed for dual numbers @@ -28,3 +28,16 @@ impl< > DualAllocator for T { } + +// TODO: Expand on this instead of including in Variable?? +pub trait DualConvert { + type Alias; + fn dual_convert(other: &Self::Alias) -> Self::Alias; +} + +impl DualConvert for Matrix { + type Alias = Matrix; + fn dual_convert(other: &Self::Alias) -> Self::Alias { + other.map(|x| x.into()) + } +} diff --git a/src/linalg/mod.rs b/src/linalg/mod.rs index 12a2827..fa9828b 100644 --- a/src/linalg/mod.rs +++ b/src/linalg/mod.rs @@ -11,7 +11,7 @@ use crate::dtype; mod dual; -pub use dual::{DualAllocator, DualScalar, DualVector, Numeric}; +pub use dual::{DualAllocator, DualConvert, DualScalar, DualVector, Numeric}; // Dual numbers pub use num_dual::Derivative; diff --git a/src/noise/gaussian.rs b/src/noise/gaussian.rs index cbe88fb..48fbc8e 100644 --- a/src/noise/gaussian.rs +++ b/src/noise/gaussian.rs @@ -20,6 +20,9 @@ tag_noise!( GaussianNoise<10>, GaussianNoise<11>, GaussianNoise<12>, + GaussianNoise<13>, + GaussianNoise<14>, + GaussianNoise<15>, ); /// A Gaussian noise model. diff --git a/src/noise/unit.rs b/src/noise/unit.rs index 5620c02..62f27d6 100644 --- a/src/noise/unit.rs +++ b/src/noise/unit.rs @@ -17,6 +17,9 @@ tag_noise!( UnitNoise<10>, UnitNoise<11>, UnitNoise<12>, + UnitNoise<13>, + UnitNoise<14>, + UnitNoise<15>, ); /// A unit noise model. diff --git a/src/residuals/imu_preint.rs b/src/residuals/imu_preint.rs new file mode 100644 index 0000000..a153762 --- /dev/null +++ b/src/residuals/imu_preint.rs @@ -0,0 +1,299 @@ +use core::fmt; + +use nalgebra::Const; + +use super::Residual6; +use crate::{ + containers::{Factor, FactorBuilder, TypedSymbol}, + dtype, + impl_residual, + linalg::{ForwardProp, Matrix, Matrix3, Vector, Vector3, VectorX}, + noise::GaussianNoise, + tag_residual, + traits::*, + variables::{ImuBias, MatrixLieGroup, VectorVar3, SE3, SO3}, +}; + +// ----------------------- The actual residual object ----------------------- // +tag_residual!(ImuPreintegrationResidual); + +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] +pub struct ImuPreintegrationResidual { + preint: Vector<9>, + gravity: Vector3, + dt: dtype, + bias_hat: ImuBias, + h_bias_accel: Matrix<9, 3>, + h_bias_gyro: Matrix<9, 3>, +} + +impl Residual6 for ImuPreintegrationResidual { + type Differ = ForwardProp>; + type DimIn = Const<30>; + type DimOut = Const<15>; + type V1 = SE3; + type V2 = VectorVar3; + type V3 = ImuBias; + type V4 = SE3; + type V5 = VectorVar3; + type V6 = ImuBias; + + fn residual6( + &self, + x1: SE3, + v1: VectorVar3, + b1: ImuBias, + x2: SE3, + v2: VectorVar3, + b2: ImuBias, + ) -> VectorX { + // Add dual types to all of our fields + let preint = Vector::<9, dtype>::dual_convert::(&self.preint); + let bias_hat = ImuBias::::dual_convert::(&self.bias_hat); + let g = Vector3::::dual_convert::(&self.gravity); + let dt = D::from(self.dt); + let h_bias_accel = Matrix::<9, 3, dtype>::dual_convert::(&self.h_bias_accel); + let h_bias_gyro = Matrix::<9, 3, dtype>::dual_convert::(&self.h_bias_gyro); + + // Compute first-order updates to preint with bias + let bias_diff = &b1 - &bias_hat; + let preint = preint + h_bias_accel * bias_diff.accel() + h_bias_gyro * bias_diff.gyro(); + + // Split preint into components + let xi_theta = preint.fixed_rows::<3>(0); + let xi_v = preint.fixed_rows::<3>(3).into_owned(); + let xi_t = preint.fixed_rows::<3>(6).into_owned(); + + let x1_r = x1.rot(); + let x1_t = x1.xyz(); + + // Estimate x2 + // R2_meas = R1 * exp(xi_theta) + let r2_meas = x1_r.compose(&SO3::exp(xi_theta.as_view())); + // v2_meas = v1 + g * dt + R1 * xi_v + let v2_meas = v1.0 + g * dt + x1_r.apply(xi_v.as_view()); + let v2_meas: VectorVar3 = v2_meas.into(); + // p2_meas = p1 + v1 * dt + 0.5 * g * dt^2 + R1 * delta_t + let p2_meas = x1_t + v1.0 * dt + g * dt * dt * D::from(0.5) + x1_r.apply(xi_t.as_view()); + let p2_meas: VectorVar3 = p2_meas.into(); + let b2_meas = b1; + + // Compute residual + let p_2: VectorVar3 = x2.xyz().into_owned().into(); + let r_r = r2_meas.ominus(x2.rot()); + let r_p = p2_meas.ominus(&p_2); + let r_vel = v2_meas.ominus(&v2); + let r_bias = b2_meas.ominus(&b2); + + let mut residual = VectorX::zeros(15); + residual.fixed_rows_mut::<3>(0).copy_from(&r_r); + residual.fixed_rows_mut::<3>(3).copy_from(&r_vel); + residual.fixed_rows_mut::<3>(6).copy_from(&r_p); + residual.fixed_rows_mut::<6>(9).copy_from(&r_bias); + + residual + } +} + +impl_residual!(6, ImuPreintegrationResidual); + +impl fmt::Display for ImuPreintegrationResidual { + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!(f, "ImuPreintegrationResidual(preint: {})", self.preint) + } +} + +// ------------------------- The Preintegrator ------------------------- // +#[derive(Clone, Debug)] +pub struct ImuParams { + g: Vector3, + cov_accel: Matrix3, + cov_gyro: Matrix3, + cov_accel_bias: Matrix3, + cov_gyro_bias: Matrix3, + cov_integration: Matrix3, + cov_init: Matrix3, +} + +/// Implements reasonable parameters for ImuParams including positive gravity +impl Default for ImuParams { + fn default() -> Self { + Self { + g: Vector3::new(0.0, 0.0, 9.81), + cov_accel: Matrix3::identity() * 1e-5, + cov_gyro: Matrix3::identity() * 1e-5, + cov_accel_bias: Matrix3::identity() * 1e-6, + cov_gyro_bias: Matrix3::identity() * 1e-6, + cov_integration: Matrix3::identity() * 1e-7, + cov_init: Matrix3::identity() * 1e-7, + } + } +} + +impl ImuParams { + /// Create a new set of parameters with positive gravity + pub fn positive() -> Self { + Self::default() + } + + /// Create a new set of parameters with negative gravity + pub fn negative() -> Self { + let mut params = Self::default(); + params.g = -params.g; + params + } + + pub fn set_scalar_accel(&mut self, val: dtype) { + self.cov_accel = Matrix3::identity() * val; + } + + pub fn set_scalar_gyro(&mut self, val: dtype) { + self.cov_gyro = Matrix3::identity() * val; + } + + pub fn set_scalar_accel_bias(&mut self, val: dtype) { + self.cov_accel_bias = Matrix3::identity() * val; + } + + pub fn set_scalar_gyro_bias(&mut self, val: dtype) { + self.cov_gyro_bias = Matrix3::identity() * val; + } + + pub fn set_scalar_integration(&mut self, val: dtype) { + self.cov_integration = Matrix3::identity() * val; + } + + pub fn set_scalar_init(&mut self, val: dtype) { + self.cov_init = Matrix3::identity() * val; + } +} + +pub struct ImuPreint { + delta_t: dtype, + delta_r: Vector3, + delta_v: Vector3, + delta_p: Vector3, + bias_hat: ImuBias, + h_bias_accel: Matrix<9, 3>, + h_bias_gyro: Matrix<9, 3>, + gravity: Vector3, +} + +#[derive(Clone, Debug)] +pub struct ImuPreintegrator { + // Mutable state that will change as we integrate + // TODO: Combine these into a struct? I pass them all into the residual anyways + delta_t: dtype, + delta_r: Vector3, + delta_v: Vector3, + delta_p: Vector3, + bias_hat: ImuBias, + h_bias_accel: Matrix<9, 3>, + h_bias_gyro: Matrix<9, 3>, + cov: Matrix<15, 15>, + // Constants + params: ImuParams, +} + +impl ImuPreintegrator { + pub fn new(params: ImuParams, bias_hat: ImuBias) -> Self { + Self { + delta_t: 0.0, + delta_r: Vector3::zeros(), + delta_v: Vector3::zeros(), + delta_p: Vector3::zeros(), + bias_hat, + h_bias_accel: Matrix::zeros(), + h_bias_gyro: Matrix::zeros(), + // init with small value to avoid singular matrix + cov: Matrix::zeros() * 1e-12, + params, + } + } + + #[allow(non_snake_case)] + pub fn integrate(&mut self, dt: dtype, accel: Vector3, gyro: Vector3) { + // Update preint + self.delta_t += dt; + let accel = accel - self.bias_hat.accel(); + let gyro = gyro - self.bias_hat.gyro(); + let r_kaccel = SO3::exp(self.delta_r.as_view()).apply(accel.as_view()); + let H = SO3::dexp(self.delta_r.as_view()); + let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); + + self.delta_r += Hinv * gyro * dt; + self.delta_v += r_kaccel * dt; + self.delta_p += self.delta_v * dt + 0.5 * r_kaccel * dt * dt; + + // Update H + + // Update covariance + // TODO: Need to verify dimensions of all these + let R = SO3::exp(self.delta_r.as_view()).to_matrix(); + let mut F = Matrix::<15, 15>::identity(); + let A = Matrix3::identity() - SO3::hat(gyro.as_view()) * dt / 2.0; + F.fixed_view_mut::<3, 3>(0, 0).copy_from(&A); + let A = Hinv * dt; + F.fixed_view_mut::<3, 3>(0, 12).copy_from(&A); + let mut A = -R * SO3::hat(accel.as_view()) * Hinv * dt; + F.fixed_view_mut::<3, 3>(6, 0).copy_from(&A); + A *= dt / 2.0; + F.fixed_view_mut::<3, 3>(3, 0).copy_from(&A); + let A = Matrix3::identity() * dt; + F.fixed_view_mut::<3, 3>(3, 6).copy_from(&A); + let mut A = R * dt; + F.fixed_view_mut::<3, 3>(6, 9).copy_from(&A); + A *= dt / 2.0; + F.fixed_view_mut::<3, 3>(3, 9).copy_from(&A); + + // TODO: Fill out this beast + let G_Q_Gt = Matrix::<15, 15>::identity(); + self.cov = F * self.cov * F.transpose() + G_Q_Gt; + } + + /// Build a corresponding factor + /// + /// This consumes the preintegrator and returns a + /// [factor](crate::factor::Factor) that can be inserted into a factor + /// graph. + pub fn build( + self, + x1: X1, + v1: V1, + b1: B1, + x2: X2, + v2: V2, + b2: B2, + ) -> Factor + where + X1: TypedSymbol, + V1: TypedSymbol, + B1: TypedSymbol, + X2: TypedSymbol, + V2: TypedSymbol, + B2: TypedSymbol, + { + // Create noise from our covariance matrix + let noise = GaussianNoise::from_matrix_cov(self.cov.as_view()); + + // Copy preint into a single vector + let mut preint: Vector<9, dtype> = Vector::zeros(); + preint.fixed_rows_mut::<3>(0).copy_from(&self.delta_r); + preint.fixed_rows_mut::<3>(3).copy_from(&self.delta_v); + preint.fixed_rows_mut::<3>(6).copy_from(&self.delta_p); + + let res = ImuPreintegrationResidual { + preint, + gravity: self.params.g, + dt: self.delta_t, + bias_hat: self.bias_hat, + h_bias_accel: self.h_bias_accel, + h_bias_gyro: self.h_bias_gyro, + }; + + FactorBuilder::new6(res, x1, v1, b1, x2, v2, b2) + .noise(noise) + .build() + } +} diff --git a/src/residuals/macros.rs b/src/residuals/macros.rs index 3f5c07f..247d542 100644 --- a/src/residuals/macros.rs +++ b/src/residuals/macros.rs @@ -9,17 +9,17 @@ macro_rules! impl_residual { ($num:expr, $name:ident $(< $T:ident : $Trait:ident >)? ) => { use paste::paste; paste!{ - impl$(<$T: $Trait + 'static>)? Residual for $name $(< $T >)? + impl$(<$T: $Trait + 'static>)? $crate::residuals::Residual for $name $(< $T >)? { type DimIn = ]>::DimIn; type DimOut = ]>::DimOut; type NumVars = $crate::linalg::Const<$num>; - fn residual(&self, values: &Values, keys: &[$crate::containers::Symbol]) -> VectorX { + fn residual(&self, values: &$crate::containers::Values, keys: &[$crate::containers::Key]) -> $crate::linalg::VectorX { self.[](values, keys) } - fn residual_jacobian(&self, values: &Values, keys: &[$crate::containers::Symbol]) -> DiffResult + fn residual_jacobian(&self, values: &$crate::containers::Values, keys: &[$crate::containers::Key]) -> $crate::linalg::DiffResult<$crate::linalg::VectorX, $crate::linalg::MatrixX> { self.[](values, keys) } diff --git a/src/residuals/mod.rs b/src/residuals/mod.rs index 4fa5354..d88ac85 100644 --- a/src/residuals/mod.rs +++ b/src/residuals/mod.rs @@ -22,4 +22,7 @@ pub use prior::PriorResidual; mod between; pub use between::BetweenResidual; +mod imu_preint; +pub use imu_preint::{ImuParams, ImuPreintegrationResidual, ImuPreintegrator}; + mod macros; diff --git a/src/variables/imu_bias.rs b/src/variables/imu_bias.rs index b1c14ff..18175bf 100644 --- a/src/variables/imu_bias.rs +++ b/src/variables/imu_bias.rs @@ -1,4 +1,4 @@ -use std::fmt; +use std::{fmt, ops}; use nalgebra::Const; @@ -127,6 +127,28 @@ impl fmt::Debug for ImuBias { } } +impl ops::Sub for ImuBias { + type Output = Self; + + fn sub(self, rhs: Self) -> Self { + ImuBias { + gyro: self.gyro - rhs.gyro, + accel: self.accel - rhs.accel, + } + } +} + +impl<'a, D: Numeric> ops::Sub for &'a ImuBias { + type Output = ImuBias; + + fn sub(self, rhs: Self) -> ImuBias { + ImuBias { + gyro: self.gyro - rhs.gyro, + accel: self.accel - rhs.accel, + } + } +} + #[cfg(test)] mod tests { use super::*; diff --git a/src/variables/so3.rs b/src/variables/so3.rs index 7b3f80f..68a4151 100644 --- a/src/variables/so3.rs +++ b/src/variables/so3.rs @@ -67,6 +67,22 @@ impl SO3 { pub fn w(&self) -> D { self.xyzw[3] } + + pub fn dexp(xi: VectorView3) -> Matrix3 { + let theta2 = xi.norm_squared(); + + let (a, b) = if theta2 < D::from(1e-3) { + (D::from(0.5), D::from(1.0) / D::from(6.0)) + } else { + let theta = theta2.sqrt(); + let a = (D::from(1.0) - theta.cos()) / theta2; + let b = (theta - theta.sin()) / (theta * theta2); + (a, b) + }; + + let hat = SO3::hat(xi); + Matrix3::identity() + hat * a + hat * hat * b + } } impl Variable for SO3 { From 3bca1ab5d2489ed58f2bfade37fb64b31b5a9ace Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 28 Aug 2024 13:22:18 -0400 Subject: [PATCH 04/13] Fix tests due to prelude removal --- README.md | 11 ++++++++++- examples/serde.rs | 3 +-- src/containers/factor.rs | 10 +++++++++- src/containers/graph.rs | 9 ++++++++- src/containers/symbol.rs | 2 +- src/containers/values.rs | 19 ++++++++++++++++--- src/lib.rs | 18 ++++++++++++++++-- src/linalg/forward_prop.rs | 6 +++++- src/linalg/numerical_diff.rs | 6 +++++- 9 files changed, 71 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index 04b7d43..685e20b 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,16 @@ We recommend you checkout the [docs](https://docs.rs/factrs/latest/factrs/) (WIP ## Example ```rust -use factrs::prelude::*; +use factrs::{ + assign_symbols, + containers::{FactorBuilder, Graph, Values}, + noise::GaussianNoise, + optimizers::GaussNewton, + residuals::{BetweenResidual, PriorResidual}, + robust::Huber, + traits::*, + variables::SO2, +}; // Assign symbols to variable types assign_symbols!(X: SO2); diff --git a/examples/serde.rs b/examples/serde.rs index 02d1c45..60f9ad8 100644 --- a/examples/serde.rs +++ b/examples/serde.rs @@ -1,8 +1,7 @@ use factrs::{ - containers::{Graph, Values, X}, + containers::{FactorBuilder, Graph, Values, X}, factors::Factor, noise::GaussianNoise, - prelude::FactorBuilder, residuals::{BetweenResidual, PriorResidual}, robust::{GemanMcClure, L2}, variables::{SE2, SO2}, diff --git a/src/containers/factor.rs b/src/containers/factor.rs index 86a0913..2add1a6 100644 --- a/src/containers/factor.rs +++ b/src/containers/factor.rs @@ -31,7 +31,15 @@ use crate::{ /// [LinearFactor]. /// /// ``` -/// # use factrs::prelude::*; +/// # use factrs::{ +/// assign_symbols, +/// containers::FactorBuilder, +/// noise::GaussianNoise, +/// optimizers::GaussNewton, +/// residuals::{PriorResidual}, +/// robust::GemanMcClure, +/// variables::VectorVar3, +/// }; /// # assign_symbols!(X: VectorVar3); /// let prior = VectorVar3::new(1.0, 2.0, 3.0); /// let residual = PriorResidual::new(prior); diff --git a/src/containers/graph.rs b/src/containers/graph.rs index fab3ad2..2678d88 100644 --- a/src/containers/graph.rs +++ b/src/containers/graph.rs @@ -14,7 +14,14 @@ use crate::{containers::Factor, dtype, linear::LinearGraph}; /// solved iteratively. /// /// ``` -/// # use factrs::prelude::*; +/// # use factrs::{ +/// assign_symbols, +/// containers::{Graph, FactorBuilder}, +/// residuals::PriorResidual, +/// robust::GemanMcClure, +/// traits::*, +/// variables::SO2, +/// }; /// # assign_symbols!(X: SO2); /// # let factor = FactorBuilder::new1(PriorResidual::new(SO2::identity()), X(0)).build(); /// let mut graph = Graph::new(); diff --git a/src/containers/symbol.rs b/src/containers/symbol.rs index 59d6406..d34a6dc 100644 --- a/src/containers/symbol.rs +++ b/src/containers/symbol.rs @@ -83,7 +83,7 @@ pub trait TypedSymbol: Symbol {} /// with the type they will be used with. This macro will create a new symbol /// and implement all the necessary traits for it to be used as a symbol. /// ``` -/// use factrs::prelude::*; +/// use factrs::{assign_symbols, variables::{SO2, SE2}}; /// assign_symbols!(X: SO2; Y: SE2); /// ``` #[macro_export] diff --git a/src/containers/values.rs b/src/containers/values.rs index 24b6a62..84eeeca 100644 --- a/src/containers/values.rs +++ b/src/containers/values.rs @@ -18,7 +18,11 @@ use crate::{ /// Values, it must implement [Variable](crate::variables::Variable), and then /// will implement [VariableSafe] via a blanket implementation. /// ``` -/// # use factrs::prelude::*; +/// # use factrs::{ +/// assign_symbols, +/// containers::Values, +/// variables::SO2, +/// }; /// # assign_symbols!(X: SO2); /// let x = SO2::from_theta(0.1); /// let mut values = Values::new(); @@ -80,7 +84,11 @@ impl Values { /// symbol and as such is guaranteed to return the correct type. Returns /// None if key isn't found. /// ``` - /// # use factrs::prelude::*; + /// # use factrs::{ + /// assign_symbols, + /// containers::Values, + /// variables::SO2, + /// }; /// # assign_symbols!(X: SO2); /// # let x = SO2::from_theta(0.1); /// # let mut values = Values::new(); @@ -149,7 +157,12 @@ impl Values { /// the values. /// /// ``` - /// # use factrs::prelude::*; + /// # use factrs::{ + /// assign_symbols, + /// containers::Values, + /// traits::*, + /// variables::SO2, + /// }; /// # assign_symbols!(X: SO2); /// # let mut values = Values::new(); /// # (0..10).for_each(|i| {values.insert(X(0), SO2::identity());} ); diff --git a/src/lib.rs b/src/lib.rs index 1eef114..4d83ee8 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -36,7 +36,16 @@ //! //! # Example //! ``` -//! use factrs::prelude::*; +//! use factrs::{ +//! assign_symbols, +//! containers::{FactorBuilder, Graph, Values}, +//! noise::GaussianNoise, +//! optimizers::GaussNewton, +//! residuals::{BetweenResidual, PriorResidual}, +//! robust::Huber, +//! traits::*, +//! variables::SO2, +//! }; //! //! // Assign symbols to variable types //! assign_symbols!(X: SO2); @@ -109,7 +118,12 @@ pub mod symbols { /// use factrs::traits::*; /// ``` pub mod traits { - pub use crate::{linalg::DualConvert, residuals::Residual, variables::Variable}; + pub use crate::{ + linalg::{Diff, DualConvert}, + optimizers::{GraphOptimizer, Optimizer}, + residuals::Residual, + variables::Variable, + }; } #[cfg(feature = "rerun")] diff --git a/src/linalg/forward_prop.rs b/src/linalg/forward_prop.rs index e7e11e7..7052980 100644 --- a/src/linalg/forward_prop.rs +++ b/src/linalg/forward_prop.rs @@ -23,7 +23,11 @@ use crate::{ /// inputs and with vector-valued outputs. /// /// ``` -/// use factrs::{linalg::*, prelude::*}; +/// use factrs::{ +/// linalg::{vectorx, Const, DiffResult, ForwardProp, Numeric, VectorX}, +/// traits::*, +/// variables::SO2, +/// }; /// /// fn f(x: SO2, y: SO2) -> VectorX { /// x.ominus(&y) diff --git a/src/linalg/numerical_diff.rs b/src/linalg/numerical_diff.rs index a7c3ac4..46c485e 100644 --- a/src/linalg/numerical_diff.rs +++ b/src/linalg/numerical_diff.rs @@ -17,7 +17,11 @@ use crate::{ /// inputs and with vector-valued outputs. /// /// ``` -/// use factrs::{linalg::*, prelude::*}; +/// use factrs::{ +/// linalg::{vectorx, DiffResult, NumericalDiff, VectorX}, +/// traits::*, +/// variables::SO2, +/// }; /// /// // We can also be generic over Numeric as in [ForwardProp] as well if desired /// fn f(x: SO2, y: SO2) -> VectorX { From fe915da87b6891555a130242cc1ae8f3f30ae53c Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 28 Aug 2024 13:22:41 -0400 Subject: [PATCH 05/13] Complete imu preint implementation done, testing next --- src/residuals/imu_preint.rs | 428 +++++++++++++++++++++++++----------- 1 file changed, 302 insertions(+), 126 deletions(-) diff --git a/src/residuals/imu_preint.rs b/src/residuals/imu_preint.rs index a153762..03013bb 100644 --- a/src/residuals/imu_preint.rs +++ b/src/residuals/imu_preint.rs @@ -1,4 +1,5 @@ use core::fmt; +use std::ops; use nalgebra::Const; @@ -7,12 +8,249 @@ use crate::{ containers::{Factor, FactorBuilder, TypedSymbol}, dtype, impl_residual, - linalg::{ForwardProp, Matrix, Matrix3, Vector, Vector3, VectorX}, + linalg::{ForwardProp, Matrix, Matrix3, Numeric, Vector3, VectorX}, noise::GaussianNoise, tag_residual, traits::*, variables::{ImuBias, MatrixLieGroup, VectorVar3, SE3, SO3}, }; +// ------------------- Objects that perform most of work ------------------- // +/// Struct to hold an Imu state +/// +/// Specifically holds an Imu state to which an ImuDelta can be applied +pub struct ImuState { + r: SO3, + v: Vector3, + p: Vector3, + bias: ImuBias, +} + +impl ImuState { + /// Propagate the state forward by the given delta + /// + /// This function takes an ImuDelta and applies it to the current state. + /// Identical to calling [ImuDelta::predict] with the current state. + pub fn propagate(&self, delta: ImuDelta) -> ImuState { + delta.predict(self) + } +} + +/// Struct to hold the preintegrated Imu delta +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] +pub struct ImuDelta { + dt: D, + xi_theta: Vector3, + xi_vel: Vector3, + xi_pos: Vector3, + bias_init: ImuBias, + h_bias_accel: Matrix<9, 3, D>, + h_bias_gyro: Matrix<9, 3, D>, + gravity: Vector3, +} + +impl ImuDelta { + fn new(gravity: Vector3, bias_init: ImuBias) -> Self { + Self { + dt: D::from(0.0), + xi_theta: Vector3::zeros(), + xi_vel: Vector3::zeros(), + xi_pos: Vector3::zeros(), + bias_init, + h_bias_accel: Matrix::zeros(), + h_bias_gyro: Matrix::zeros(), + gravity, + } + } + + fn remove_bias(&self, imu: &ImuMeasurement) -> ImuMeasurement { + imu - &self.bias_init + } + + fn first_order_update(&self, bias_diff: &ImuBias, idx: usize) -> Vector3 { + self.h_bias_accel.fixed_rows(idx) * bias_diff.accel() + + self.h_bias_gyro.fixed_rows(idx) * bias_diff.gyro() + } + + pub fn predict(&self, x1: &ImuState) -> ImuState { + let ImuState { r, v, p, bias } = x1; + + // Compute first-order updates to preint with bias + let bias_diff = bias - &self.bias_init; + let xi_theta = self.xi_theta + self.first_order_update(&bias_diff, 0); + let xi_v = self.xi_vel + self.first_order_update(&bias_diff, 3); + let xi_p = self.xi_pos + self.first_order_update(&bias_diff, 6); + + // Estimate x2 + // R2_meas = R1 * exp(xi_theta) + let r2_meas = r.compose(&SO3::exp(xi_theta.as_view())); + // v2_meas = v + g * dt + R1 * xi_v + let v2_meas = v + self.gravity * self.dt + r.apply(xi_v.as_view()); + let v2_meas = v2_meas.into(); + // p2_meas = p1 + v * dt + 0.5 * g * dt^2 + R1 * xi_p + let p2_meas = p + + v * self.dt + + self.gravity * self.dt * self.dt * D::from(0.5) + + r.apply(xi_p.as_view()); + let p2_meas = p2_meas.into(); + let b2_meas = bias.clone(); + + ImuState { + r: r2_meas, + v: v2_meas, + p: p2_meas, + bias: b2_meas, + } + } +} + +// None of these methods should need to be dualized / "backpropagated" +// Can just assume default dtype +impl ImuDelta { + #[allow(non_snake_case)] + pub fn integrate(&mut self, imu: &ImuMeasurement) { + self.dt += imu.dt; + let accel_world = SO3::exp(self.xi_theta.as_view()).apply(imu.accel.as_view()); + let H = SO3::dexp(self.xi_theta.as_view()); + let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); + + self.xi_theta += Hinv * imu.gyro * imu.dt; + self.xi_vel += accel_world * imu.dt; + self.xi_pos += self.xi_vel * imu.dt + accel_world * (imu.dt * imu.dt * 0.5); + } + + #[allow(non_snake_case)] + pub fn A(&self, imu: &ImuMeasurement) -> Matrix<15, 15> { + let H = SO3::dexp(self.xi_theta.as_view()); + let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); + let R = SO3::exp(self.xi_theta.as_view()).to_matrix(); + + let mut A = Matrix::<15, 15>::identity(); + + // First column (wrt theta) + let M = Matrix3::identity() - SO3::hat(imu.gyro.as_view()) * imu.dt / 2.0; + A.fixed_view_mut::<3, 3>(0, 0).copy_from(&M); + let mut M = -R * SO3::hat(imu.accel.as_view()) * H * imu.dt; + A.fixed_view_mut::<3, 3>(3, 0).copy_from(&M); + M *= imu.dt / 2.0; + A.fixed_view_mut::<3, 3>(6, 0).copy_from(&M); + + // Second column (wrt vel) + let M = Matrix3::identity() * imu.dt; + A.fixed_view_mut::<3, 3>(0, 6).copy_from(&M); + + // Third column (wrt pos) + + // Fourth column (wrt gyro bias) + let M = -Hinv * imu.dt; + A.fixed_view_mut::<3, 3>(0, 9).copy_from(&M); + + // Fifth column (wrt accel bias) + let mut M = -R * imu.dt; + A.fixed_view_mut::<3, 3>(6, 9).copy_from(&M); + M *= imu.dt / 2.0; + A.fixed_view_mut::<3, 3>(3, 9).copy_from(&M); + + A + } + + #[allow(non_snake_case)] + pub fn B_Q_BT(&self, imu: &ImuMeasurement, p: &ImuParams) -> Matrix<15, 15> { + let H = SO3::dexp(self.xi_theta.as_view()); + let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); + let R = SO3::exp(self.xi_theta.as_view()).to_matrix(); + + // Construct all partials + let H_theta_w = Hinv * imu.dt; + let H_theta_winit = -Hinv * self.dt; + + let H_v_a = R * imu.dt; + let H_v_ainit = -R * self.dt; + + let H_p_a = H_v_a * imu.dt / 2.0; + let H_p_int: Matrix3 = Matrix3::identity(); + let H_p_ainit = H_v_ainit * self.dt / 2.0; + + // Copy them into place + let mut B = Matrix::<15, 15>::zeros(); + // First column (wrt theta) + let M = H_theta_w * p.cov_gyro_bias * H_theta_w.transpose() + + H_theta_winit * p.cov_winit * H_theta_winit.transpose(); + B.fixed_view_mut::<3, 3>(0, 0).copy_from(&M); + + // Second column (wrt vel) + let M = H_v_a * p.cov_accel * H_v_a.transpose() + + H_v_ainit * p.cov_ainit * H_v_ainit.transpose(); + B.fixed_view_mut::<3, 3>(3, 3).copy_from(&M); + let M = H_p_a * p.cov_accel * H_v_a.transpose() + + H_p_ainit * p.cov_ainit * H_v_ainit.transpose(); + B.fixed_view_mut::<3, 3>(6, 3).copy_from(&M); + + // Third column (wrt pos) + let M = H_v_a * p.cov_accel * H_p_a.transpose() + + H_v_ainit * p.cov_ainit * H_p_ainit.transpose(); + B.fixed_view_mut::<3, 3>(3, 6).copy_from(&M); + let M = H_p_a * p.cov_accel * H_p_a.transpose() + + H_p_int * p.cov_integration * H_p_int.transpose() + + H_p_ainit * p.cov_ainit * H_p_ainit.transpose(); + B.fixed_view_mut::<3, 3>(6, 6).copy_from(&M); + + // Fourth column (wrt gyro bias) + B.fixed_view_mut::<3, 3>(9, 9).copy_from(&p.cov_gyro_bias); + + // Fifth column (wrt accel bias) + B.fixed_view_mut::<3, 3>(12, 12) + .copy_from(&p.cov_accel_bias); + + B + } +} + +impl DualConvert for ImuDelta { + type Alias = ImuDelta
; + fn dual_convert(other: &Self::Alias) -> Self::Alias
{ + ImuDelta { + dt: other.dt.into(), + xi_theta: Vector3::::dual_convert(&other.xi_theta), + xi_vel: Vector3::::dual_convert(&other.xi_vel), + xi_pos: Vector3::::dual_convert(&other.xi_pos), + bias_init: ImuBias::::dual_convert(&other.bias_init), + h_bias_accel: Matrix::<9, 3, D>::dual_convert(&other.h_bias_accel), + h_bias_gyro: Matrix::<9, 3, D>::dual_convert(&other.h_bias_gyro), + gravity: Vector3::::dual_convert(&other.gravity), + } + } +} + +pub struct ImuMeasurement { + accel: Vector3, + gyro: Vector3, + dt: D, +} + +impl ops::Sub> for ImuMeasurement { + type Output = Self; + + fn sub(self, rhs: ImuBias) -> Self { + ImuMeasurement { + gyro: self.gyro - rhs.gyro(), + accel: self.accel - rhs.accel(), + dt: self.dt, + } + } +} + +impl<'a, D: Numeric> ops::Sub<&'a ImuBias> for &'a ImuMeasurement { + type Output = ImuMeasurement; + + fn sub(self, rhs: &'a ImuBias) -> ImuMeasurement { + ImuMeasurement { + gyro: self.gyro - rhs.gyro(), + accel: self.accel - rhs.accel(), + dt: self.dt, + } + } +} // ----------------------- The actual residual object ----------------------- // tag_residual!(ImuPreintegrationResidual); @@ -20,12 +258,7 @@ tag_residual!(ImuPreintegrationResidual); #[derive(Clone, Debug)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct ImuPreintegrationResidual { - preint: Vector<9>, - gravity: Vector3, - dt: dtype, - bias_hat: ImuBias, - h_bias_accel: Matrix<9, 3>, - h_bias_gyro: Matrix<9, 3>, + delta: ImuDelta, } impl Residual6 for ImuPreintegrationResidual { @@ -49,41 +282,29 @@ impl Residual6 for ImuPreintegrationResidual { b2: ImuBias, ) -> VectorX { // Add dual types to all of our fields - let preint = Vector::<9, dtype>::dual_convert::(&self.preint); - let bias_hat = ImuBias::::dual_convert::(&self.bias_hat); - let g = Vector3::::dual_convert::(&self.gravity); - let dt = D::from(self.dt); - let h_bias_accel = Matrix::<9, 3, dtype>::dual_convert::(&self.h_bias_accel); - let h_bias_gyro = Matrix::<9, 3, dtype>::dual_convert::(&self.h_bias_gyro); - - // Compute first-order updates to preint with bias - let bias_diff = &b1 - &bias_hat; - let preint = preint + h_bias_accel * bias_diff.accel() + h_bias_gyro * bias_diff.gyro(); - - // Split preint into components - let xi_theta = preint.fixed_rows::<3>(0); - let xi_v = preint.fixed_rows::<3>(3).into_owned(); - let xi_t = preint.fixed_rows::<3>(6).into_owned(); - - let x1_r = x1.rot(); - let x1_t = x1.xyz(); - - // Estimate x2 - // R2_meas = R1 * exp(xi_theta) - let r2_meas = x1_r.compose(&SO3::exp(xi_theta.as_view())); - // v2_meas = v1 + g * dt + R1 * xi_v - let v2_meas = v1.0 + g * dt + x1_r.apply(xi_v.as_view()); - let v2_meas: VectorVar3 = v2_meas.into(); - // p2_meas = p1 + v1 * dt + 0.5 * g * dt^2 + R1 * delta_t - let p2_meas = x1_t + v1.0 * dt + g * dt * dt * D::from(0.5) + x1_r.apply(xi_t.as_view()); - let p2_meas: VectorVar3 = p2_meas.into(); - let b2_meas = b1; - - // Compute residual + let delta = ImuDelta::::dual_convert(&self.delta); + + // Pull out the measurements + let start = ImuState { + r: x1.rot().clone(), + v: v1.0, + p: x1.xyz().into_owned(), + bias: b1, + }; + let ImuState { + r: r2_meas, + v: v2_meas, + p: p2_meas, + bias: b2_meas, + } = delta.predict(&start); + let p2_meas = VectorVar3::from(p2_meas); + let v2_meas = VectorVar3::from(v2_meas); + + // Compute residuals let p_2: VectorVar3 = x2.xyz().into_owned().into(); let r_r = r2_meas.ominus(x2.rot()); - let r_p = p2_meas.ominus(&p_2); let r_vel = v2_meas.ominus(&v2); + let r_p = p2_meas.ominus(&p_2); let r_bias = b2_meas.ominus(&b2); let mut residual = VectorX::zeros(15); @@ -100,33 +321,40 @@ impl_residual!(6, ImuPreintegrationResidual); impl fmt::Display for ImuPreintegrationResidual { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { - write!(f, "ImuPreintegrationResidual(preint: {})", self.preint) + write!( + f, + "ImuPreintegrationResidual(theta: {}, v: {}, t: {})", + self.delta.xi_theta, self.delta.xi_vel, self.delta.xi_pos + ) } } // ------------------------- The Preintegrator ------------------------- // #[derive(Clone, Debug)] pub struct ImuParams { - g: Vector3, + // TODO: Remove gravity from here? + gravity: Vector3, cov_accel: Matrix3, cov_gyro: Matrix3, cov_accel_bias: Matrix3, cov_gyro_bias: Matrix3, cov_integration: Matrix3, - cov_init: Matrix3, + cov_winit: Matrix3, + cov_ainit: Matrix3, } /// Implements reasonable parameters for ImuParams including positive gravity impl Default for ImuParams { fn default() -> Self { Self { - g: Vector3::new(0.0, 0.0, 9.81), + gravity: Vector3::new(0.0, 0.0, 9.81), cov_accel: Matrix3::identity() * 1e-5, cov_gyro: Matrix3::identity() * 1e-5, cov_accel_bias: Matrix3::identity() * 1e-6, cov_gyro_bias: Matrix3::identity() * 1e-6, cov_integration: Matrix3::identity() * 1e-7, - cov_init: Matrix3::identity() * 1e-7, + cov_winit: Matrix3::identity() * 1e-7, + cov_ainit: Matrix3::identity() * 1e-7, } } } @@ -140,7 +368,7 @@ impl ImuParams { /// Create a new set of parameters with negative gravity pub fn negative() -> Self { let mut params = Self::default(); - params.g = -params.g; + params.gravity = -params.gravity; params } @@ -164,99 +392,62 @@ impl ImuParams { self.cov_integration = Matrix3::identity() * val; } + // TODO: Do I really need seperate values for this? + // In practice, I think every makes cov_winit = cov_ainit + // For now, the public interface assumes they are the same, but behind the + // scenes we're using both pub fn set_scalar_init(&mut self, val: dtype) { - self.cov_init = Matrix3::identity() * val; + self.cov_winit = Matrix3::identity() * val; + self.cov_ainit = Matrix3::identity() * val; } } -pub struct ImuPreint { - delta_t: dtype, - delta_r: Vector3, - delta_v: Vector3, - delta_p: Vector3, - bias_hat: ImuBias, - h_bias_accel: Matrix<9, 3>, - h_bias_gyro: Matrix<9, 3>, - gravity: Vector3, -} - +/// Performs Imu preintegration #[derive(Clone, Debug)] pub struct ImuPreintegrator { // Mutable state that will change as we integrate - // TODO: Combine these into a struct? I pass them all into the residual anyways - delta_t: dtype, - delta_r: Vector3, - delta_v: Vector3, - delta_p: Vector3, - bias_hat: ImuBias, - h_bias_accel: Matrix<9, 3>, - h_bias_gyro: Matrix<9, 3>, + delta: ImuDelta, cov: Matrix<15, 15>, // Constants params: ImuParams, } impl ImuPreintegrator { - pub fn new(params: ImuParams, bias_hat: ImuBias) -> Self { + pub fn new(params: ImuParams, bias_init: ImuBias) -> Self { + let delta = ImuDelta::new(params.gravity, bias_init); Self { - delta_t: 0.0, - delta_r: Vector3::zeros(), - delta_v: Vector3::zeros(), - delta_p: Vector3::zeros(), - bias_hat, - h_bias_accel: Matrix::zeros(), - h_bias_gyro: Matrix::zeros(), + delta, // init with small value to avoid singular matrix - cov: Matrix::zeros() * 1e-12, + cov: Matrix::zeros() * 1e-14, params, } } #[allow(non_snake_case)] - pub fn integrate(&mut self, dt: dtype, accel: Vector3, gyro: Vector3) { - // Update preint - self.delta_t += dt; - let accel = accel - self.bias_hat.accel(); - let gyro = gyro - self.bias_hat.gyro(); - let r_kaccel = SO3::exp(self.delta_r.as_view()).apply(accel.as_view()); - let H = SO3::dexp(self.delta_r.as_view()); - let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); - - self.delta_r += Hinv * gyro * dt; - self.delta_v += r_kaccel * dt; - self.delta_p += self.delta_v * dt + 0.5 * r_kaccel * dt * dt; + pub fn integrate(&mut self, imu: ImuMeasurement) { + // Construct all matrices before integrating + let A = self.delta.A(&imu); + let B_Q_BT = self.delta.B_Q_BT(&imu, &self.params); - // Update H + // Update preintegration + self.delta.integrate(&imu); // Update covariance - // TODO: Need to verify dimensions of all these - let R = SO3::exp(self.delta_r.as_view()).to_matrix(); - let mut F = Matrix::<15, 15>::identity(); - let A = Matrix3::identity() - SO3::hat(gyro.as_view()) * dt / 2.0; - F.fixed_view_mut::<3, 3>(0, 0).copy_from(&A); - let A = Hinv * dt; - F.fixed_view_mut::<3, 3>(0, 12).copy_from(&A); - let mut A = -R * SO3::hat(accel.as_view()) * Hinv * dt; - F.fixed_view_mut::<3, 3>(6, 0).copy_from(&A); - A *= dt / 2.0; - F.fixed_view_mut::<3, 3>(3, 0).copy_from(&A); - let A = Matrix3::identity() * dt; - F.fixed_view_mut::<3, 3>(3, 6).copy_from(&A); - let mut A = R * dt; - F.fixed_view_mut::<3, 3>(6, 9).copy_from(&A); - A *= dt / 2.0; - F.fixed_view_mut::<3, 3>(3, 9).copy_from(&A); - - // TODO: Fill out this beast - let G_Q_Gt = Matrix::<15, 15>::identity(); - self.cov = F * self.cov * F.transpose() + G_Q_Gt; + self.cov = A * self.cov * A.transpose() + B_Q_BT; + + // Update H + let Amini = A.fixed_view::<9, 9>(0, 0); + // This should come from B, turns out it's identical as A + let Bgyro = A.fixed_view::<9, 3>(0, 9); + let Baccel = A.fixed_view::<9, 3>(0, 12); + self.delta.h_bias_gyro = Amini * self.delta.h_bias_gyro + Bgyro; + self.delta.h_bias_accel = Amini * self.delta.h_bias_accel + Baccel; } /// Build a corresponding factor /// /// This consumes the preintegrator and returns a - /// [factor](crate::factor::Factor) that can be inserted into a factor - /// graph. + /// [factor](crate::factor::Factor) with the proper noise model. pub fn build( self, x1: X1, @@ -276,22 +467,7 @@ impl ImuPreintegrator { { // Create noise from our covariance matrix let noise = GaussianNoise::from_matrix_cov(self.cov.as_view()); - - // Copy preint into a single vector - let mut preint: Vector<9, dtype> = Vector::zeros(); - preint.fixed_rows_mut::<3>(0).copy_from(&self.delta_r); - preint.fixed_rows_mut::<3>(3).copy_from(&self.delta_v); - preint.fixed_rows_mut::<3>(6).copy_from(&self.delta_p); - - let res = ImuPreintegrationResidual { - preint, - gravity: self.params.g, - dt: self.delta_t, - bias_hat: self.bias_hat, - h_bias_accel: self.h_bias_accel, - h_bias_gyro: self.h_bias_gyro, - }; - + let res = ImuPreintegrationResidual { delta: self.delta }; FactorBuilder::new6(res, x1, v1, b1, x2, v2, b2) .noise(noise) .build() From d0a4cfb7f4abd1c907e474682a3db5041ce4628d Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 28 Aug 2024 14:14:55 -0400 Subject: [PATCH 06/13] Cleaned up implementation quite a bit --- src/residuals/imu_preint.rs | 475 --------------------------- src/residuals/imu_preint/README.md | 0 src/residuals/imu_preint/delta.rs | 217 ++++++++++++ src/residuals/imu_preint/mod.rs | 7 + src/residuals/imu_preint/newtypes.rs | 55 ++++ src/residuals/imu_preint/residual.rs | 221 +++++++++++++ src/residuals/mod.rs | 9 +- src/variables/imu_bias.rs | 9 +- src/variables/vector.rs | 1 - 9 files changed, 514 insertions(+), 480 deletions(-) delete mode 100644 src/residuals/imu_preint.rs create mode 100644 src/residuals/imu_preint/README.md create mode 100644 src/residuals/imu_preint/delta.rs create mode 100644 src/residuals/imu_preint/mod.rs create mode 100644 src/residuals/imu_preint/newtypes.rs create mode 100644 src/residuals/imu_preint/residual.rs diff --git a/src/residuals/imu_preint.rs b/src/residuals/imu_preint.rs deleted file mode 100644 index 03013bb..0000000 --- a/src/residuals/imu_preint.rs +++ /dev/null @@ -1,475 +0,0 @@ -use core::fmt; -use std::ops; - -use nalgebra::Const; - -use super::Residual6; -use crate::{ - containers::{Factor, FactorBuilder, TypedSymbol}, - dtype, - impl_residual, - linalg::{ForwardProp, Matrix, Matrix3, Numeric, Vector3, VectorX}, - noise::GaussianNoise, - tag_residual, - traits::*, - variables::{ImuBias, MatrixLieGroup, VectorVar3, SE3, SO3}, -}; -// ------------------- Objects that perform most of work ------------------- // -/// Struct to hold an Imu state -/// -/// Specifically holds an Imu state to which an ImuDelta can be applied -pub struct ImuState { - r: SO3, - v: Vector3, - p: Vector3, - bias: ImuBias, -} - -impl ImuState { - /// Propagate the state forward by the given delta - /// - /// This function takes an ImuDelta and applies it to the current state. - /// Identical to calling [ImuDelta::predict] with the current state. - pub fn propagate(&self, delta: ImuDelta) -> ImuState { - delta.predict(self) - } -} - -/// Struct to hold the preintegrated Imu delta -#[derive(Clone, Debug)] -#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] -pub struct ImuDelta { - dt: D, - xi_theta: Vector3, - xi_vel: Vector3, - xi_pos: Vector3, - bias_init: ImuBias, - h_bias_accel: Matrix<9, 3, D>, - h_bias_gyro: Matrix<9, 3, D>, - gravity: Vector3, -} - -impl ImuDelta { - fn new(gravity: Vector3, bias_init: ImuBias) -> Self { - Self { - dt: D::from(0.0), - xi_theta: Vector3::zeros(), - xi_vel: Vector3::zeros(), - xi_pos: Vector3::zeros(), - bias_init, - h_bias_accel: Matrix::zeros(), - h_bias_gyro: Matrix::zeros(), - gravity, - } - } - - fn remove_bias(&self, imu: &ImuMeasurement) -> ImuMeasurement { - imu - &self.bias_init - } - - fn first_order_update(&self, bias_diff: &ImuBias, idx: usize) -> Vector3 { - self.h_bias_accel.fixed_rows(idx) * bias_diff.accel() - + self.h_bias_gyro.fixed_rows(idx) * bias_diff.gyro() - } - - pub fn predict(&self, x1: &ImuState) -> ImuState { - let ImuState { r, v, p, bias } = x1; - - // Compute first-order updates to preint with bias - let bias_diff = bias - &self.bias_init; - let xi_theta = self.xi_theta + self.first_order_update(&bias_diff, 0); - let xi_v = self.xi_vel + self.first_order_update(&bias_diff, 3); - let xi_p = self.xi_pos + self.first_order_update(&bias_diff, 6); - - // Estimate x2 - // R2_meas = R1 * exp(xi_theta) - let r2_meas = r.compose(&SO3::exp(xi_theta.as_view())); - // v2_meas = v + g * dt + R1 * xi_v - let v2_meas = v + self.gravity * self.dt + r.apply(xi_v.as_view()); - let v2_meas = v2_meas.into(); - // p2_meas = p1 + v * dt + 0.5 * g * dt^2 + R1 * xi_p - let p2_meas = p - + v * self.dt - + self.gravity * self.dt * self.dt * D::from(0.5) - + r.apply(xi_p.as_view()); - let p2_meas = p2_meas.into(); - let b2_meas = bias.clone(); - - ImuState { - r: r2_meas, - v: v2_meas, - p: p2_meas, - bias: b2_meas, - } - } -} - -// None of these methods should need to be dualized / "backpropagated" -// Can just assume default dtype -impl ImuDelta { - #[allow(non_snake_case)] - pub fn integrate(&mut self, imu: &ImuMeasurement) { - self.dt += imu.dt; - let accel_world = SO3::exp(self.xi_theta.as_view()).apply(imu.accel.as_view()); - let H = SO3::dexp(self.xi_theta.as_view()); - let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); - - self.xi_theta += Hinv * imu.gyro * imu.dt; - self.xi_vel += accel_world * imu.dt; - self.xi_pos += self.xi_vel * imu.dt + accel_world * (imu.dt * imu.dt * 0.5); - } - - #[allow(non_snake_case)] - pub fn A(&self, imu: &ImuMeasurement) -> Matrix<15, 15> { - let H = SO3::dexp(self.xi_theta.as_view()); - let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); - let R = SO3::exp(self.xi_theta.as_view()).to_matrix(); - - let mut A = Matrix::<15, 15>::identity(); - - // First column (wrt theta) - let M = Matrix3::identity() - SO3::hat(imu.gyro.as_view()) * imu.dt / 2.0; - A.fixed_view_mut::<3, 3>(0, 0).copy_from(&M); - let mut M = -R * SO3::hat(imu.accel.as_view()) * H * imu.dt; - A.fixed_view_mut::<3, 3>(3, 0).copy_from(&M); - M *= imu.dt / 2.0; - A.fixed_view_mut::<3, 3>(6, 0).copy_from(&M); - - // Second column (wrt vel) - let M = Matrix3::identity() * imu.dt; - A.fixed_view_mut::<3, 3>(0, 6).copy_from(&M); - - // Third column (wrt pos) - - // Fourth column (wrt gyro bias) - let M = -Hinv * imu.dt; - A.fixed_view_mut::<3, 3>(0, 9).copy_from(&M); - - // Fifth column (wrt accel bias) - let mut M = -R * imu.dt; - A.fixed_view_mut::<3, 3>(6, 9).copy_from(&M); - M *= imu.dt / 2.0; - A.fixed_view_mut::<3, 3>(3, 9).copy_from(&M); - - A - } - - #[allow(non_snake_case)] - pub fn B_Q_BT(&self, imu: &ImuMeasurement, p: &ImuParams) -> Matrix<15, 15> { - let H = SO3::dexp(self.xi_theta.as_view()); - let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); - let R = SO3::exp(self.xi_theta.as_view()).to_matrix(); - - // Construct all partials - let H_theta_w = Hinv * imu.dt; - let H_theta_winit = -Hinv * self.dt; - - let H_v_a = R * imu.dt; - let H_v_ainit = -R * self.dt; - - let H_p_a = H_v_a * imu.dt / 2.0; - let H_p_int: Matrix3 = Matrix3::identity(); - let H_p_ainit = H_v_ainit * self.dt / 2.0; - - // Copy them into place - let mut B = Matrix::<15, 15>::zeros(); - // First column (wrt theta) - let M = H_theta_w * p.cov_gyro_bias * H_theta_w.transpose() - + H_theta_winit * p.cov_winit * H_theta_winit.transpose(); - B.fixed_view_mut::<3, 3>(0, 0).copy_from(&M); - - // Second column (wrt vel) - let M = H_v_a * p.cov_accel * H_v_a.transpose() - + H_v_ainit * p.cov_ainit * H_v_ainit.transpose(); - B.fixed_view_mut::<3, 3>(3, 3).copy_from(&M); - let M = H_p_a * p.cov_accel * H_v_a.transpose() - + H_p_ainit * p.cov_ainit * H_v_ainit.transpose(); - B.fixed_view_mut::<3, 3>(6, 3).copy_from(&M); - - // Third column (wrt pos) - let M = H_v_a * p.cov_accel * H_p_a.transpose() - + H_v_ainit * p.cov_ainit * H_p_ainit.transpose(); - B.fixed_view_mut::<3, 3>(3, 6).copy_from(&M); - let M = H_p_a * p.cov_accel * H_p_a.transpose() - + H_p_int * p.cov_integration * H_p_int.transpose() - + H_p_ainit * p.cov_ainit * H_p_ainit.transpose(); - B.fixed_view_mut::<3, 3>(6, 6).copy_from(&M); - - // Fourth column (wrt gyro bias) - B.fixed_view_mut::<3, 3>(9, 9).copy_from(&p.cov_gyro_bias); - - // Fifth column (wrt accel bias) - B.fixed_view_mut::<3, 3>(12, 12) - .copy_from(&p.cov_accel_bias); - - B - } -} - -impl DualConvert for ImuDelta { - type Alias = ImuDelta
; - fn dual_convert(other: &Self::Alias) -> Self::Alias
{ - ImuDelta { - dt: other.dt.into(), - xi_theta: Vector3::::dual_convert(&other.xi_theta), - xi_vel: Vector3::::dual_convert(&other.xi_vel), - xi_pos: Vector3::::dual_convert(&other.xi_pos), - bias_init: ImuBias::::dual_convert(&other.bias_init), - h_bias_accel: Matrix::<9, 3, D>::dual_convert(&other.h_bias_accel), - h_bias_gyro: Matrix::<9, 3, D>::dual_convert(&other.h_bias_gyro), - gravity: Vector3::::dual_convert(&other.gravity), - } - } -} - -pub struct ImuMeasurement { - accel: Vector3, - gyro: Vector3, - dt: D, -} - -impl ops::Sub> for ImuMeasurement { - type Output = Self; - - fn sub(self, rhs: ImuBias) -> Self { - ImuMeasurement { - gyro: self.gyro - rhs.gyro(), - accel: self.accel - rhs.accel(), - dt: self.dt, - } - } -} - -impl<'a, D: Numeric> ops::Sub<&'a ImuBias> for &'a ImuMeasurement { - type Output = ImuMeasurement; - - fn sub(self, rhs: &'a ImuBias) -> ImuMeasurement { - ImuMeasurement { - gyro: self.gyro - rhs.gyro(), - accel: self.accel - rhs.accel(), - dt: self.dt, - } - } -} - -// ----------------------- The actual residual object ----------------------- // -tag_residual!(ImuPreintegrationResidual); - -#[derive(Clone, Debug)] -#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] -pub struct ImuPreintegrationResidual { - delta: ImuDelta, -} - -impl Residual6 for ImuPreintegrationResidual { - type Differ = ForwardProp>; - type DimIn = Const<30>; - type DimOut = Const<15>; - type V1 = SE3; - type V2 = VectorVar3; - type V3 = ImuBias; - type V4 = SE3; - type V5 = VectorVar3; - type V6 = ImuBias; - - fn residual6( - &self, - x1: SE3, - v1: VectorVar3, - b1: ImuBias, - x2: SE3, - v2: VectorVar3, - b2: ImuBias, - ) -> VectorX { - // Add dual types to all of our fields - let delta = ImuDelta::::dual_convert(&self.delta); - - // Pull out the measurements - let start = ImuState { - r: x1.rot().clone(), - v: v1.0, - p: x1.xyz().into_owned(), - bias: b1, - }; - let ImuState { - r: r2_meas, - v: v2_meas, - p: p2_meas, - bias: b2_meas, - } = delta.predict(&start); - let p2_meas = VectorVar3::from(p2_meas); - let v2_meas = VectorVar3::from(v2_meas); - - // Compute residuals - let p_2: VectorVar3 = x2.xyz().into_owned().into(); - let r_r = r2_meas.ominus(x2.rot()); - let r_vel = v2_meas.ominus(&v2); - let r_p = p2_meas.ominus(&p_2); - let r_bias = b2_meas.ominus(&b2); - - let mut residual = VectorX::zeros(15); - residual.fixed_rows_mut::<3>(0).copy_from(&r_r); - residual.fixed_rows_mut::<3>(3).copy_from(&r_vel); - residual.fixed_rows_mut::<3>(6).copy_from(&r_p); - residual.fixed_rows_mut::<6>(9).copy_from(&r_bias); - - residual - } -} - -impl_residual!(6, ImuPreintegrationResidual); - -impl fmt::Display for ImuPreintegrationResidual { - fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { - write!( - f, - "ImuPreintegrationResidual(theta: {}, v: {}, t: {})", - self.delta.xi_theta, self.delta.xi_vel, self.delta.xi_pos - ) - } -} - -// ------------------------- The Preintegrator ------------------------- // -#[derive(Clone, Debug)] -pub struct ImuParams { - // TODO: Remove gravity from here? - gravity: Vector3, - cov_accel: Matrix3, - cov_gyro: Matrix3, - cov_accel_bias: Matrix3, - cov_gyro_bias: Matrix3, - cov_integration: Matrix3, - cov_winit: Matrix3, - cov_ainit: Matrix3, -} - -/// Implements reasonable parameters for ImuParams including positive gravity -impl Default for ImuParams { - fn default() -> Self { - Self { - gravity: Vector3::new(0.0, 0.0, 9.81), - cov_accel: Matrix3::identity() * 1e-5, - cov_gyro: Matrix3::identity() * 1e-5, - cov_accel_bias: Matrix3::identity() * 1e-6, - cov_gyro_bias: Matrix3::identity() * 1e-6, - cov_integration: Matrix3::identity() * 1e-7, - cov_winit: Matrix3::identity() * 1e-7, - cov_ainit: Matrix3::identity() * 1e-7, - } - } -} - -impl ImuParams { - /// Create a new set of parameters with positive gravity - pub fn positive() -> Self { - Self::default() - } - - /// Create a new set of parameters with negative gravity - pub fn negative() -> Self { - let mut params = Self::default(); - params.gravity = -params.gravity; - params - } - - pub fn set_scalar_accel(&mut self, val: dtype) { - self.cov_accel = Matrix3::identity() * val; - } - - pub fn set_scalar_gyro(&mut self, val: dtype) { - self.cov_gyro = Matrix3::identity() * val; - } - - pub fn set_scalar_accel_bias(&mut self, val: dtype) { - self.cov_accel_bias = Matrix3::identity() * val; - } - - pub fn set_scalar_gyro_bias(&mut self, val: dtype) { - self.cov_gyro_bias = Matrix3::identity() * val; - } - - pub fn set_scalar_integration(&mut self, val: dtype) { - self.cov_integration = Matrix3::identity() * val; - } - - // TODO: Do I really need seperate values for this? - // In practice, I think every makes cov_winit = cov_ainit - // For now, the public interface assumes they are the same, but behind the - // scenes we're using both - pub fn set_scalar_init(&mut self, val: dtype) { - self.cov_winit = Matrix3::identity() * val; - self.cov_ainit = Matrix3::identity() * val; - } -} - -/// Performs Imu preintegration -#[derive(Clone, Debug)] -pub struct ImuPreintegrator { - // Mutable state that will change as we integrate - delta: ImuDelta, - cov: Matrix<15, 15>, - // Constants - params: ImuParams, -} - -impl ImuPreintegrator { - pub fn new(params: ImuParams, bias_init: ImuBias) -> Self { - let delta = ImuDelta::new(params.gravity, bias_init); - Self { - delta, - // init with small value to avoid singular matrix - cov: Matrix::zeros() * 1e-14, - params, - } - } - - #[allow(non_snake_case)] - pub fn integrate(&mut self, imu: ImuMeasurement) { - // Construct all matrices before integrating - let A = self.delta.A(&imu); - let B_Q_BT = self.delta.B_Q_BT(&imu, &self.params); - - // Update preintegration - self.delta.integrate(&imu); - - // Update covariance - self.cov = A * self.cov * A.transpose() + B_Q_BT; - - // Update H - let Amini = A.fixed_view::<9, 9>(0, 0); - // This should come from B, turns out it's identical as A - let Bgyro = A.fixed_view::<9, 3>(0, 9); - let Baccel = A.fixed_view::<9, 3>(0, 12); - self.delta.h_bias_gyro = Amini * self.delta.h_bias_gyro + Bgyro; - self.delta.h_bias_accel = Amini * self.delta.h_bias_accel + Baccel; - } - - /// Build a corresponding factor - /// - /// This consumes the preintegrator and returns a - /// [factor](crate::factor::Factor) with the proper noise model. - pub fn build( - self, - x1: X1, - v1: V1, - b1: B1, - x2: X2, - v2: V2, - b2: B2, - ) -> Factor - where - X1: TypedSymbol, - V1: TypedSymbol, - B1: TypedSymbol, - X2: TypedSymbol, - V2: TypedSymbol, - B2: TypedSymbol, - { - // Create noise from our covariance matrix - let noise = GaussianNoise::from_matrix_cov(self.cov.as_view()); - let res = ImuPreintegrationResidual { delta: self.delta }; - FactorBuilder::new6(res, x1, v1, b1, x2, v2, b2) - .noise(noise) - .build() - } -} diff --git a/src/residuals/imu_preint/README.md b/src/residuals/imu_preint/README.md new file mode 100644 index 0000000..e69de29 diff --git a/src/residuals/imu_preint/delta.rs b/src/residuals/imu_preint/delta.rs new file mode 100644 index 0000000..dbcc2f5 --- /dev/null +++ b/src/residuals/imu_preint/delta.rs @@ -0,0 +1,217 @@ +use core::fmt; + +use super::{Accel, Gravity, Gyro, ImuCovariance, ImuState}; +use crate::{ + dtype, + linalg::{Matrix, Matrix3, Numeric, Vector3}, + traits::{DualConvert, Variable}, + variables::{ImuBias, MatrixLieGroup, SO3}, +}; + +/// Struct to hold the preintegrated Imu delta +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] +pub(crate) struct ImuDelta { + dt: D, + xi_theta: Vector3, + xi_vel: Vector3, + xi_pos: Vector3, + bias_init: ImuBias, + h_bias_accel: Matrix<9, 3, D>, + h_bias_gyro: Matrix<9, 3, D>, + gravity: Vector3, +} + +impl ImuDelta { + pub(crate) fn new(gravity: Gravity, bias_init: ImuBias) -> Self { + Self { + dt: D::from(0.0), + xi_theta: Vector3::zeros(), + xi_vel: Vector3::zeros(), + xi_pos: Vector3::zeros(), + bias_init, + h_bias_accel: Matrix::zeros(), + h_bias_gyro: Matrix::zeros(), + gravity: gravity.0, + } + } + + pub(crate) fn bias_init(&self) -> &ImuBias { + &self.bias_init + } + + pub(crate) fn first_order_update(&self, bias_diff: &ImuBias, idx: usize) -> Vector3 { + self.h_bias_accel.fixed_rows(idx) * bias_diff.accel() + + self.h_bias_gyro.fixed_rows(idx) * bias_diff.gyro() + } + + pub(crate) fn predict(&self, x1: &ImuState) -> ImuState { + let ImuState { r, v, p, bias } = x1; + + // Compute first-order updates to preint with bias + let bias_diff = bias - &self.bias_init; + let xi_theta = self.xi_theta + self.first_order_update(&bias_diff, 0); + let xi_v = self.xi_vel + self.first_order_update(&bias_diff, 3); + let xi_p = self.xi_pos + self.first_order_update(&bias_diff, 6); + + // Estimate x2 + // R2_meas = R1 * exp(xi_theta) + let r2_meas = r.compose(&SO3::exp(xi_theta.as_view())); + + // v2_meas = v + g * dt + R1 * xi_v + let v2_meas = v + self.gravity * self.dt + r.apply(xi_v.as_view()); + + // p2_meas = p1 + v * dt + 0.5 * g * dt^2 + R1 * xi_p + let p2_meas = p + + v * self.dt + + self.gravity * self.dt * self.dt * D::from(0.5) + + r.apply(xi_p.as_view()); + + let b2_meas = bias.clone(); + + ImuState { + r: r2_meas, + v: v2_meas, + p: p2_meas, + bias: b2_meas, + } + } +} + +// None of these methods should need to be dualized / "backpropagated" +// Can just assume default dtype +impl ImuDelta { + #[allow(non_snake_case)] + pub(crate) fn propagate_H(&mut self, A: &Matrix<15, 15>) { + let Amini = A.fixed_view::<9, 9>(0, 0); + // This should come from B, turns out it's identical as A + let Bgyro = A.fixed_view::<9, 3>(0, 9); + let Baccel = A.fixed_view::<9, 3>(0, 12); + self.h_bias_gyro = Amini * self.h_bias_gyro + Bgyro; + self.h_bias_accel = Amini * self.h_bias_accel + Baccel; + } + + #[allow(non_snake_case)] + pub(crate) fn integrate(&mut self, gyro: &Gyro, accel: &Accel, dt: dtype) { + self.dt += dt; + let accel_world = SO3::exp(self.xi_theta.as_view()).apply(accel.0.as_view()); + let H = SO3::dexp(self.xi_theta.as_view()); + let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); + + self.xi_theta += Hinv * gyro.0 * dt; + self.xi_vel += accel_world * dt; + self.xi_pos += self.xi_vel * dt + accel_world * (dt * dt * 0.5); + } + + #[allow(non_snake_case)] + pub(crate) fn A(&self, gyro: &Gyro, accel: &Accel, dt: dtype) -> Matrix<15, 15> { + let H = SO3::dexp(self.xi_theta.as_view()); + let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); + let R = SO3::exp(self.xi_theta.as_view()).to_matrix(); + + let mut A = Matrix::<15, 15>::identity(); + + // First column (wrt theta) + let M = Matrix3::identity() - SO3::hat(gyro.0.as_view()) * dt / 2.0; + A.fixed_view_mut::<3, 3>(0, 0).copy_from(&M); + let mut M = -R * SO3::hat(accel.0.as_view()) * H * dt; + A.fixed_view_mut::<3, 3>(3, 0).copy_from(&M); + M *= dt / 2.0; + A.fixed_view_mut::<3, 3>(6, 0).copy_from(&M); + + // Second column (wrt vel) + let M = Matrix3::identity() * dt; + A.fixed_view_mut::<3, 3>(0, 6).copy_from(&M); + + // Third column (wrt pos) + + // Fourth column (wrt gyro bias) + let M = -Hinv * dt; + A.fixed_view_mut::<3, 3>(0, 9).copy_from(&M); + + // Fifth column (wrt accel bias) + let mut M = -R * dt; + A.fixed_view_mut::<3, 3>(6, 9).copy_from(&M); + M *= dt / 2.0; + A.fixed_view_mut::<3, 3>(3, 9).copy_from(&M); + + A + } + + #[allow(non_snake_case)] + pub(crate) fn B_Q_BT(&self, p: &ImuCovariance, dt: dtype) -> Matrix<15, 15> { + let H = SO3::dexp(self.xi_theta.as_view()); + let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); + let R = SO3::exp(self.xi_theta.as_view()).to_matrix(); + + // Construct all partials + let H_theta_w = Hinv * dt; + let H_theta_winit = -Hinv * self.dt; + + let H_v_a = R * dt; + let H_v_ainit = -R * self.dt; + + let H_p_a = H_v_a * dt / 2.0; + let H_p_int: Matrix3 = Matrix3::identity(); + let H_p_ainit = H_v_ainit * self.dt / 2.0; + + // Copy them into place + let mut B = Matrix::<15, 15>::zeros(); + // First column (wrt theta) + let M = H_theta_w * p.cov_gyro_bias * H_theta_w.transpose() + + H_theta_winit * p.cov_winit * H_theta_winit.transpose(); + B.fixed_view_mut::<3, 3>(0, 0).copy_from(&M); + + // Second column (wrt vel) + let M = H_v_a * p.cov_accel * H_v_a.transpose() + + H_v_ainit * p.cov_ainit * H_v_ainit.transpose(); + B.fixed_view_mut::<3, 3>(3, 3).copy_from(&M); + let M = H_p_a * p.cov_accel * H_v_a.transpose() + + H_p_ainit * p.cov_ainit * H_v_ainit.transpose(); + B.fixed_view_mut::<3, 3>(6, 3).copy_from(&M); + + // Third column (wrt pos) + let M = H_v_a * p.cov_accel * H_p_a.transpose() + + H_v_ainit * p.cov_ainit * H_p_ainit.transpose(); + B.fixed_view_mut::<3, 3>(3, 6).copy_from(&M); + let M = H_p_a * p.cov_accel * H_p_a.transpose() + + H_p_int * p.cov_integration * H_p_int.transpose() + + H_p_ainit * p.cov_ainit * H_p_ainit.transpose(); + B.fixed_view_mut::<3, 3>(6, 6).copy_from(&M); + + // Fourth column (wrt gyro bias) + B.fixed_view_mut::<3, 3>(9, 9).copy_from(&p.cov_gyro_bias); + + // Fifth column (wrt accel bias) + B.fixed_view_mut::<3, 3>(12, 12) + .copy_from(&p.cov_accel_bias); + + B + } +} + +impl DualConvert for ImuDelta { + type Alias = ImuDelta
; + fn dual_convert(other: &Self::Alias) -> Self::Alias
{ + ImuDelta { + dt: other.dt.into(), + xi_theta: Vector3::::dual_convert(&other.xi_theta), + xi_vel: Vector3::::dual_convert(&other.xi_vel), + xi_pos: Vector3::::dual_convert(&other.xi_pos), + bias_init: ImuBias::::dual_convert(&other.bias_init), + h_bias_accel: Matrix::<9, 3, D>::dual_convert(&other.h_bias_accel), + h_bias_gyro: Matrix::<9, 3, D>::dual_convert(&other.h_bias_gyro), + gravity: Vector3::::dual_convert(&other.gravity), + } + } +} + +impl fmt::Display for ImuDelta { + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!( + f, + "ImuDelta(dt: {}, theta: {}, v: {}, p: {})", + self.dt, self.xi_theta, self.xi_vel, self.xi_pos + ) + } +} diff --git a/src/residuals/imu_preint/mod.rs b/src/residuals/imu_preint/mod.rs new file mode 100644 index 0000000..cb9400f --- /dev/null +++ b/src/residuals/imu_preint/mod.rs @@ -0,0 +1,7 @@ +mod newtypes; +pub use newtypes::{Accel, Gravity, Gyro, ImuState}; + +mod delta; + +mod residual; +pub use residual::{ImuCovariance, ImuPreintegrationResidual, ImuPreintegrator}; diff --git a/src/residuals/imu_preint/newtypes.rs b/src/residuals/imu_preint/newtypes.rs new file mode 100644 index 0000000..29985c5 --- /dev/null +++ b/src/residuals/imu_preint/newtypes.rs @@ -0,0 +1,55 @@ +use crate::{ + dtype, + linalg::{Numeric, Vector3}, + variables::{ImuBias, SO3}, +}; + +/// Gyro measurement +/// +/// This is a newtype for the gyro measurement ensure that the accel and gyro +/// aren't mixed up. +pub struct Gyro(pub Vector3); + +impl Gyro { + pub fn remove_bias(&self, bias: &ImuBias) -> Gyro { + Gyro(self.0 - bias.gyro()) + } +} + +/// Accel measurement newtype +/// +/// This is a newtype for the accel measurement ensure that the accel and gyro +/// aren't mixed up. +pub struct Accel(pub Vector3); + +impl Accel { + pub fn remove_bias(&self, bias: &ImuBias) -> Accel { + Accel(self.0 - bias.accel()) + } +} + +/// Gravity vector +/// +/// This is a newtype for the gravity vector to ensure that it is not mixed up +/// with other vectors and to provide some convenience methods. +pub struct Gravity(pub Vector3); + +impl Gravity { + pub fn up() -> Self { + Gravity(Vector3::new(D::from(0.0), D::from(0.0), D::from(9.81))) + } + + pub fn down() -> Self { + Gravity(Vector3::new(D::from(0.0), D::from(0.0), D::from(-9.81))) + } +} + +/// Struct to hold an Imu state +/// +/// Specifically holds an Imu state to which an ImuDelta can be applied +pub struct ImuState { + pub r: SO3, + pub v: Vector3, + pub p: Vector3, + pub bias: ImuBias, +} diff --git a/src/residuals/imu_preint/residual.rs b/src/residuals/imu_preint/residual.rs new file mode 100644 index 0000000..d6d01f2 --- /dev/null +++ b/src/residuals/imu_preint/residual.rs @@ -0,0 +1,221 @@ +use std::fmt; + +use nalgebra::Const; + +use super::{delta::ImuDelta, Accel, Gravity, Gyro, ImuState}; +use crate::{ + containers::{Factor, FactorBuilder, TypedSymbol}, + dtype, + impl_residual, + linalg::{ForwardProp, Matrix, Matrix3, VectorX}, + noise::GaussianNoise, + residuals::Residual6, + tag_residual, + traits::*, + variables::{ImuBias, VectorVar3, SE3}, +}; +// ------------------------- Covariances ------------------------- // + +#[derive(Clone, Debug)] +pub struct ImuCovariance { + pub cov_accel: Matrix3, + pub cov_gyro: Matrix3, + pub cov_accel_bias: Matrix3, + pub cov_gyro_bias: Matrix3, + pub cov_integration: Matrix3, + pub cov_winit: Matrix3, + pub cov_ainit: Matrix3, +} + +/// Implements reasonable parameters for ImuCovariance including positive +/// gravity +impl Default for ImuCovariance { + fn default() -> Self { + Self { + cov_accel: Matrix3::identity() * 1e-5, + cov_gyro: Matrix3::identity() * 1e-5, + cov_accel_bias: Matrix3::identity() * 1e-6, + cov_gyro_bias: Matrix3::identity() * 1e-6, + cov_integration: Matrix3::identity() * 1e-7, + cov_winit: Matrix3::identity() * 1e-7, + cov_ainit: Matrix3::identity() * 1e-7, + } + } +} + +impl ImuCovariance { + pub fn set_scalar_accel(&mut self, val: dtype) { + self.cov_accel = Matrix3::identity() * val; + } + + pub fn set_scalar_gyro(&mut self, val: dtype) { + self.cov_gyro = Matrix3::identity() * val; + } + + pub fn set_scalar_accel_bias(&mut self, val: dtype) { + self.cov_accel_bias = Matrix3::identity() * val; + } + + pub fn set_scalar_gyro_bias(&mut self, val: dtype) { + self.cov_gyro_bias = Matrix3::identity() * val; + } + + pub fn set_scalar_integration(&mut self, val: dtype) { + self.cov_integration = Matrix3::identity() * val; + } + + // In practice, I think everyone makes cov_winit = cov_ainit + // For now, the public interface assumes they are the same, but behind the + // scenes we're using both + pub fn set_scalar_init(&mut self, val: dtype) { + self.cov_winit = Matrix3::identity() * val; + self.cov_ainit = Matrix3::identity() * val; + } +} + +// ------------------------- The Preintegrator ------------------------- // +/// Performs Imu preintegration +#[derive(Clone, Debug)] +pub struct ImuPreintegrator { + // Mutable state that will change as we integrate + delta: ImuDelta, + cov: Matrix<15, 15>, + // Constants + params: ImuCovariance, +} + +impl ImuPreintegrator { + pub fn new(params: ImuCovariance, bias_init: ImuBias, gravity: Gravity) -> Self { + let delta = ImuDelta::new(gravity, bias_init); + Self { + delta, + // init with small value to avoid singular matrix + cov: Matrix::zeros() * 1e-14, + params, + } + } + + #[allow(non_snake_case)] + pub fn integrate(&mut self, gyro: Gyro, accel: Accel, dt: dtype) { + // Remove bias estimate + let gyro = gyro.remove_bias(self.delta.bias_init()); + let accel = accel.remove_bias(self.delta.bias_init()); + + // Construct all matrices before integrating + let A = self.delta.A(&gyro, &accel, dt); + let B_Q_BT = self.delta.B_Q_BT(&self.params, dt); + + // Update preintegration + self.delta.integrate(&gyro, &accel, dt); + + // Update covariance + self.cov = A * self.cov * A.transpose() + B_Q_BT; + + // Update H for bias updates + self.delta.propagate_H(&A); + } + + /// Build a corresponding factor + /// + /// This consumes the preintegrator and returns a + /// [factor](crate::factor::Factor) with the proper noise model. + pub fn build( + self, + x1: X1, + v1: V1, + b1: B1, + x2: X2, + v2: V2, + b2: B2, + ) -> Factor + where + X1: TypedSymbol, + V1: TypedSymbol, + B1: TypedSymbol, + X2: TypedSymbol, + V2: TypedSymbol, + B2: TypedSymbol, + { + // Create noise from our covariance matrix + // TODO: Does covariance need to be modified as preint will be modified? + let noise = GaussianNoise::from_matrix_cov(self.cov.as_view()); + let res = ImuPreintegrationResidual { delta: self.delta }; + FactorBuilder::new6(res, x1, v1, b1, x2, v2, b2) + .noise(noise) + .build() + } +} + +// ------------------------- The Residual ------------------------- // + +tag_residual!(ImuPreintegrationResidual); + +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] +pub struct ImuPreintegrationResidual { + delta: ImuDelta, +} + +impl Residual6 for ImuPreintegrationResidual { + type Differ = ForwardProp>; + type DimIn = Const<30>; + type DimOut = Const<15>; + type V1 = SE3; + type V2 = VectorVar3; + type V3 = ImuBias; + type V4 = SE3; + type V5 = VectorVar3; + type V6 = ImuBias; + + fn residual6( + &self, + x1: SE3, + v1: VectorVar3, + b1: ImuBias, + x2: SE3, + v2: VectorVar3, + b2: ImuBias, + ) -> VectorX { + // Add dual types to all of our fields + let delta = ImuDelta::::dual_convert(&self.delta); + + // Pull out the measurements + let start = ImuState { + r: x1.rot().clone(), + v: v1.0, + p: x1.xyz().into_owned(), + bias: b1, + }; + let ImuState { + r: r2_meas, + v: v2_meas, + p: p2_meas, + bias: b2_meas, + } = delta.predict(&start); + let p_2: VectorVar3 = x2.xyz().into_owned().into(); + let p2_meas: VectorVar3 = p2_meas.into(); + let v2_meas: VectorVar3 = v2_meas.into(); + + // Compute residuals + let r_r = r2_meas.ominus(x2.rot()); + let r_vel = v2_meas.ominus(&v2); + let r_p = p2_meas.ominus(&p_2); + let r_bias = b2_meas.ominus(&b2); + + let mut residual = VectorX::zeros(15); + residual.fixed_rows_mut::<3>(0).copy_from(&r_r); + residual.fixed_rows_mut::<3>(3).copy_from(&r_vel); + residual.fixed_rows_mut::<3>(6).copy_from(&r_p); + residual.fixed_rows_mut::<6>(9).copy_from(&r_bias); + + residual + } +} + +impl_residual!(6, ImuPreintegrationResidual); + +impl fmt::Display for ImuPreintegrationResidual { + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!(f, "ImuPreintegrationResidual({})", self.delta) + } +} diff --git a/src/residuals/mod.rs b/src/residuals/mod.rs index d88ac85..3e95313 100644 --- a/src/residuals/mod.rs +++ b/src/residuals/mod.rs @@ -23,6 +23,13 @@ mod between; pub use between::BetweenResidual; mod imu_preint; -pub use imu_preint::{ImuParams, ImuPreintegrationResidual, ImuPreintegrator}; +pub use imu_preint::{ + Accel, + Gravity, + Gyro, + ImuCovariance, + ImuPreintegrationResidual, + ImuPreintegrator, +}; mod macros; diff --git a/src/variables/imu_bias.rs b/src/variables/imu_bias.rs index 18175bf..6a53134 100644 --- a/src/variables/imu_bias.rs +++ b/src/variables/imu_bias.rs @@ -6,6 +6,7 @@ use super::Variable; use crate::{ dtype, linalg::{DualVector, Numeric, Vector3, VectorDim}, + residuals::{Accel, Gyro}, tag_variable, }; @@ -24,9 +25,11 @@ pub struct ImuBias { impl ImuBias { /// Create a new IMU bias - // TODO: I would love to see a way to verify these aren't put in backwards - pub fn new(gyro: Vector3, accel: Vector3) -> Self { - ImuBias { gyro, accel } + pub fn new(gyro: Gyro, accel: Accel) -> Self { + ImuBias { + gyro: gyro.0, + accel: accel.0, + } } /// Get the gyro bias diff --git a/src/variables/vector.rs b/src/variables/vector.rs index 4b92fcc..f14390a 100644 --- a/src/variables/vector.rs +++ b/src/variables/vector.rs @@ -86,7 +86,6 @@ impl Variable for VectorVar { } } -// TODO: New methods for each size macro_rules! impl_vector_new { ($($num:literal, [$($args:ident),*]);* $(;)?) => {$( impl VectorVar<$num, D> { From d11f43e463087c3e86de3b9c33624886ecd004f9 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Fri, 30 Aug 2024 15:25:09 -0400 Subject: [PATCH 07/13] Tweaks per Taylor's suggestion --- src/residuals/imu_preint/delta.rs | 10 +++++----- src/residuals/imu_preint/newtypes.rs | 6 ++++++ src/residuals/imu_preint/residual.rs | 2 +- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/residuals/imu_preint/delta.rs b/src/residuals/imu_preint/delta.rs index dbcc2f5..e92bc7e 100644 --- a/src/residuals/imu_preint/delta.rs +++ b/src/residuals/imu_preint/delta.rs @@ -19,7 +19,7 @@ pub(crate) struct ImuDelta { bias_init: ImuBias, h_bias_accel: Matrix<9, 3, D>, h_bias_gyro: Matrix<9, 3, D>, - gravity: Vector3, + gravity: Gravity, } impl ImuDelta { @@ -32,7 +32,7 @@ impl ImuDelta { bias_init, h_bias_accel: Matrix::zeros(), h_bias_gyro: Matrix::zeros(), - gravity: gravity.0, + gravity, } } @@ -59,12 +59,12 @@ impl ImuDelta { let r2_meas = r.compose(&SO3::exp(xi_theta.as_view())); // v2_meas = v + g * dt + R1 * xi_v - let v2_meas = v + self.gravity * self.dt + r.apply(xi_v.as_view()); + let v2_meas = v + self.gravity.0 * self.dt + r.apply(xi_v.as_view()); // p2_meas = p1 + v * dt + 0.5 * g * dt^2 + R1 * xi_p let p2_meas = p + v * self.dt - + self.gravity * self.dt * self.dt * D::from(0.5) + + self.gravity.0 * self.dt * self.dt * D::from(0.5) + r.apply(xi_p.as_view()); let b2_meas = bias.clone(); @@ -201,7 +201,7 @@ impl DualConvert for ImuDelta { bias_init: ImuBias::::dual_convert(&other.bias_init), h_bias_accel: Matrix::<9, 3, D>::dual_convert(&other.h_bias_accel), h_bias_gyro: Matrix::<9, 3, D>::dual_convert(&other.h_bias_gyro), - gravity: Vector3::::dual_convert(&other.gravity), + gravity: Gravity(Vector3::::dual_convert(&other.gravity.0)), } } } diff --git a/src/residuals/imu_preint/newtypes.rs b/src/residuals/imu_preint/newtypes.rs index 29985c5..281a01c 100644 --- a/src/residuals/imu_preint/newtypes.rs +++ b/src/residuals/imu_preint/newtypes.rs @@ -8,6 +8,8 @@ use crate::{ /// /// This is a newtype for the gyro measurement ensure that the accel and gyro /// aren't mixed up. +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct Gyro(pub Vector3); impl Gyro { @@ -20,6 +22,8 @@ impl Gyro { /// /// This is a newtype for the accel measurement ensure that the accel and gyro /// aren't mixed up. +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct Accel(pub Vector3); impl Accel { @@ -32,6 +36,8 @@ impl Accel { /// /// This is a newtype for the gravity vector to ensure that it is not mixed up /// with other vectors and to provide some convenience methods. +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct Gravity(pub Vector3); impl Gravity { diff --git a/src/residuals/imu_preint/residual.rs b/src/residuals/imu_preint/residual.rs index d6d01f2..76f9729 100644 --- a/src/residuals/imu_preint/residual.rs +++ b/src/residuals/imu_preint/residual.rs @@ -90,7 +90,7 @@ impl ImuPreintegrator { Self { delta, // init with small value to avoid singular matrix - cov: Matrix::zeros() * 1e-14, + cov: Matrix::identity() * 1e-14, params, } } From d402e0708613a2ddc5b9871ce12fb0fe744f36c7 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Fri, 30 Aug 2024 16:56:04 -0400 Subject: [PATCH 08/13] Added tests for ImuDelta, new AccelUnbiased newtypes --- src/residuals/imu_preint/delta.rs | 177 +++++++++++++++++++++++---- src/residuals/imu_preint/mod.rs | 2 +- src/residuals/imu_preint/newtypes.rs | 34 ++++- src/residuals/imu_preint/residual.rs | 17 +-- src/variables/imu_bias.rs | 1 + src/variables/so3.rs | 1 + src/variables/traits.rs | 9 ++ 7 files changed, 201 insertions(+), 40 deletions(-) diff --git a/src/residuals/imu_preint/delta.rs b/src/residuals/imu_preint/delta.rs index e92bc7e..365c0f8 100644 --- a/src/residuals/imu_preint/delta.rs +++ b/src/residuals/imu_preint/delta.rs @@ -1,6 +1,6 @@ use core::fmt; -use super::{Accel, Gravity, Gyro, ImuCovariance, ImuState}; +use super::{AccelUnbiased, Gravity, GyroUnbiased, ImuCovariance, ImuState}; use crate::{ dtype, linalg::{Matrix, Matrix3, Numeric, Vector3}, @@ -76,43 +76,52 @@ impl ImuDelta { bias: b2_meas, } } -} - -// None of these methods should need to be dualized / "backpropagated" -// Can just assume default dtype -impl ImuDelta { - #[allow(non_snake_case)] - pub(crate) fn propagate_H(&mut self, A: &Matrix<15, 15>) { - let Amini = A.fixed_view::<9, 9>(0, 0); - // This should come from B, turns out it's identical as A - let Bgyro = A.fixed_view::<9, 3>(0, 9); - let Baccel = A.fixed_view::<9, 3>(0, 12); - self.h_bias_gyro = Amini * self.h_bias_gyro + Bgyro; - self.h_bias_accel = Amini * self.h_bias_accel + Baccel; - } + // TODO: Should this also propagate the H matrix? + // We'll have to construct A if we want to do that... #[allow(non_snake_case)] - pub(crate) fn integrate(&mut self, gyro: &Gyro, accel: &Accel, dt: dtype) { + pub(crate) fn integrate(&mut self, gyro: &GyroUnbiased, accel: &AccelUnbiased, dt: D) { self.dt += dt; let accel_world = SO3::exp(self.xi_theta.as_view()).apply(accel.0.as_view()); let H = SO3::dexp(self.xi_theta.as_view()); let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); - self.xi_theta += Hinv * gyro.0 * dt; - self.xi_vel += accel_world * dt; + // Integrate (make sure integration occurs in the correct order) self.xi_pos += self.xi_vel * dt + accel_world * (dt * dt * 0.5); + self.xi_vel += accel_world * dt; + self.xi_theta += Hinv * gyro.0 * dt; } #[allow(non_snake_case)] - pub(crate) fn A(&self, gyro: &Gyro, accel: &Accel, dt: dtype) -> Matrix<15, 15> { + pub(crate) fn propagate_H(&mut self, A: &Matrix<15, 15, D>) { + let Amini = A.fixed_view::<9, 9>(0, 0); + // This should come from B, turns out it's identical as A + let Bgyro = A.fixed_view::<9, 3>(0, 9); + let Baccel = A.fixed_view::<9, 3>(0, 12); + self.h_bias_gyro = Amini * self.h_bias_gyro + Bgyro; + self.h_bias_accel = Amini * self.h_bias_accel + Baccel; + } + + #[allow(non_snake_case)] + pub(crate) fn A( + &self, + gyro: &GyroUnbiased, + accel: &AccelUnbiased, + dt: D, + ) -> Matrix<15, 15, D> { let H = SO3::dexp(self.xi_theta.as_view()); let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); - let R = SO3::exp(self.xi_theta.as_view()).to_matrix(); + let R: nalgebra::Matrix< + D, + nalgebra::Const<3>, + nalgebra::Const<3>, + nalgebra::ArrayStorage, + > = SO3::exp(self.xi_theta.as_view()).to_matrix(); - let mut A = Matrix::<15, 15>::identity(); + let mut A = Matrix::<15, 15, D>::identity(); // First column (wrt theta) - let M = Matrix3::identity() - SO3::hat(gyro.0.as_view()) * dt / 2.0; + let M = Matrix3::::identity() - SO3::hat(gyro.0.as_view()) * dt / D::from(2.0); A.fixed_view_mut::<3, 3>(0, 0).copy_from(&M); let mut M = -R * SO3::hat(accel.0.as_view()) * H * dt; A.fixed_view_mut::<3, 3>(3, 0).copy_from(&M); @@ -120,8 +129,8 @@ impl ImuDelta { A.fixed_view_mut::<3, 3>(6, 0).copy_from(&M); // Second column (wrt vel) - let M = Matrix3::identity() * dt; - A.fixed_view_mut::<3, 3>(0, 6).copy_from(&M); + let M = Matrix3::::identity() * dt; + A.fixed_view_mut::<3, 3>(6, 3).copy_from(&M); // Third column (wrt pos) @@ -131,13 +140,16 @@ impl ImuDelta { // Fifth column (wrt accel bias) let mut M = -R * dt; - A.fixed_view_mut::<3, 3>(6, 9).copy_from(&M); + A.fixed_view_mut::<3, 3>(3, 12).copy_from(&M); M *= dt / 2.0; - A.fixed_view_mut::<3, 3>(3, 9).copy_from(&M); + A.fixed_view_mut::<3, 3>(6, 12).copy_from(&M); A } +} +// This functions will never need to be backpropped through +impl ImuDelta { #[allow(non_snake_case)] pub(crate) fn B_Q_BT(&self, p: &ImuCovariance, dt: dtype) -> Matrix<15, 15> { let H = SO3::dexp(self.xi_theta.as_view()); @@ -215,3 +227,116 @@ impl fmt::Display for ImuDelta { ) } } + +#[cfg(test)] +mod test { + use matrixcompare::{assert_matrix_eq, assert_scalar_eq}; + use nalgebra::Const; + + use super::*; + use crate::{ + linalg::{DualVector, ForwardProp, VectorX}, + residuals::{Accel, Gyro}, + traits::*, + }; + + // Helper function to integrate a constant gyro / accel for a given amount of + // time + fn integrate( + gyro: &Gyro, + accel: &Accel, + bias: &ImuBias, + n: i32, + t: dtype, + ) -> ImuDelta { + let mut delta = ImuDelta::new(Gravity::up(), bias.clone()); + // Remove the bias + let accel = accel.remove_bias(bias); + let gyro = gyro.remove_bias(bias); + let dt = D::from(t / n as dtype); + + for _ in 0..n { + // propagate H + let a = delta.A(&gyro, &accel, dt); + delta.propagate_H(&a); + // Integrate values + delta.integrate(&gyro, &accel, dt); + } + + delta + } + + // Test contast acceleration + #[test] + fn integrate_accel() { + let a = 5.0; + let t = 3.0; + let n = 100; + + let accel = Accel(Vector3::new(a, 0.0, 0.0)); + let gyro = Gyro(Vector3::zeros()); + + let delta = integrate(&gyro, &accel, &ImuBias::identity(), n, t); + + println!("Delta: {}", delta); + assert_scalar_eq!(delta.dt, t, comp = abs, tol = 1e-5); + assert_matrix_eq!(delta.xi_vel, accel.0 * t, comp = abs, tol = 1e-5); + assert_matrix_eq!(delta.xi_pos, accel.0 * t * t / 2.0, comp = abs, tol = 1e-5); + } + + // Test constant angular velocity + #[test] + fn integrate_gyro() { + let a = 5.0; + let t = 3.0; + let n = 100; + + let accel = Accel(Vector3::zeros()); + let gyro = Gyro(Vector3::new(0.0, 0.0, a)); + + let delta = integrate(&gyro, &accel, &ImuBias::identity(), n, t); + + println!("Delta: {}", delta); + assert_scalar_eq!(delta.dt, t, comp = abs, tol = 1e-5); + assert_matrix_eq!(delta.xi_theta, gyro.0 * t, comp = abs, tol = 1e-5); + } + + #[test] + #[allow(non_snake_case)] + fn propagate_h_accel() { + let t = 1.0; + let n = 2; + let accel = Accel::new(1.0, 2.0, 3.0); + let gyro = Gyro::new(0.1, 0.2, 0.3); + let bias = ImuBias::new(Gyro::new(1.0, 2.0, 3.0), Accel::new(0.1, 0.2, 0.3)); + + // Compute the H matrix + let delta = integrate(&gyro, &accel, &bias, n, t); + let H_accel_got = delta.h_bias_accel; + let H_gyro_got = delta.h_bias_gyro; + + // Compute the H matrix via forward prop + let integrate_diff = |bias: ImuBias>>| { + let accel = Accel(accel.0.map(|a| a.into())); + let gyro = Gyro(gyro.0.map(|g| g.into())); + let delta = integrate(&gyro, &accel, &bias, n, t); + let mut preint = VectorX::zeros(9); + preint.fixed_rows_mut::<3>(0).copy_from(&delta.xi_theta); + preint.fixed_rows_mut::<3>(3).copy_from(&delta.xi_vel); + preint.fixed_rows_mut::<3>(6).copy_from(&delta.xi_pos); + preint + }; + let H_exp = ForwardProp::>::jacobian_1(integrate_diff, &bias).diff; + let H_gyro_exp = H_exp.fixed_view::<9, 3>(0, 0); + let H_accel_exp = H_exp.fixed_view::<9, 3>(0, 3); + + println!("H_accel_got: {:.4}", H_accel_got); + println!("H_accel_exp: {:.4}", H_accel_exp); + assert_matrix_eq!(H_accel_got, H_accel_exp, comp = abs, tol = 1e-5); + + // TODO: Gyro still failing + println!("H_gyro_got: {:.4}", H_gyro_got); + println!("H_gyro_exp: {:.4}", H_gyro_exp); + assert_matrix_eq!(H_gyro_got, H_gyro_exp, comp = abs, tol = 1e-5); + } +} diff --git a/src/residuals/imu_preint/mod.rs b/src/residuals/imu_preint/mod.rs index cb9400f..c0c635b 100644 --- a/src/residuals/imu_preint/mod.rs +++ b/src/residuals/imu_preint/mod.rs @@ -1,5 +1,5 @@ mod newtypes; -pub use newtypes::{Accel, Gravity, Gyro, ImuState}; +pub use newtypes::{Accel, AccelUnbiased, Gravity, Gyro, GyroUnbiased, ImuState}; mod delta; diff --git a/src/residuals/imu_preint/newtypes.rs b/src/residuals/imu_preint/newtypes.rs index 281a01c..a12c5cb 100644 --- a/src/residuals/imu_preint/newtypes.rs +++ b/src/residuals/imu_preint/newtypes.rs @@ -4,7 +4,7 @@ use crate::{ variables::{ImuBias, SO3}, }; -/// Gyro measurement +/// Raw gyro measurement /// /// This is a newtype for the gyro measurement ensure that the accel and gyro /// aren't mixed up. @@ -13,12 +13,23 @@ use crate::{ pub struct Gyro(pub Vector3); impl Gyro { - pub fn remove_bias(&self, bias: &ImuBias) -> Gyro { - Gyro(self.0 - bias.gyro()) + pub fn remove_bias(&self, bias: &ImuBias) -> GyroUnbiased { + GyroUnbiased(self.0 - bias.gyro()) + } + + pub fn zeros() -> Self { + Gyro(Vector3::zeros()) + } + + pub fn new(x: D, y: D, z: D) -> Self { + Gyro(Vector3::new(x, y, z)) } } -/// Accel measurement newtype +/// Gyro measurement with bias removed +pub struct GyroUnbiased(pub Vector3); + +/// Raw accel measurement newtype /// /// This is a newtype for the accel measurement ensure that the accel and gyro /// aren't mixed up. @@ -27,11 +38,22 @@ impl Gyro { pub struct Accel(pub Vector3); impl Accel { - pub fn remove_bias(&self, bias: &ImuBias) -> Accel { - Accel(self.0 - bias.accel()) + pub fn remove_bias(&self, bias: &ImuBias) -> AccelUnbiased { + AccelUnbiased(self.0 - bias.accel()) + } + + pub fn zeros() -> Self { + Accel(Vector3::zeros()) + } + + pub fn new(x: D, y: D, z: D) -> Self { + Accel(Vector3::new(x, y, z)) } } +/// Accel measurement with bias removed +pub struct AccelUnbiased(pub Vector3); + /// Gravity vector /// /// This is a newtype for the gravity vector to ensure that it is not mixed up diff --git a/src/residuals/imu_preint/residual.rs b/src/residuals/imu_preint/residual.rs index 76f9729..7332d29 100644 --- a/src/residuals/imu_preint/residual.rs +++ b/src/residuals/imu_preint/residual.rs @@ -27,8 +27,7 @@ pub struct ImuCovariance { pub cov_ainit: Matrix3, } -/// Implements reasonable parameters for ImuCovariance including positive -/// gravity +/// Implements reasonable parameters for ImuCovariance impl Default for ImuCovariance { fn default() -> Self { Self { @@ -137,9 +136,10 @@ impl ImuPreintegrator { B2: TypedSymbol, { // Create noise from our covariance matrix - // TODO: Does covariance need to be modified as preint will be modified? let noise = GaussianNoise::from_matrix_cov(self.cov.as_view()); + // Create the residual let res = ImuPreintegrationResidual { delta: self.delta }; + // Build the factor FactorBuilder::new6(res, x1, v1, b1, x2, v2, b2) .noise(noise) .build() @@ -197,10 +197,13 @@ impl Residual6 for ImuPreintegrationResidual { let v2_meas: VectorVar3 = v2_meas.into(); // Compute residuals - let r_r = r2_meas.ominus(x2.rot()); - let r_vel = v2_meas.ominus(&v2); - let r_p = p2_meas.ominus(&p_2); - let r_bias = b2_meas.ominus(&b2); + // Because of how the noise is integrated, + // we have to use the right version of ominus here + // This won't matter so much for the vector elements (right = left for vectors) + let r_r = r2_meas.ominus_right(x2.rot()); + let r_vel = v2_meas.ominus_right(&v2); + let r_p = p2_meas.ominus_right(&p_2); + let r_bias = b2_meas.ominus_right(&b2); let mut residual = VectorX::zeros(15); residual.fixed_rows_mut::<3>(0).copy_from(&r_r); diff --git a/src/variables/imu_bias.rs b/src/variables/imu_bias.rs index 6a53134..b229535 100644 --- a/src/variables/imu_bias.rs +++ b/src/variables/imu_bias.rs @@ -12,6 +12,7 @@ use crate::{ tag_variable!(ImuBias); +// TODO: Use newtypes internally as well? /// IMU bias /// /// The IMU bias is a 6D vector containing the gyro and accel biases. It is diff --git a/src/variables/so3.rs b/src/variables/so3.rs index 68a4151..728fb37 100644 --- a/src/variables/so3.rs +++ b/src/variables/so3.rs @@ -68,6 +68,7 @@ impl SO3 { self.xyzw[3] } + // TODO: This needs significant testing pub fn dexp(xi: VectorView3) -> Matrix3 { let theta2 = xi.norm_squared(); diff --git a/src/variables/traits.rs b/src/variables/traits.rs index f388cc5..c9793dd 100644 --- a/src/variables/traits.rs +++ b/src/variables/traits.rs @@ -89,6 +89,15 @@ pub trait Variable: Clone + Sized + Display + Debug { y.inverse().compose(self).log() } } + + fn ominus_right(&self, y: &Self) -> VectorX { + y.inverse().compose(self).log() + } + + fn ominus_left(&self, y: &Self) -> VectorX { + self.compose(&y.inverse()).log() + } + /// Subtract out portion from other variable. /// /// This can be seen as a "tip-to-tail" computation. IE it computes the From a901c4d4c5bae546ecf9b8c82900c72154998f31 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Mon, 2 Sep 2024 15:27:30 -0400 Subject: [PATCH 09/13] Added make_a test, fixed H_prop test --- src/residuals/imu_preint/delta.rs | 96 +++++++++++++++++++++++++++++-- src/variables/so3.rs | 14 ++++- 2 files changed, 104 insertions(+), 6 deletions(-) diff --git a/src/residuals/imu_preint/delta.rs b/src/residuals/imu_preint/delta.rs index 365c0f8..614c41b 100644 --- a/src/residuals/imu_preint/delta.rs +++ b/src/residuals/imu_preint/delta.rs @@ -102,6 +102,8 @@ impl ImuDelta { self.h_bias_accel = Amini * self.h_bias_accel + Baccel; } + // TODO(Easton): Should this just be auto-diffed? Need to benchmark to see if + // that's faster #[allow(non_snake_case)] pub(crate) fn A( &self, @@ -235,9 +237,10 @@ mod test { use super::*; use crate::{ - linalg::{DualVector, ForwardProp, VectorX}, + linalg::{DualVector, ForwardProp, Vector, VectorX}, residuals::{Accel, Gyro}, traits::*, + variables::VectorVar, }; // Helper function to integrate a constant gyro / accel for a given amount of @@ -301,9 +304,89 @@ mod test { assert_matrix_eq!(delta.xi_theta, gyro.0 * t, comp = abs, tol = 1e-5); } + #[test] + fn make_a() { + let dt = 0.1; + let v: nalgebra::Matrix, Const<1>, nalgebra::ArrayStorage> = + Vector::<15>::from_fn(|i, _| i as dtype / 10.0); + let gyro = Gyro::new(3.0, 2.0, 1.0); + let accel: Accel = Accel::new(1.0, 2.0, 3.0); + + fn delta_from_vec(v: Vector<15, D>) -> ImuDelta { + let xi_theta = v.fixed_rows::<3>(0).into_owned(); + let xi_vel = v.fixed_rows::<3>(3).into_owned(); + let xi_pos = v.fixed_rows::<3>(6).into_owned(); + let bias_init = ImuBias::new( + Gyro(v.fixed_rows::<3>(9).into_owned()), + Accel(v.fixed_rows::<3>(12).into_owned()), + ); + ImuDelta { + dt: D::from(0.0), + xi_theta, + xi_vel, + xi_pos, + bias_init, + h_bias_accel: Matrix::zeros(), + h_bias_gyro: Matrix::zeros(), + gravity: Gravity::up(), + } + } + + let f = |v: VectorVar<15, DualVector>>| { + // construct measurements + let gyro = Gyro(gyro.0.map(|g| g.into())); + let accel = Accel(accel.0.map(|a| a.into())); + + // make delta from vector + let mut delta = delta_from_vec(v.0); + + // Integrate + let gyro = gyro.remove_bias(&delta.bias_init); + let accel = accel.remove_bias(&delta.bias_init); + delta.integrate(&gyro, &accel, DualVector::>::from(dt)); + + // Return the delta as a vector + let mut out = VectorX::zeros(15); + out.fixed_rows_mut::<3>(0).copy_from(&delta.xi_theta); + out.fixed_rows_mut::<3>(3).copy_from(&delta.xi_vel); + out.fixed_rows_mut::<3>(6).copy_from(&delta.xi_pos); + out.fixed_rows_mut::<3>(9).copy_from(delta.bias_init.gyro()); + out.fixed_rows_mut::<3>(12) + .copy_from(delta.bias_init.accel()); + + out + }; + + // Make expected A + let vv: VectorVar<15> = VectorVar::from(v); + let a_exp = ForwardProp::>::jacobian_1(f, &vv).diff; + + // Make got A + let delta = delta_from_vec(v); + let gyro = gyro.remove_bias(&delta.bias_init); + let accel = accel.remove_bias(&delta.bias_init); + let a_got = delta.A(&gyro, &accel, dt); + + println!("A_exp: {:.4}", a_exp); + println!("A_got: {:.4}", a_got); + // First column is an approximation, loosen the tolerance on those + assert_matrix_eq!( + a_exp.fixed_view::<12, 3>(3, 0), + a_got.fixed_view::<12, 3>(3, 0), + comp = abs, + tol = 1e-3 + ); + assert_matrix_eq!( + a_exp.fixed_view::<15, 12>(0, 3), + a_got.fixed_view::<15, 12>(0, 3), + comp = abs, + tol = 1e-5 + ); + } + #[test] #[allow(non_snake_case)] - fn propagate_h_accel() { + fn propagate_h() { let t = 1.0; let n = 2; let accel = Accel::new(1.0, 2.0, 3.0); @@ -334,9 +417,14 @@ mod test { println!("H_accel_exp: {:.4}", H_accel_exp); assert_matrix_eq!(H_accel_got, H_accel_exp, comp = abs, tol = 1e-5); - // TODO: Gyro still failing println!("H_gyro_got: {:.4}", H_gyro_got); println!("H_gyro_exp: {:.4}", H_gyro_exp); - assert_matrix_eq!(H_gyro_got, H_gyro_exp, comp = abs, tol = 1e-5); + // Skip top 3 rows, it's an approximation + assert_matrix_eq!( + H_gyro_got.fixed_view::<6, 3>(3, 0), + H_gyro_exp.fixed_view::<6, 3>(3, 0), + comp = abs, + tol = 1e-5 + ); } } diff --git a/src/variables/so3.rs b/src/variables/so3.rs index 728fb37..f718a8d 100644 --- a/src/variables/so3.rs +++ b/src/variables/so3.rs @@ -69,10 +69,11 @@ impl SO3 { } // TODO: This needs significant testing + // TODO: find reference for this pub fn dexp(xi: VectorView3) -> Matrix3 { let theta2 = xi.norm_squared(); - let (a, b) = if theta2 < D::from(1e-3) { + let (a, b) = if theta2 < D::from(1e-6) { (D::from(0.5), D::from(1.0) / D::from(6.0)) } else { let theta = theta2.sqrt(); @@ -82,7 +83,11 @@ impl SO3 { }; let hat = SO3::hat(xi); - Matrix3::identity() + hat * a + hat * hat * b + println!("xi: {}", xi); + println!("hat: {}", hat); + // TODO: gtsam says minus here for -hat a, but ethan eade says plus + // Everything in ImuDelta works with a minus + Matrix3::identity() - hat * a + hat * hat * b } } @@ -343,4 +348,9 @@ mod tests { test_variable!(SO3); test_lie!(SO3); + + // #[test] + // fn dexp() { + // fn exp() + // } } From dd08a3d372d8946c3046258182289a4845c8bc27 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Mon, 2 Sep 2024 15:56:34 -0400 Subject: [PATCH 10/13] Test SO3 dexp --- src/linalg/numerical_diff.rs | 80 ++++++++++++++++++++++++++++++++---- src/variables/so3.rs | 32 ++++++++++----- 2 files changed, 94 insertions(+), 18 deletions(-) diff --git a/src/linalg/numerical_diff.rs b/src/linalg/numerical_diff.rs index 46c485e..c21afc8 100644 --- a/src/linalg/numerical_diff.rs +++ b/src/linalg/numerical_diff.rs @@ -37,11 +37,6 @@ use crate::{ /// ``` pub struct NumericalDiff; -macro_rules! count { - () => (0usize); - ( $x:tt $($xs:tt)* ) => (1usize + count!($($xs)*)); -} - macro_rules! numerical_maker { ($num:expr, $( ($idx:expr, $name:ident, $var:ident) ),*) => { paste! { @@ -53,7 +48,6 @@ macro_rules! numerical_maker { // Get Dimension let mut dim = 0; $(dim += $name.dim();)* - let num_vars = count!($( $name )*); let res = f($( $name.clone(), )*); @@ -61,7 +55,7 @@ macro_rules! numerical_maker { let mut jac: MatrixX = MatrixX::zeros(res.len(), dim); let mut tvs = [$( VectorX::zeros($name.dim()), )*]; - for i in 0..num_vars { + for i in 0..$num { let mut curr_dim = 0; for j in 0..tvs[i].len() { tvs[i][j] = eps; @@ -77,6 +71,8 @@ macro_rules! numerical_maker { let delta = (plus - minus) / (2.0 * eps); jac.columns_mut(curr_dim + j, 1).copy_from(&delta); + + tvs[i][j] = 0.0; } curr_dim += tvs[i].len(); } @@ -112,3 +108,73 @@ impl Diff for NumericalDiff { (5, v6, V6) ); } + +macro_rules! numerical_variable_maker { + ($num:expr, $( ($idx:expr, $name:ident, $var:ident) ),*) => { + paste! { + #[allow(unused_assignments)] + pub fn []<$( $var: Variable, )* VOut: Variable, F: Fn($($var,)*) -> VOut> + (f: F, $($name: &$var,)*) -> DiffResult { + let eps = dtype::powi(10.0, -PWR); + + // Get Dimension + let mut dim = 0; + $(dim += $name.dim();)* + + let res = f($( $name.clone(), )*); + + // Compute gradient + let mut jac: MatrixX = MatrixX::zeros(VOut::DIM, dim); + let mut tvs = [$( VectorX::zeros($name.dim()), )*]; + + for i in 0..$num { + let mut curr_dim = 0; + for j in 0..tvs[i].len() { + tvs[i][j] = eps; + // TODO: It'd be more efficient to not have to add tangent vectors to each variable + // However, I couldn't find a way to do this for a single vector without having to + // do a nested iteration of $name which isn't allowed + $(let [<$name _og>] = $name.oplus(tvs[$idx].as_view());)* + let plus = f($( [<$name _og>], )*); + + tvs[i][j] = -eps; + $(let [<$name _og>] = $name.oplus(tvs[$idx].as_view());)* + let minus = f($( [<$name _og>], )*); + + let delta = plus.ominus(&minus) / (2.0 * eps); + jac.columns_mut(curr_dim + j, 1).copy_from(&delta); + + tvs[i][j] = 0.0; + } + curr_dim += tvs[i].len(); + } + + DiffResult { value: res, diff: jac } + } + } + }; +} + +impl NumericalDiff { + numerical_variable_maker!(1, (0, v1, V1)); + numerical_variable_maker!(2, (0, v1, V1), (1, v2, V2)); + numerical_variable_maker!(3, (0, v1, V1), (1, v2, V2), (2, v3, V3)); + numerical_variable_maker!(4, (0, v1, V1), (1, v2, V2), (2, v3, V3), (3, v4, V4)); + numerical_variable_maker!( + 5, + (0, v1, V1), + (1, v2, V2), + (2, v3, V3), + (3, v4, V4), + (4, v5, V5) + ); + numerical_variable_maker!( + 6, + (0, v1, V1), + (1, v2, V2), + (2, v3, V3), + (3, v4, V4), + (4, v5, V5), + (5, v6, V6) + ); +} diff --git a/src/variables/so3.rs b/src/variables/so3.rs index f718a8d..aa0e2f0 100644 --- a/src/variables/so3.rs +++ b/src/variables/so3.rs @@ -68,8 +68,6 @@ impl SO3 { self.xyzw[3] } - // TODO: This needs significant testing - // TODO: find reference for this pub fn dexp(xi: VectorView3) -> Matrix3 { let theta2 = xi.norm_squared(); @@ -83,10 +81,9 @@ impl SO3 { }; let hat = SO3::hat(xi); - println!("xi: {}", xi); - println!("hat: {}", hat); - // TODO: gtsam says minus here for -hat a, but ethan eade says plus - // Everything in ImuDelta works with a minus + // gtsam says minus here for -hat a, but ethan eade says plus + // Empirically (via our test & jac in ImuDelta), minus is correct + // Need to find reference to confirm Matrix3::identity() - hat * a + hat * hat * b } } @@ -342,15 +339,28 @@ impl fmt::Debug for SO3 { #[cfg(test)] mod tests { + use matrixcompare::assert_matrix_eq; + use super::*; - use crate::{test_lie, test_variable}; + use crate::{linalg::NumericalDiff, test_lie, test_variable, variables::VectorVar3}; test_variable!(SO3); test_lie!(SO3); - // #[test] - // fn dexp() { - // fn exp() - // } + #[test] + fn dexp() { + let xi = Vector3::new(0.1, 0.2, 0.3); + let got = SO3::dexp(xi.as_view()); + + let exp = NumericalDiff::<6>::jacobian_variable_1( + |x: VectorVar3| SO3::exp(Vector3::from(x).as_view()), + &VectorVar3::from(xi), + ) + .diff; + + println!("got: {}", got); + println!("exp: {}", exp); + assert_matrix_eq!(got, exp, comp = abs, tol = 1e-6); + } } From 1c852d02d89d9d478082be99e8f0bcd13f88d558 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Mon, 2 Sep 2024 16:19:14 -0400 Subject: [PATCH 11/13] Reorganize integrate function a smidge --- src/residuals/imu_preint/delta.rs | 97 +++++++--------------------- src/residuals/imu_preint/residual.rs | 63 ++++++++++++++++-- src/variables/traits.rs | 16 +++-- 3 files changed, 91 insertions(+), 85 deletions(-) diff --git a/src/residuals/imu_preint/delta.rs b/src/residuals/imu_preint/delta.rs index 614c41b..bb3f2d0 100644 --- a/src/residuals/imu_preint/delta.rs +++ b/src/residuals/imu_preint/delta.rs @@ -1,6 +1,6 @@ use core::fmt; -use super::{AccelUnbiased, Gravity, GyroUnbiased, ImuCovariance, ImuState}; +use super::{AccelUnbiased, Gravity, GyroUnbiased, ImuState}; use crate::{ dtype, linalg::{Matrix, Matrix3, Numeric, Vector3}, @@ -12,10 +12,10 @@ use crate::{ #[derive(Clone, Debug)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub(crate) struct ImuDelta { - dt: D, - xi_theta: Vector3, - xi_vel: Vector3, - xi_pos: Vector3, + pub(crate) dt: D, + pub(crate) xi_theta: Vector3, + pub(crate) xi_vel: Vector3, + pub(crate) xi_pos: Vector3, bias_init: ImuBias, h_bias_accel: Matrix<9, 3, D>, h_bias_gyro: Matrix<9, 3, D>, @@ -36,7 +36,7 @@ impl ImuDelta { } } - pub(crate) fn bias_init(&self) -> &ImuBias { + pub fn bias_init(&self) -> &ImuBias { &self.bias_init } @@ -77,10 +77,17 @@ impl ImuDelta { } } - // TODO: Should this also propagate the H matrix? - // We'll have to construct A if we want to do that... #[allow(non_snake_case)] - pub(crate) fn integrate(&mut self, gyro: &GyroUnbiased, accel: &AccelUnbiased, dt: D) { + pub(crate) fn integrate( + &mut self, + gyro: &GyroUnbiased, + accel: &AccelUnbiased, + dt: D, + ) -> Matrix<15, 15, D> { + // Construct jacobian + let A = self.A(gyro, accel, dt); + + // Setup self.dt += dt; let accel_world = SO3::exp(self.xi_theta.as_view()).apply(accel.0.as_view()); let H = SO3::dexp(self.xi_theta.as_view()); @@ -90,10 +97,15 @@ impl ImuDelta { self.xi_pos += self.xi_vel * dt + accel_world * (dt * dt * 0.5); self.xi_vel += accel_world * dt; self.xi_theta += Hinv * gyro.0 * dt; + + // Propagate H + self.propagate_H(&A); + + A } #[allow(non_snake_case)] - pub(crate) fn propagate_H(&mut self, A: &Matrix<15, 15, D>) { + fn propagate_H(&mut self, A: &Matrix<15, 15, D>) { let Amini = A.fixed_view::<9, 9>(0, 0); // This should come from B, turns out it's identical as A let Bgyro = A.fixed_view::<9, 3>(0, 9); @@ -105,12 +117,7 @@ impl ImuDelta { // TODO(Easton): Should this just be auto-diffed? Need to benchmark to see if // that's faster #[allow(non_snake_case)] - pub(crate) fn A( - &self, - gyro: &GyroUnbiased, - accel: &AccelUnbiased, - dt: D, - ) -> Matrix<15, 15, D> { + fn A(&self, gyro: &GyroUnbiased, accel: &AccelUnbiased, dt: D) -> Matrix<15, 15, D> { let H = SO3::dexp(self.xi_theta.as_view()); let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); let R: nalgebra::Matrix< @@ -150,60 +157,6 @@ impl ImuDelta { } } -// This functions will never need to be backpropped through -impl ImuDelta { - #[allow(non_snake_case)] - pub(crate) fn B_Q_BT(&self, p: &ImuCovariance, dt: dtype) -> Matrix<15, 15> { - let H = SO3::dexp(self.xi_theta.as_view()); - let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); - let R = SO3::exp(self.xi_theta.as_view()).to_matrix(); - - // Construct all partials - let H_theta_w = Hinv * dt; - let H_theta_winit = -Hinv * self.dt; - - let H_v_a = R * dt; - let H_v_ainit = -R * self.dt; - - let H_p_a = H_v_a * dt / 2.0; - let H_p_int: Matrix3 = Matrix3::identity(); - let H_p_ainit = H_v_ainit * self.dt / 2.0; - - // Copy them into place - let mut B = Matrix::<15, 15>::zeros(); - // First column (wrt theta) - let M = H_theta_w * p.cov_gyro_bias * H_theta_w.transpose() - + H_theta_winit * p.cov_winit * H_theta_winit.transpose(); - B.fixed_view_mut::<3, 3>(0, 0).copy_from(&M); - - // Second column (wrt vel) - let M = H_v_a * p.cov_accel * H_v_a.transpose() - + H_v_ainit * p.cov_ainit * H_v_ainit.transpose(); - B.fixed_view_mut::<3, 3>(3, 3).copy_from(&M); - let M = H_p_a * p.cov_accel * H_v_a.transpose() - + H_p_ainit * p.cov_ainit * H_v_ainit.transpose(); - B.fixed_view_mut::<3, 3>(6, 3).copy_from(&M); - - // Third column (wrt pos) - let M = H_v_a * p.cov_accel * H_p_a.transpose() - + H_v_ainit * p.cov_ainit * H_p_ainit.transpose(); - B.fixed_view_mut::<3, 3>(3, 6).copy_from(&M); - let M = H_p_a * p.cov_accel * H_p_a.transpose() - + H_p_int * p.cov_integration * H_p_int.transpose() - + H_p_ainit * p.cov_ainit * H_p_ainit.transpose(); - B.fixed_view_mut::<3, 3>(6, 6).copy_from(&M); - - // Fourth column (wrt gyro bias) - B.fixed_view_mut::<3, 3>(9, 9).copy_from(&p.cov_gyro_bias); - - // Fifth column (wrt accel bias) - B.fixed_view_mut::<3, 3>(12, 12) - .copy_from(&p.cov_accel_bias); - - B - } -} - impl DualConvert for ImuDelta { type Alias = ImuDelta
; fn dual_convert(other: &Self::Alias) -> Self::Alias
{ @@ -259,10 +212,6 @@ mod test { let dt = D::from(t / n as dtype); for _ in 0..n { - // propagate H - let a = delta.A(&gyro, &accel, dt); - delta.propagate_H(&a); - // Integrate values delta.integrate(&gyro, &accel, dt); } diff --git a/src/residuals/imu_preint/residual.rs b/src/residuals/imu_preint/residual.rs index 7332d29..b5119e9 100644 --- a/src/residuals/imu_preint/residual.rs +++ b/src/residuals/imu_preint/residual.rs @@ -12,7 +12,7 @@ use crate::{ residuals::Residual6, tag_residual, traits::*, - variables::{ImuBias, VectorVar3, SE3}, + variables::{ImuBias, MatrixLieGroup, VectorVar3, SE3, SO3}, }; // ------------------------- Covariances ------------------------- // @@ -94,6 +94,59 @@ impl ImuPreintegrator { } } + // TODO: Test (make sure dts are all correct) + #[allow(non_snake_case)] + fn B_Q_BT(&self, dt: dtype) -> Matrix<15, 15> { + let p = &self.params; + let H = SO3::dexp(self.delta.xi_theta.as_view()); + let Hinv = H.try_inverse().expect("Failed to invert H(theta)"); + let R = SO3::exp(self.delta.xi_theta.as_view()).to_matrix(); + + // Construct all partials + let H_theta_w = Hinv * dt; + let H_theta_winit = -Hinv * dt; + + let H_v_a = R * dt; + let H_v_ainit = -R * dt; + + let H_p_a = H_v_a * dt / 2.0; + let H_p_int: Matrix3 = Matrix3::identity(); + let H_p_ainit = H_v_ainit * dt / 2.0; + + // Copy them into place + let mut B = Matrix::<15, 15>::zeros(); + // First column (wrt theta) + let M = H_theta_w * p.cov_gyro_bias * H_theta_w.transpose() + + H_theta_winit * p.cov_winit * H_theta_winit.transpose(); + B.fixed_view_mut::<3, 3>(0, 0).copy_from(&M); + + // Second column (wrt vel) + let M = H_v_a * p.cov_accel * H_v_a.transpose() + + H_v_ainit * p.cov_ainit * H_v_ainit.transpose(); + B.fixed_view_mut::<3, 3>(3, 3).copy_from(&M); + let M = H_p_a * p.cov_accel * H_v_a.transpose() + + H_p_ainit * p.cov_ainit * H_v_ainit.transpose(); + B.fixed_view_mut::<3, 3>(6, 3).copy_from(&M); + + // Third column (wrt pos) + let M = H_v_a * p.cov_accel * H_p_a.transpose() + + H_v_ainit * p.cov_ainit * H_p_ainit.transpose(); + B.fixed_view_mut::<3, 3>(3, 6).copy_from(&M); + let M = H_p_a * p.cov_accel * H_p_a.transpose() + + H_p_int * p.cov_integration * H_p_int.transpose() + + H_p_ainit * p.cov_ainit * H_p_ainit.transpose(); + B.fixed_view_mut::<3, 3>(6, 6).copy_from(&M); + + // Fourth column (wrt gyro bias) + B.fixed_view_mut::<3, 3>(9, 9).copy_from(&p.cov_gyro_bias); + + // Fifth column (wrt accel bias) + B.fixed_view_mut::<3, 3>(12, 12) + .copy_from(&p.cov_accel_bias); + + B + } + #[allow(non_snake_case)] pub fn integrate(&mut self, gyro: Gyro, accel: Accel, dt: dtype) { // Remove bias estimate @@ -101,17 +154,13 @@ impl ImuPreintegrator { let accel = accel.remove_bias(self.delta.bias_init()); // Construct all matrices before integrating - let A = self.delta.A(&gyro, &accel, dt); - let B_Q_BT = self.delta.B_Q_BT(&self.params, dt); + let B_Q_BT = self.B_Q_BT(dt); // Update preintegration - self.delta.integrate(&gyro, &accel, dt); + let A = self.delta.integrate(&gyro, &accel, dt); // Update covariance self.cov = A * self.cov * A.transpose() + B_Q_BT; - - // Update H for bias updates - self.delta.propagate_H(&A); } /// Build a corresponding factor diff --git a/src/variables/traits.rs b/src/variables/traits.rs index c9793dd..e1d2afd 100644 --- a/src/variables/traits.rs +++ b/src/variables/traits.rs @@ -66,12 +66,20 @@ pub trait Variable: Clone + Sized + Display + Debug { /// $$ fn oplus(&self, xi: VectorViewX) -> Self { if cfg!(feature = "left") { - Self::exp(xi).compose(self) + self.oplus_left(xi) } else { - self.compose(&Self::exp(xi)) + self.oplus_right(xi) } } + fn oplus_right(&self, xi: VectorViewX) -> Self { + self.compose(&Self::exp(xi)) + } + + fn oplus_left(&self, xi: VectorViewX) -> Self { + Self::exp(xi).compose(self) + } + /// Compares two group elements in the tangent space /// /// By default this uses the "right" version as found in Micro Lie Theory @@ -84,9 +92,9 @@ pub trait Variable: Clone + Sized + Display + Debug { /// $$ fn ominus(&self, y: &Self) -> VectorX { if cfg!(feature = "left") { - self.compose(&y.inverse()).log() + self.ominus_left(y) } else { - y.inverse().compose(self).log() + self.ominus_right(y) } } From f0673a55d6ff37361449d064317ef5c2f4708021 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Tue, 3 Sep 2024 15:40:45 -0400 Subject: [PATCH 12/13] Finalized IMU documentation --- src/lib.rs | 2 +- src/residuals/imu_preint/README.md | 209 +++++++++++++++++++++++++++ src/residuals/imu_preint/mod.rs | 7 +- src/residuals/imu_preint/newtypes.rs | 4 + src/residuals/imu_preint/residual.rs | 97 ++++++++++++- src/residuals/mod.rs | 11 +- src/variables/imu_bias.rs | 8 + 7 files changed, 323 insertions(+), 15 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 4d83ee8..710fd44 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -100,7 +100,7 @@ pub mod variables; /// Untagged symbols if `unchecked` API is desired. /// -/// We strongly recommend using [assign_symbols](crate::assign_symbols) to +/// We strongly recommend using [assign_symbols] to /// create and tag symbols with the appropriate types. However, we provide a /// number of pre-defined symbols if desired. Note these objects can't be tagged /// due to the orphan rules. diff --git a/src/residuals/imu_preint/README.md b/src/residuals/imu_preint/README.md index e69de29..c04b9af 100644 --- a/src/residuals/imu_preint/README.md +++ b/src/residuals/imu_preint/README.md @@ -0,0 +1,209 @@ +Preintegration of IMU measurements. + +Inertial Measurement Unit (IMU) preintegration is a common technique used to summarize multiple IMU measurements into a single constraint used in preintegration. Our implementation is based on "local coordination integration", similar to the one [used in gtsam by default](https://github.com/borglab/gtsam/blob/4.2.0/doc/ImuFactor.pdf). +# Example +``` +use factrs::residuals::{ImuPreintegrator, Accel, Gravity, Gyro, ImuCovariance}; +use factrs::variables::{SE3, VectorVar3, ImuBias}; +use factrs::assign_symbols; + +assign_symbols!(X: SE3; V: VectorVar3; B: ImuBias); + +let mut preint = + ImuPreintegrator::new(ImuCovariance::default(), ImuBias::zeros(), Gravity::up()); + +let accel = Accel::new(0.1, 0.2, 9.81); +let gyro = Gyro::new(0.1, 0.2, 0.3); +let dt = 0.01; +// Integrate measurements for 100 steps +for _ in 0..100 { + preint.integrate(&gyro, &accel, dt); +} + +// Build the factor +let factor = preint.build(X(0), V(0), B(0), X(1), V(1), B(1)); +``` + +# Theory +
+Note, our convention throughout the library is to always put rotations first +in any list, vector, matrix ordering, etc. +
+ +Our state will consist of five components, specifically the rotation from body to the world frame $R$, body velocity in the world frame $v$, body position in the world frame $p$, +angular velocity bias $b^\omega$, and linear acceleration bias $b^a$. We'll +denote the vector $X$ as these components together in the order listed +previously. + +An IMU measures body angular velocity $\omega$ and body linear acceleration +$a$. This results in the continuous time differential equation + +$$\begin{aligned} +\dot{R} &= R (\omega - b^\omega) ^\wedge \\\\ +\dot{v} &= g + R (a - b^a) \\\\ +\dot{p} &= v \\\\ +\end{aligned}$$ + +with initial conditions given by $R_0, v_0, p_0, b^{\omega}_0, b^a_0$, or +$X_0$. + +The goal here is to find an integration that is *independent* of +these initial conditions. In other words, we want a solution $\Delta X$ that +allows us to perform $X_1 = X_0 \cdot \Delta X$ for some custom operation +$\cdot$. This results in an integration we can do beforehand, aka a +"preintegration". + +## Handling Gravity + +The first thing to note is there is a number of constants in the system that +can be removed to simplify things. For example, gravity in the velocity +differential equation, +$$ +\dot{v} \triangleq \dot{v}^g + \dot{v}^a = g + R(a - b^a) +$$ +$\dot{v}^g$ is a constant and can be integrated afterwards. Similarly, for +the position component, we have +$$ +\dot{p} \triangleq \dot{p}^0 + \dot{p}^g + \dot{p}^a = v_0 + v^g + v^a +$$ +where $\dot{p}_0$ and $\dot{p}^g$ are constants and can be integrated +afterwards. This brings our differential equations to +$$ \begin{aligned} +\dot{R} &= R (\omega - b^\omega) ^\wedge \\\\ +\dot{v}^a &= R(a - b^a) \\\\ +\dot{p}^a &= v^a \\\\ +\end{aligned} $$ + +## Local Coordinates + +Additionally, rather than integrate in the global frame, we can integrate in +a local frame that is aligned with the initial rotation $R_0$. We recommend +reviewing (CITE) for more information on this. This results in +the equations +$$ \begin{aligned} +\dot{\theta} &= H(\theta)^{-1} (\omega - b^\omega) \\\\ +\dot{v}^a &= \exp(\theta^\wedge) (a - b^a) \\\\ +\dot{p}^a &= v^a \\\\ +\end{aligned} $$ +where $\theta$ is the local rotation in the Lie algebra and $H(\theta)$ is +the Jacobian of the exponential map at $\theta$. + +## Discretization + +Moving this into discrete time using a simple Euler scheme results in, +$$ \begin{aligned} +\theta_{k+1} &= \theta_k + H(\theta_k)^{-1} (\omega_k - b^\omega_0) \Delta t \\\\ +v^a_{k+1} &= v^a_k + \exp(\theta_k^\wedge) (a_k - b^a_0) \Delta t \\\\ +p^a_{k+1} &= p^a_k + v^a_k \Delta t + \exp(\theta_k^\wedge) (a_k - b^a_0) \Delta t^2 / 2 \\\\ +\end{aligned} $$ +where we have assumed a constant bias over our timestep. We are now +independent of the initial conditions (except for bias which we'll cover +shortly). Additionally, the bias will have the simple discretized form, +$$ \begin{aligned} +b^\omega_{k+1} &= b^\omega_k \\\\ +b^a_{k+1} &= b^a_k \\\\ +\end{aligned} $$ + +## Covariance Propagation + +TODO: Include some reference to Kalibur github on noise model units + +We also propagate the covariance of the entire system. There is six noise +terms that we introduce to account for the various errors of the system, +each of which is a 3-vector with a 3x3 covariance matrix. These are +- $\epsilon_{\omega}$: the noise of the angular velocity measurement +- $\epsilon_{a}$: the noise of the linear acceleration measurement +- $\epsilon_{\omega^b}$: the noise of the angular velocity bias +- $\epsilon_{a^b}$: the noise of the linear acceleration bias +- $\epsilon_{int}$: the noise of the integration error +- $\epsilon_{init}$: the noise of the bias initialization +These will be stacked into a 18-vector with a block-diagonal 18x18 covariance +matrix $Q$. We introduce the noise as follows, +$$ \begin{aligned} +\theta_{k+1} &= \theta_k + H(\theta_k)^{-1} (\omega_k + \epsilon_\omega - b^\omega_0 - \epsilon_{init}) \Delta t \\\\ +v^a_{k+1} &= v^a_k + \exp(\theta_k^\wedge) (a_k + \epsilon_a - b^a_0 - \epsilon_{init}) \Delta t \\\\ +p^a_{k+1} &= p^a_k + v^a_k \Delta t + \exp(\theta_k^\wedge) (a_k + \epsilon_a - b^a_0 - \epsilon_{init}) \Delta t^2 / 2 + \epsilon_{int} \\\\ +b^\omega_{k+1} &= b^\omega_k + \epsilon_{\omega^b} \\\\ +b^a_{k+1} &= b^a_k + \epsilon_{a^b} \\\\ +\end{aligned} $$ + +We can then propagate a covariance matrix for our preintegrated values using the Jacobian of the system with respect to the noise terms. +$$ \begin{aligned} +A_k &\triangleq \frac{\partial f}{\partial X} = \begin{bmatrix} +I - \frac{\Delta t}{2} \tilde{\omega}_k & 0 & 0 & -H(\theta_k)^{-1}\Delta t & 0 \\\\ +-R_k \tilde{a}^\wedge H(\theta) \Delta t & I & 0 & 0 & -R_k \Delta t \\\\ +-R_k \tilde{a}^\wedge H(\theta) \Delta t^2 / 2 & I \Delta t & I & 0 & -R_k \Delta t^2 /2 \\\\ +0 & 0 & 0 & I & 0 \\\\ +0 & 0 & 0 & 0 & I \\\\ +\end{bmatrix} \\\\ +B_k &\triangleq \frac{\partial f}{\partial \epsilon} = \begin{bmatrix} +H(\theta_k)^{-1} \Delta t & 0 & 0 & 0 & 0 & -H(\theta)^{-1}\Delta t \\\\ +0 & R_k \Delta t & 0 & 0 & 0 & -R_k \Delta t \\\\ +0 & R_k \Delta t^2 / 2 & 0 & 0 & I & -R_k \Delta t^2 / 2 \\\\ +0 & 0 & I & 0 & 0 & 0 \\\\ +0 & 0 & 0 & I & 0 & 0 \\\\ +\end{bmatrix} \\\\ +\end{aligned} $$ + +where we have used +$$ \begin{aligned} +\tilde{\omega} &\triangleq \omega - b^\omega_0 \\\\ +\tilde{a} &\triangleq a - b^a_0 \\\\ +R_k &\triangleq \exp(\theta_k^\wedge) \\\\ +\frac{\partial H(\theta_k)^{-1} \tilde{\omega}}{\partial \theta} &\approx \frac{-1}{2}\tilde{\omega}^\wedge \\\\ +\frac{\partial R_k \tilde{a}}{\partial \theta} &\approx R_k \tilde{a}^\wedge H(\theta_k) \\\\ +\end{aligned} $$ + +Put all together, we have the covariance propagation equation +$$ +\Sigma_{k+1} = A_k \Sigma_k A_k^T + B_k Q B_k^T +$$ + +## First-order Bias Updates + +As mentioned previously, there still exists a dependence on the initial bias parameters. We approximate the impact of bias changes using a first order approximation. This is done using the Jacobian of the system with respect to the bias terms. We can construct these Jacobians iteratively as follows, letting $x = [\theta, v, p]^\top$, and linearizing about an initial bias $\tilde{b}^\omega_0$ + +$$ \begin{aligned} +H_{\omega}^{x_{k+1}} &\triangleq \frac{\partial \theta_{k+1}}{\partial b^\omega_0} = \frac{\partial \theta_{k+1}}{\partial \theta_k} \frac{\partial \theta_k}{\partial b^\omega_0} + \frac{\partial \theta_{k+1}}{\partial \omega_k} = \frac{\partial \theta_{k+1}}{\partial \theta_k} H_{\omega}^{x_{k}} + \frac{\partial \theta_{k+1}}{\partial \omega_k} +\end{aligned} $$ + +Where the acceleration bias update is similar. Fortunately, the Jacobians required here are already computed in the $A_k$ matrix defined previously and we can simply extract them as needed. + +## Custom Operator + +Returning to our custom operator that we mentioned earlier to accomplish $X_1 = X_0 \cdot \Delta X$, we can define what it will be, taking into account the first-order updates, local coordinates, and the gravity terms we removed earlier. + +First, we update our preintegration equations to include any changes due to bias updates, +$$ \begin{aligned} +\tilde{\theta} &= \theta + H_{\omega}^{\theta_{k+1}} (b^\omega_0 - \tilde{b}^\omega_0) + H_{a}^{\theta_{k+1}} (b^a_0 - \tilde{b}^a_0) \\\\ +\tilde{v}^a &= v^a + H_{\omega}^{v_{k+1}} (b^\omega_0 - \tilde{b}^\omega_0) + H_{a}^{v_{k+1}} (b^a_0 - \tilde{b}^a_0) \\\\ +\tilde{p}^a &= p^a + H_{\omega}^{p_{k+1}} (b^\omega_0 - \tilde{b}^\omega_0) + H_{a}^{p_{k+1}} (b^a_0 - \tilde{b}^a_0) \\\\ +\end{aligned} $$ + +Then, we can define our custom operator as follows, + +$$ \begin{aligned} +R_1 &= R_0 \exp(\tilde{\theta}^\wedge) \\\\ +v_1 &= R_0 \tilde{v}^a + v_0 + g \Delta t \\\\ +p_1 &= R_0 \tilde{p}^a + p_0 + v_0 \Delta t + g \Delta t^2 / 2 \\\\ +b_1^\omega &= b_0^\omega \\\\ +b_1^a &= b_0^a \\\\ +\end{aligned} $$ + +The covariance we propagated earlier will exist in the local frame (the right hand side) of these measurements. We thus can define our residual using a "right ominus" operator, +$$ \begin{aligned} +X_1 = X_0 \cdot (\Delta X \cdot \epsilon) \implies r = \epsilon = \log( X_1^{-1} (X_0 \cdot \Delta X) ) +\end{aligned} $$ + + +## Implementation Details + +[ImuPreintegrator] handles the integration of $\Delta X$ and the covariance propagation. It'll be the main entrypoint for doing Imu preintegration. It can produce a factor that contains the proper residual and covariance for use in a factor graph. + +[ImuCovariance] holds the six different covariances utilized in the covariance propagation. It also contains a handful of helper functions for simplify setting covariance values. + +Internally (if you care to peruse the codebase), there is a number of structures that follow naturally from the theory. For example, +- `ImuDelta` represents $\Delta X$ and contains $g, \Delta t, \theta, v^a, p^a, \tilde{b}^\omega, \tilde{b}^a$. +- `ImuState` represents $X$ and contains $R, v, p, b^\omega, b^a$. +- `ImuBias` represents the bias terms and contains $b^\omega, b^a$. +- `ImuPreintegrationResidual` is used to calculate the final residual. \ No newline at end of file diff --git a/src/residuals/imu_preint/mod.rs b/src/residuals/imu_preint/mod.rs index c0c635b..e6045f5 100644 --- a/src/residuals/imu_preint/mod.rs +++ b/src/residuals/imu_preint/mod.rs @@ -1,7 +1,10 @@ +#![doc = include_str!("README.md")] + mod newtypes; -pub use newtypes::{Accel, AccelUnbiased, Gravity, Gyro, GyroUnbiased, ImuState}; +pub(crate) use newtypes::ImuState; +pub use newtypes::{Accel, AccelUnbiased, Gravity, Gyro, GyroUnbiased}; mod delta; mod residual; -pub use residual::{ImuCovariance, ImuPreintegrationResidual, ImuPreintegrator}; +pub use residual::{ImuCovariance, ImuPreintegrator}; diff --git a/src/residuals/imu_preint/newtypes.rs b/src/residuals/imu_preint/newtypes.rs index a12c5cb..8972210 100644 --- a/src/residuals/imu_preint/newtypes.rs +++ b/src/residuals/imu_preint/newtypes.rs @@ -13,6 +13,7 @@ use crate::{ pub struct Gyro(pub Vector3); impl Gyro { + /// Remove the bias from the gyro measurement pub fn remove_bias(&self, bias: &ImuBias) -> GyroUnbiased { GyroUnbiased(self.0 - bias.gyro()) } @@ -38,6 +39,7 @@ pub struct GyroUnbiased(pub Vector3); pub struct Accel(pub Vector3); impl Accel { + /// Remove the bias from the accel measurement pub fn remove_bias(&self, bias: &ImuBias) -> AccelUnbiased { AccelUnbiased(self.0 - bias.accel()) } @@ -63,10 +65,12 @@ pub struct AccelUnbiased(pub Vector3); pub struct Gravity(pub Vector3); impl Gravity { + /// Helper to get the gravity vector pointing up, i.e. [0, 0, 9.81] pub fn up() -> Self { Gravity(Vector3::new(D::from(0.0), D::from(0.0), D::from(9.81))) } + /// Helper to get the gravity vector pointing down, i.e. [0, 0, -9.81] pub fn down() -> Self { Gravity(Vector3::new(D::from(0.0), D::from(0.0), D::from(-9.81))) } diff --git a/src/residuals/imu_preint/residual.rs b/src/residuals/imu_preint/residual.rs index b5119e9..88d7302 100644 --- a/src/residuals/imu_preint/residual.rs +++ b/src/residuals/imu_preint/residual.rs @@ -4,7 +4,7 @@ use nalgebra::Const; use super::{delta::ImuDelta, Accel, Gravity, Gyro, ImuState}; use crate::{ - containers::{Factor, FactorBuilder, TypedSymbol}, + containers::{Factor, FactorBuilder, Symbol, TypedSymbol}, dtype, impl_residual, linalg::{ForwardProp, Matrix, Matrix3, VectorX}, @@ -16,6 +16,13 @@ use crate::{ }; // ------------------------- Covariances ------------------------- // +/// Covariance parameters for the IMU preintegration +/// +/// Trys to come with semi-resonable defaults for the covariance parameters +/// ``` +/// use factrs::residuals::ImuCovariance; +/// let cov = ImuCovariance::default(); +/// ``` #[derive(Clone, Debug)] pub struct ImuCovariance { pub cov_accel: Matrix3, @@ -43,22 +50,27 @@ impl Default for ImuCovariance { } impl ImuCovariance { + /// Set $\epsilon_a$ covariance to a diagonal pub fn set_scalar_accel(&mut self, val: dtype) { self.cov_accel = Matrix3::identity() * val; } + /// Set $\epsilon_\omega$ covariance to a diagonal pub fn set_scalar_gyro(&mut self, val: dtype) { self.cov_gyro = Matrix3::identity() * val; } + /// Set $\epsilon_{a^b}$ covariance to a diagonal pub fn set_scalar_accel_bias(&mut self, val: dtype) { self.cov_accel_bias = Matrix3::identity() * val; } + /// Set $\epsilon_{\omega^b}$ covariance to a diagonal pub fn set_scalar_gyro_bias(&mut self, val: dtype) { self.cov_gyro_bias = Matrix3::identity() * val; } + /// Set $\epsilon_{int}$ covariance to a diagonal pub fn set_scalar_integration(&mut self, val: dtype) { self.cov_integration = Matrix3::identity() * val; } @@ -66,6 +78,7 @@ impl ImuCovariance { // In practice, I think everyone makes cov_winit = cov_ainit // For now, the public interface assumes they are the same, but behind the // scenes we're using both + /// Set $\epsilon_{init}$ covariance to a diagonal pub fn set_scalar_init(&mut self, val: dtype) { self.cov_winit = Matrix3::identity() * val; self.cov_ainit = Matrix3::identity() * val; @@ -74,6 +87,31 @@ impl ImuCovariance { // ------------------------- The Preintegrator ------------------------- // /// Performs Imu preintegration +/// +/// This is the main entrypoint for the IMU preintegration functionality. See +/// [IMU Preintegration module](crate::residuals::imu_preint) for more details +/// on the theory. +/// ``` +/// use factrs::residuals::{ImuPreintegrator, Accel, Gravity, Gyro, ImuCovariance}; +/// use factrs::variables::{SE3, VectorVar3, ImuBias}; +/// use factrs::assign_symbols; +/// +/// assign_symbols!(X: SE3; V: VectorVar3; B: ImuBias); +/// +/// let mut preint = +/// ImuPreintegrator::new(ImuCovariance::default(), ImuBias::zeros(), Gravity::up()); +/// +/// let accel = Accel::new(0.1, 0.2, 9.81); +/// let gyro = Gyro::new(0.1, 0.2, 0.3); +/// let dt = 0.01; +/// // Integrate measurements for 100 steps +/// for _ in 0..100 { +/// preint.integrate(&gyro, &accel, dt); +/// } +/// +/// // Build the factor +/// let factor = preint.build(X(0), V(0), B(0), X(1), V(1), B(1)); +/// ``` #[derive(Clone, Debug)] pub struct ImuPreintegrator { // Mutable state that will change as we integrate @@ -84,6 +122,9 @@ pub struct ImuPreintegrator { } impl ImuPreintegrator { + /// Construct a new ImuPreintegrator + /// + /// Requires the covariance parameters, initial bias, and gravity vector pub fn new(params: ImuCovariance, bias_init: ImuBias, gravity: Gravity) -> Self { let delta = ImuDelta::new(gravity, bias_init); Self { @@ -147,8 +188,17 @@ impl ImuPreintegrator { B } + /// Integrate a single IMU measurement + /// ``` + /// # use factrs::residuals::imu_preint::*; + /// # use factrs::variables::ImuBias; + /// # let mut preint = ImuPreintegrator::new(ImuCovariance::default(), ImuBias::zeros(), Gravity::up()); + /// let gyro = Gyro::new(0.1, 0.2, 0.3); + /// let accel = Accel::new(0.1, 0.2, 0.3); + /// preint.integrate(&gyro, &accel, 0.01); + /// ``` #[allow(non_snake_case)] - pub fn integrate(&mut self, gyro: Gyro, accel: Accel, dt: dtype) { + pub fn integrate(&mut self, gyro: &Gyro, accel: &Accel, dt: dtype) { // Remove bias estimate let gyro = gyro.remove_bias(self.delta.bias_init()); let accel = accel.remove_bias(self.delta.bias_init()); @@ -166,7 +216,17 @@ impl ImuPreintegrator { /// Build a corresponding factor /// /// This consumes the preintegrator and returns a - /// [factor](crate::factor::Factor) with the proper noise model. + /// [factor](crate::containers::Factor) with the proper noise model. + /// Requires properly typed symbols, likely created via + /// [assign_symbols](crate::assign_symbols). + /// ``` + /// # use factrs::residuals::imu_preint::*; + /// # use factrs::variables::*; + /// # use factrs::assign_symbols; + /// # assign_symbols!(X: SE3; V: VectorVar3; B: ImuBias); + /// # let preint = ImuPreintegrator::new(ImuCovariance::default(), ImuBias::zeros(), Gravity::up()); + /// let factor = preint.build(X(0), V(0), B(0), X(1), V(1), B(1)); + /// ``` pub fn build( self, x1: X1, @@ -193,6 +253,37 @@ impl ImuPreintegrator { .noise(noise) .build() } + + /// Build a corresponding factor, with unchecked symbols + /// + /// Same as [build](ImuPreintegrator::build), but without the symbol type + /// checking + pub fn build_unchecked( + self, + x1: X1, + v1: V1, + b1: B1, + x2: X2, + v2: V2, + b2: B2, + ) -> Factor + where + X1: Symbol, + V1: Symbol, + B1: Symbol, + X2: Symbol, + V2: Symbol, + B2: Symbol, + { + // Create noise from our covariance matrix + let noise = GaussianNoise::from_matrix_cov(self.cov.as_view()); + // Create the residual + let res = ImuPreintegrationResidual { delta: self.delta }; + // Build the factor + FactorBuilder::new6_unchecked(res, x1, v1, b1, x2, v2, b2) + .noise(noise) + .build() + } } // ------------------------- The Residual ------------------------- // diff --git a/src/residuals/mod.rs b/src/residuals/mod.rs index 3e95313..9d91a89 100644 --- a/src/residuals/mod.rs +++ b/src/residuals/mod.rs @@ -22,14 +22,7 @@ pub use prior::PriorResidual; mod between; pub use between::BetweenResidual; -mod imu_preint; -pub use imu_preint::{ - Accel, - Gravity, - Gyro, - ImuCovariance, - ImuPreintegrationResidual, - ImuPreintegrator, -}; +pub mod imu_preint; +pub use imu_preint::{Accel, Gravity, Gyro, ImuCovariance, ImuPreintegrator}; mod macros; diff --git a/src/variables/imu_bias.rs b/src/variables/imu_bias.rs index b229535..432c24c 100644 --- a/src/variables/imu_bias.rs +++ b/src/variables/imu_bias.rs @@ -33,6 +33,14 @@ impl ImuBias { } } + /// Create an IMU bias of zeros (same as identity) + pub fn zeros() -> Self { + ImuBias { + gyro: Vector3::zeros(), + accel: Vector3::zeros(), + } + } + /// Get the gyro bias pub fn gyro(&self) -> &Vector3 { &self.gyro From 5cb09748f93f7d9bd7d7c8db3c18aba6816a1bfc Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Mon, 9 Sep 2024 09:52:03 -0400 Subject: [PATCH 13/13] Update rerun version --- Cargo.lock | 244 +++++++++++++++++++----------------- Cargo.toml | 2 +- src/linalg/nalgebra_wrap.rs | 3 +- src/rerun.rs | 3 +- src/variables/traits.rs | 2 +- 5 files changed, 133 insertions(+), 121 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index d5d1e80..9b9c0a3 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -249,7 +249,7 @@ checksum = "3b43422f69d8ff38f95f1b2bb76517c91589a924d1559a0e935d7c8ce0274c11" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -284,7 +284,7 @@ checksum = "c6fa2087f2753a7da8cc1c0dbfcf89579dd57458e36769de5ac750b4671737ca" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -420,7 +420,7 @@ checksum = "1ee891b04274a59bd38b412188e24b849617b2e45a0fd8d057deb63e7403761b" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -720,7 +720,7 @@ dependencies = [ "proc-macro2", "quote", "rustc_version", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -797,7 +797,7 @@ dependencies = [ "heck 0.4.1", "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -818,7 +818,7 @@ checksum = "de0d48a183585823424a4ce1aa132d174a6a81bd540895822eb4c8373a8e49e8" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -863,7 +863,7 @@ checksum = "3bf679796c0322556351f287a51b49e48f7c4986e727b5dd78c972d30e2e16cc" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -1135,7 +1135,7 @@ checksum = "87750cf4b7a4c0625b1529e4c543c2182106e4dedc60a2a6455e00d212c489ac" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -1473,6 +1473,15 @@ dependencies = [ "windows-sys 0.52.0", ] +[[package]] +name = "itertools" +version = "0.10.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b0fd2260e829bddf4cb6ea802289de2f86d6a7a690192fbe91b3f46e0f2c8473" +dependencies = [ + "either", +] + [[package]] name = "itertools" version = "0.13.0" @@ -1703,7 +1712,7 @@ checksum = "254a5372af8fc138e36684761d3c0cdb758a4410e938babcff1c860ce14ddbfc" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -1847,7 +1856,7 @@ checksum = "ed3955f1a9c7c0c15e092f9c887db08b1fc683305fdf6eb6684f22555355e202" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -2132,7 +2141,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5f12335488a2f3b0a83b14edad48dca9879ce89b2edd10e80237e4e852dd645e" dependencies = [ "proc-macro2", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -2156,14 +2165,15 @@ dependencies = [ [[package]] name = "puffin" -version = "0.19.0" +version = "0.19.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b9f76ad4bb049fded4e572df72cbb6381ff5d1f41f85c3a04b56e4eca287a02f" +checksum = "fa9dae7b05c02ec1a6bc9bcf20d8bc64a7dcbf57934107902a872014899b741f" dependencies = [ "anyhow", "bincode", "byteorder", "cfg-if", + "itertools 0.10.5", "lz4_flex", "once_cell", "parking_lot", @@ -2214,9 +2224,9 @@ checksum = "a1d01941d82fa2ab50be1e79e6714289dd7cde78eba4c074bc5a4374f650dfe0" [[package]] name = "quote" -version = "1.0.36" +version = "1.0.37" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0fa76aaf39101c457836aec0ce2316dbdc3ab723cdda1c6bd4e6ad4208acaca7" +checksum = "b5b9d34b8991d19d98081b46eacdd8eb58c6f2b201139f7c5f643cc155a633af" dependencies = [ "proc-macro2", ] @@ -2317,18 +2327,18 @@ dependencies = [ [[package]] name = "re_build_info" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "958b9f9310bdc194578aa851fa1fdd06b9b74dcd4da2a05acfd76e71fb6440ca" +checksum = "bb89ca2a069a90b152610c5d4771b7a831ed9d15b054548a08dfe7b41710d53c" dependencies = [ "serde", ] [[package]] name = "re_build_tools" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0a5c00c90429b32d6c510eadaa8641a42179a1eab156e25c8fba6d662b83f9e0" +checksum = "1b66fc95fed87eba938856c49d029b2525d1d90a1fd8f45c419fd75f57e09afc" dependencies = [ "anyhow", "cargo_metadata 0.18.1", @@ -2341,96 +2351,93 @@ dependencies = [ [[package]] name = "re_case" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "afecb88ab9e8a1544b9a0b5b7e9f5d997696624856274822ef9fa3dafa044485" +checksum = "752ce7308c890cd44d7c4072ef5fb9ec1131a78535ff2cc8058cf93120bcfee2" dependencies = [ "convert_case 0.6.0", ] [[package]] name = "re_chunk" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "71158f674fc6da5d5fea3e894dbbb885764c59ed0176281b9bc0f4c7da461e5d" +checksum = "e48a9e622debb4ebe8de81714b47d48c89eba7c38c57213b9d15fe0080ac1c67" dependencies = [ "ahash", "anyhow", - "backtrace", + "bytemuck", "crossbeam", "document-features", - "itertools", + "itertools 0.13.0", "nohash-hasher", "rand", "re_arrow2", - "re_build_info", + "re_error", "re_format", "re_format_arrow", "re_log", "re_log_types", - "re_string_interner", "re_tracing", "re_tuid", "re_types_core", + "serde", "similar-asserts", - "smallvec", - "static_assertions", "thiserror", ] [[package]] -name = "re_crash_handler" -version = "0.17.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7343f81f3f0fadfeba0ad21175e62db7fc5533ee8f2bc3a571c98bb33d12f530" -dependencies = [ - "backtrace", - "itertools", - "libc", - "parking_lot", - "re_build_info", -] - -[[package]] -name = "re_data_store" -version = "0.17.0" +name = "re_chunk_store" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7ab1755bb3b411f50e2aeeff18ebc2a2e9818bd5bb9537964489cae798f7316c" +checksum = "428c6c5ef2bdc0c8d9afee239d8d1ba51dc60559e60a302a361d7fe62727009a" dependencies = [ "ahash", "document-features", "indent", - "itertools", + "itertools 0.13.0", "nohash-hasher", "once_cell", "parking_lot", "re_arrow2", + "re_chunk", "re_format", - "re_format_arrow", "re_log", "re_log_types", "re_tracing", "re_types_core", - "smallvec", "thiserror", "web-time", ] +[[package]] +name = "re_crash_handler" +version = "0.18.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0a27eaac446e4cb6e151560d3215dceee779a1fd0e7c3cde81b4a6708c26d29d" +dependencies = [ + "backtrace", + "itertools 0.13.0", + "libc", + "parking_lot", + "re_build_info", +] + [[package]] name = "re_entity_db" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5c53f9ae7edf36481a9147cd26049ec9449f77bf17f8baf6626ae44a80a0bc6c" +checksum = "19b2485d7d304ab4a569e6cffa1ab9e9574fe412bb2dd81dad15a5a98f914924" dependencies = [ "ahash", "document-features", "emath", - "getrandom", - "itertools", + "itertools 0.13.0", "nohash-hasher", "parking_lot", "re_build_info", - "re_data_store", + "re_chunk", + "re_chunk_store", "re_format", "re_int_histogram", "re_log", @@ -2446,24 +2453,24 @@ dependencies = [ [[package]] name = "re_error" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2a238304455818c724cd69769bb12dc17526a3df9ac126092815ae72b266fb0e" +checksum = "33aefcf4ba9f02854105067dba72b30ead0b94beadd26d242d188d2cad855890" [[package]] name = "re_format" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fee779c60cde0552f740838a084b8b654d7cdd1c8d5881579fd8f69ba230bc12" +checksum = "84f4d2734843d2e4403c0378ac7ee1721511a5ddcbaf48d8bfe3208be38b2750" dependencies = [ "num-traits", ] [[package]] name = "re_format_arrow" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fe62af5594044ac9c9651a7c9b47db3088d1f89cd05ee2e84a6355042648c295" +checksum = "e093b8c9ca524367f65f63e7bef27510eea05a2e0dca75ead7cfcf778f55f0e9" dependencies = [ "comfy-table", "re_arrow2", @@ -2473,9 +2480,9 @@ dependencies = [ [[package]] name = "re_int_histogram" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "82f9fe0aae220d59227e1b577c67c0304f23e59419849d483047a0dae30f650b" +checksum = "952d88f59e10f3b5f20c1f672a416a2f38d193a356d37e654ea8514fb7d069f3" dependencies = [ "smallvec", "static_assertions", @@ -2483,9 +2490,9 @@ dependencies = [ [[package]] name = "re_log" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b04b37346b0cef146c875d286d707f0c7764a09239b741585dead9834ffcb5c3" +checksum = "a44cc0f4eb4a82fa7b2d16605c103256594e07262e1117b56472886f7e309fa9" dependencies = [ "env_logger 0.10.2", "js-sys", @@ -2498,13 +2505,14 @@ dependencies = [ [[package]] name = "re_log_encoding" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "014af653eefc26378b09d6b0f48472582a77ffc0428e65d850bfa8aba1daa231" +checksum = "9ff50fa788bfe385f63b8e8dbdb4a5f97f312dcd0a4b752dfdd8f11e970f7bd2" dependencies = [ "lz4_flex", "parking_lot", "re_build_info", + "re_chunk", "re_log", "re_log_types", "re_smart_channel", @@ -2515,19 +2523,20 @@ dependencies = [ [[package]] name = "re_log_types" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d4df039b428c0f02b8deafdf3879c84e25b616708fa09dedbfbcc999f51febe3" +checksum = "c953bc26e15d4bda28585a434df4bcfeaa0fc1f1174bd99a78c6347b529f8860" dependencies = [ "ahash", "anyhow", "backtrace", + "bytemuck", "clean-path", "crossbeam", "document-features", "fixed", "half", - "itertools", + "itertools 0.13.0", "natord", "nohash-hasher", "num-derive", @@ -2535,7 +2544,6 @@ dependencies = [ "re_arrow2", "re_build_info", "re_format", - "re_format_arrow", "re_log", "re_string_interner", "re_tracing", @@ -2544,7 +2552,6 @@ dependencies = [ "serde", "serde_bytes", "similar-asserts", - "smallvec", "static_assertions", "thiserror", "time", @@ -2555,14 +2562,14 @@ dependencies = [ [[package]] name = "re_memory" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "290b13c95fe6146709dcdbca790f08f677a0b73ca0a85402f04e844fb2e7db40" +checksum = "3d456b886bc5aea51abc720a0ead28bf2593c7dfc87c12f6d088030e30c28018" dependencies = [ "ahash", "backtrace", "emath", - "itertools", + "itertools 0.13.0", "memory-stats", "nohash-hasher", "once_cell", @@ -2578,46 +2585,45 @@ dependencies = [ [[package]] name = "re_query" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a43317de51580d71f81a5f385661bc191ebfa3a0fae733274afddf084a7d64eb" +checksum = "04bdaca11104426f316fdf94a59bc6333f2f0f062bc61d8dfbcf7f5f7929d99d" dependencies = [ "ahash", "anyhow", "backtrace", "indent", - "indexmap", - "itertools", + "itertools 0.13.0", "nohash-hasher", "parking_lot", "paste", "re_arrow2", - "re_data_store", + "re_chunk", + "re_chunk_store", "re_error", "re_format", "re_log", "re_log_types", "re_tracing", - "re_tuid", "re_types_core", "seq-macro", - "static_assertions", "thiserror", ] [[package]] name = "re_sdk" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fc80eaf36c1cbac09c1914d8ccace804536d5b3046ca387806258b4736c883f4" +checksum = "4120478d467a8cc9cbcb97746aae9042f3a8ffa2abcd3e17ce7f81f64e390209" dependencies = [ "ahash", "crossbeam", "document-features", - "itertools", + "itertools 0.13.0", "libc", "once_cell", "parking_lot", + "re_arrow2", "re_build_info", "re_build_tools", "re_chunk", @@ -2632,9 +2638,9 @@ dependencies = [ [[package]] name = "re_sdk_comms" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "65db075826857c30842adc76ea6615b8d263f9be9f30f2d4f89f448056da68cc" +checksum = "3f1463c37abe42f636738a6f6d7206cefb89d410058fb4ce974f397c8ef77600" dependencies = [ "ahash", "crossbeam", @@ -2649,9 +2655,9 @@ dependencies = [ [[package]] name = "re_smart_channel" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d2e0cc5f522f64534edf44fbd1ff03d706157b4a13597be661db7c88c7863cd7" +checksum = "6a20571dc97dc22a89ffb34ed46f16dc917eb428e5676210161d9a8d04f92547" dependencies = [ "crossbeam", "parking_lot", @@ -2662,9 +2668,9 @@ dependencies = [ [[package]] name = "re_string_interner" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2f6349840b6af8671eaf483453d600ff7fe747b8baa65fb69f69179b243e98bc" +checksum = "57858c98a25ce888815957cfd725115b51c848512334cbf432547289a3259c42" dependencies = [ "ahash", "nohash-hasher", @@ -2676,9 +2682,9 @@ dependencies = [ [[package]] name = "re_tracing" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f4c16e0c6a298497f1576447f3c8a3bb7607034e9019c778917925b67a0c66da" +checksum = "9414e648bf8b9d64db949c3cf87d1ef742cc96687d2ef982c6b6c68f386a1401" dependencies = [ "puffin", "puffin_http", @@ -2688,9 +2694,9 @@ dependencies = [ [[package]] name = "re_tuid" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bb7b6e25fb7b5201e96b7241c208426158211d676a635fc4b9ddbbb378e72ce6" +checksum = "38e45f11127f1052a784a71227e4064d695ba0275f9a9d5fe5cb595bb69e645e" dependencies = [ "document-features", "getrandom", @@ -2701,9 +2707,9 @@ dependencies = [ [[package]] name = "re_types" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4e48cb4fd4c787ab56484f58dcce00728c70e88fdd2f6b694ec3eb589c41465b" +checksum = "12a5181eb21eb0a2efb6129b1f3b10c485dfff0b3a795077630edf609d65dc0b" dependencies = [ "anyhow", "array-init", @@ -2712,7 +2718,7 @@ dependencies = [ "emath", "half", "infer", - "itertools", + "itertools 0.13.0", "linked-hash-map", "mime_guess2", "ndarray", @@ -2724,6 +2730,7 @@ dependencies = [ "re_build_tools", "re_format", "re_log", + "re_log_types", "re_tracing", "re_types_builder", "re_types_core", @@ -2734,16 +2741,16 @@ dependencies = [ [[package]] name = "re_types_builder" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "319906afc4fe193dc3ff5d5db5d1e3d64f93322beabf0bd3bf7bbd5e4c991c27" +checksum = "7222c14159d0792b728489db0eb18c09f643faee8f120e14dbee045455edd413" dependencies = [ "anyhow", "camino", "clang-format", "flatbuffers", "indent", - "itertools", + "itertools 0.13.0", "prettyplease", "proc-macro2", "quote", @@ -2755,7 +2762,7 @@ dependencies = [ "re_log", "re_tracing", "rust-format", - "syn 2.0.68", + "syn 2.0.77", "tempfile", "unindent", "xshell", @@ -2763,15 +2770,15 @@ dependencies = [ [[package]] name = "re_types_core" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "aea9a20eb65fa69872e2a68cb6a5348f886b2c7be9ff2aee4777f20b30eda8c6" +checksum = "3aa5181a363c04196046dca82575550d5d04f2ad52d8ebdc6d36adc79a94851d" dependencies = [ "anyhow", "backtrace", "bytemuck", "document-features", - "itertools", + "itertools 0.13.0", "nohash-hasher", "once_cell", "re_arrow2", @@ -2837,19 +2844,21 @@ checksum = "7a66a03ae7c801facd77a29370b4faec201768915ac14a721ba36f20bc9c209b" [[package]] name = "rerun" -version = "0.17.0" +version = "0.18.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "36720e9d27c1c65ca0a36a2356da9ae011f57f0c81b5173ce4fb7144ae88b0af" +checksum = "f8e6473101c2440d9dcad8d96aef558e38f0b8bb65b3f64f050d3b37b8316ccf" dependencies = [ "anyhow", "document-features", - "itertools", + "itertools 0.13.0", "puffin", "rayon", "re_build_info", "re_build_tools", + "re_chunk", "re_crash_handler", "re_entity_db", + "re_error", "re_format", "re_log", "re_log_types", @@ -2858,6 +2867,7 @@ dependencies = [ "re_smart_channel", "re_tracing", "re_types", + "similar-asserts", ] [[package]] @@ -3032,7 +3042,7 @@ checksum = "500cbc0ebeb6f46627f50f3f5811ccf6bf00643be300b4c3eabc0ef55dc5b5ba" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -3054,7 +3064,7 @@ checksum = "6c64451ba24fc7a6a2d60fc75dd9c83c90903b19028d4eff35e88fc1e86564e9" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -3189,7 +3199,7 @@ dependencies = [ "proc-macro2", "quote", "rustversion", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -3205,9 +3215,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.68" +version = "2.0.77" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "901fa70d88b9d6c98022e23b4136f9f3e54e4662c3bc1bd1d84a42a9a0f0c1e9" +checksum = "9f35bcdf61fd8e7be6caf75f429fdca8beb3ed76584befb503b1569faee373ed" dependencies = [ "proc-macro2", "quote", @@ -3280,7 +3290,7 @@ checksum = "46c3384250002a6d5af4d114f2845d37b57521033f30d5c3f46c4d70e1197533" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -3369,7 +3379,7 @@ checksum = "34704c8d6ebcbc939824180af020566b01a7c01f80641264eba0999f6c2b6be7" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -3424,7 +3434,7 @@ checksum = "ac73887f47b9312552aa90ef477927ff014d63d1920ca8037c6c1951eab64bb1" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] @@ -3564,7 +3574,7 @@ dependencies = [ "once_cell", "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", "wasm-bindgen-shared", ] @@ -3598,7 +3608,7 @@ checksum = "e94f17b526d0a461a191c78ea52bbce64071ed5c04c9ffe424dcb38f74171bb7" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", "wasm-bindgen-backend", "wasm-bindgen-shared", ] @@ -3945,7 +3955,7 @@ checksum = "15e934569e47891f7d9411f1a451d947a60e000ab3bd24fbb970f000387d1b3b" dependencies = [ "proc-macro2", "quote", - "syn 2.0.68", + "syn 2.0.77", ] [[package]] diff --git a/Cargo.toml b/Cargo.toml index f541235..78d009a 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -31,7 +31,7 @@ typetag = { version = "0.2.16", optional = true } serde_json = { version = "1.0.120", optional = true } # rerun support -rerun = { version = "0.17", optional = true, default-features = false, features = [ +rerun = { version = "0.18", optional = true, default-features = false, features = [ "sdk", ] } diff --git a/src/linalg/nalgebra_wrap.rs b/src/linalg/nalgebra_wrap.rs index 8f573b8..1429f85 100644 --- a/src/linalg/nalgebra_wrap.rs +++ b/src/linalg/nalgebra_wrap.rs @@ -5,6 +5,7 @@ pub use nalgebra::{ allocator::Allocator, dmatrix as matrixx, dvector as vectorx, + ComplexField, Const, DefaultAllocator, Dim, @@ -113,7 +114,7 @@ pub type VectorView5<'a, D = dtype> = na::VectorView<'a, D, Const<5>>; pub type VectorView6<'a, D = dtype> = na::VectorView<'a, D, Const<6>>; // Generic, taking in sizes with Const -pub type VectorDim = OVector; +pub type VectorDim = OVector; pub type MatrixDim, D = dtype> = na::Matrix>::Buffer>; pub type MatrixViewDim<'a, R, C = Const<1>, D = dtype> = na::MatrixView<'a, D, R, C>; diff --git a/src/rerun.rs b/src/rerun.rs index 4e099fc..e3daaaa 100644 --- a/src/rerun.rs +++ b/src/rerun.rs @@ -1,4 +1,5 @@ use rerun::{ + components::RotationQuat, Arrows2D, Arrows3D, AsComponents, @@ -158,7 +159,7 @@ impl<'a> From<&'a SO3> for Rotation3D { so3.z() as f32, so3.w() as f32, ]; - Rotation3D::Quaternion(Quaternion::from_xyzw(xyzw)) + Rotation3D::Quaternion(RotationQuat(Quaternion::from_xyzw(xyzw))) } } diff --git a/src/variables/traits.rs b/src/variables/traits.rs index e1d2afd..6d65d8e 100644 --- a/src/variables/traits.rs +++ b/src/variables/traits.rs @@ -111,7 +111,7 @@ pub trait Variable: Clone + Sized + Display + Debug { /// This can be seen as a "tip-to-tail" computation. IE it computes the /// transformation between two poses. I like to think of it as "taking away" /// the portion subtracted out, for example given a chain of poses $a, b, - /// c$, + /// c$, the following "removes" the portion from $a$ to $b$. /// /// $$ /// {}_a T_c \boxminus {}_a T_b = ({}_a T_b)^{-1} {}_a T_c = {}_b T_c