From 06411bac46009f7a5d607d52157661697f028906 Mon Sep 17 00:00:00 2001 From: kpbaks Date: Fri, 12 Apr 2024 22:17:47 +0200 Subject: [PATCH 1/6] feat(factorgraph): initial dump of factorgraph rewrite --- .pre-commit-config.yaml | 2 + .../src/factorgraph/factor/dynamic.rs | 83 +++ .../src/factorgraph/factor/interrobot.rs | 201 +++++++ .../factor/marginalise_factor_distance.rs | 335 ++++++++++++ .../src/factorgraph/factor/mod.rs | 496 ++++++++++++++++++ .../src/factorgraph/factor/obstacle.rs | 127 +++++ .../src/factorgraph/factor/pose.rs | 37 ++ .../src/factorgraph/factorgraph.rs | 308 +++++++++++ .../gbpplanner-rs/src/factorgraph/graphviz.rs | 0 crates/gbpplanner-rs/src/factorgraph/id.rs | 133 +++++ .../gbpplanner-rs/src/factorgraph/message.rs | 165 ++++++ crates/gbpplanner-rs/src/factorgraph/mod.rs | 51 ++ crates/gbpplanner-rs/src/factorgraph/node.rs | 101 ++++ .../gbpplanner-rs/src/factorgraph/variable.rs | 393 ++++++++++++++ 14 files changed, 2432 insertions(+) create mode 100644 crates/gbpplanner-rs/src/factorgraph/factor/dynamic.rs create mode 100644 crates/gbpplanner-rs/src/factorgraph/factor/interrobot.rs create mode 100644 crates/gbpplanner-rs/src/factorgraph/factor/marginalise_factor_distance.rs create mode 100644 crates/gbpplanner-rs/src/factorgraph/factor/mod.rs create mode 100644 crates/gbpplanner-rs/src/factorgraph/factor/obstacle.rs create mode 100644 crates/gbpplanner-rs/src/factorgraph/factor/pose.rs create mode 100644 crates/gbpplanner-rs/src/factorgraph/factorgraph.rs create mode 100644 crates/gbpplanner-rs/src/factorgraph/graphviz.rs create mode 100644 crates/gbpplanner-rs/src/factorgraph/id.rs create mode 100644 crates/gbpplanner-rs/src/factorgraph/message.rs create mode 100644 crates/gbpplanner-rs/src/factorgraph/mod.rs create mode 100644 crates/gbpplanner-rs/src/factorgraph/node.rs create mode 100644 crates/gbpplanner-rs/src/factorgraph/variable.rs diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 08348b91..97809a45 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -22,3 +22,5 @@ repos: - id: conventional-pre-commit stages: [commit-msg] args: [] + + diff --git a/crates/gbpplanner-rs/src/factorgraph/factor/dynamic.rs b/crates/gbpplanner-rs/src/factorgraph/factor/dynamic.rs new file mode 100644 index 00000000..2ab0686b --- /dev/null +++ b/crates/gbpplanner-rs/src/factorgraph/factor/dynamic.rs @@ -0,0 +1,83 @@ +//! + +use gbp_linalg::prelude::*; + +use super::{FactorState, Model}; +use crate::factorgraph::DOFS; + +/// Dynamic factor: constant velocity model +#[derive(Debug)] +pub struct DynamicFactor { + cached_jacobian: Matrix, +} + +impl DynamicFactor { + pub const NEIGHBORS: usize = 2; + + #[must_use] + pub fn new(state: &mut FactorState, delta_t: Float) -> Self { + let eye = Matrix::::eye(DOFS / 2); + let zeros = Matrix::::zeros((DOFS / 2, DOFS / 2)); + #[allow(clippy::similar_names)] + let qc_inv = Float::powi(state.strength, -2) * &eye; + + #[allow(clippy::similar_names)] + let qi_inv = concatenate![ + Axis(0), + concatenate![ + Axis(1), + 12.0 * Float::powi(delta_t, -3) * &qc_inv, + -6.0 * Float::powi(delta_t, -2) * &qc_inv + ], + concatenate![ + Axis(1), + -6.0 * Float::powi(delta_t, -2) * &qc_inv, + (4.0 / delta_t) * &qc_inv + ] + ]; + debug_assert_eq!(qi_inv.shape(), &[DOFS, DOFS]); + + state.measurement_precision = qi_inv; + + let cached_jacobian = concatenate![ + Axis(0), + concatenate![Axis(1), eye, delta_t * &eye, -1.0 * &eye, zeros], + concatenate![Axis(1), zeros, eye, zeros, -1.0 * &eye] + ]; + debug_assert_eq!(cached_jacobian.shape(), &[DOFS, DOFS * 2]); + + Self { cached_jacobian } + } +} + +impl Model for DynamicFactor { + #[inline] + fn name(&self) -> &'static str { + "DynamicFactor" + } + + #[inline] + fn jacobian(&mut self, _state: &FactorState, _x: &Vector) -> Matrix { + self.cached_jacobian.clone() + } + + #[inline(always)] + fn measure(&mut self, _state: &FactorState, x: &Vector) -> Vector { + self.cached_jacobian.dot(x) + } + + #[inline(always)] + fn skip(&mut self, _state: &FactorState) -> bool { + false + } + + #[inline(always)] + fn jacobian_delta(&self) -> Float { + 1e-8 + } + + #[inline(always)] + fn linear(&self) -> bool { + true + } +} diff --git a/crates/gbpplanner-rs/src/factorgraph/factor/interrobot.rs b/crates/gbpplanner-rs/src/factorgraph/factor/interrobot.rs new file mode 100644 index 00000000..63b23d72 --- /dev/null +++ b/crates/gbpplanner-rs/src/factorgraph/factor/interrobot.rs @@ -0,0 +1,201 @@ +use gbp_linalg::prelude::*; + +// TODO: add to and from +#[derive(Debug, Clone, Copy)] +pub struct InterRobotConnection { + pub id_of_robot_connected_with: RobotId, + pub index_of_connected_variable_in_other_robots_factorgraph: NodeIndex, +} + +impl InterRobotConnection { + #[must_use] + pub fn new( + id_of_robot_connected_with: RobotId, + index_of_connected_variable_in_other_robots_factorgraph: NodeIndex, + ) -> Self { + Self { + id_of_robot_connected_with, + index_of_connected_variable_in_other_robots_factorgraph, + } + } +} + +// #[derive(Debug, Clone, Copy, PartialEq, Eq)] +// pub struct Skip(bool); + +/// Interrobot factor: for avoidance of other robots +/// This factor results in a high energy or cost if two robots are planning to +/// be in the same position at the same timestep (collision). This factor is +/// created between variables of two robots. The factor has 0 energy if the +/// variables are further away than the safety distance. +#[derive(Debug, Clone)] +pub struct InterRobotFactor { + safety_distance: Float, + skip: bool, + pub connection: InterRobotConnection, +} + +#[derive(Debug, thiserror::Error)] +pub enum InterRobotFactorError { + #[error("The robot radius must be positive, but it was {0}")] + NegativeRobotRadius(Float), +} + +impl InterRobotFactor { + pub const NEIGHBORS: usize = 2; + + #[must_use] + pub fn new( + robot_radius: Float, + connection: InterRobotConnection, + ) -> Result { + if robot_radius < 0.0 { + return Err(InterRobotFactorError::NegativeRobotRadius(robot_radius)); + } + let epsilon = 0.2 * robot_radius; + + Ok(Self { + safety_distance: 2.0 * robot_radius + epsilon, + skip: false, + connection, + }) + } +} + +impl Model for InterRobotFactor { + #[inline(always)] + fn name(&self) -> &'static str { + "InterRobotFactor" + } + + fn jacobian(&mut self, state: &FactorState, x: &Vector) -> Matrix { + let dofs = state.dofs.get(); + let mut jacobian = Matrix::::zeros((state.initial_measurement.len(), dofs * 2)); + let x_diff = { + let offset = dofs / 2; + let mut x_diff = x.slice(s![..offset]).sub(&x.slice(s![dofs..dofs + offset])); + + // NOTE: In gbplanner, they weight this by the robot id, why they do this is + // unclear as a robot id should be unique, and not have any + // semantics of distance/weight. + for i in 0..offset { + x_diff[i] += 1e-6 * self.connection.id_of_robot_connected_with.index() as Float; + // Add a tiny random offset to avoid div/0 errors + } + x_diff + }; + + let radius = x_diff.euclidean_norm(); + if radius <= self.safety_distance { + // TODO: why do we change the Jacobian if we are not outside the safety + // distance? + + // J(0, seqN(0, n_dofs_ / 2)) = -1.f / safety_distance_ / r * X_diff; + jacobian + .slice_mut(s![0, ..dofs / 2]) + .assign(&(-1.0 / self.safety_distance / radius * &x_diff)); + + // J(0, seqN(n_dofs_, n_dofs_ / 2)) = 1.f / safety_distance_ / r * X_diff; + jacobian + .slice_mut(s![0, dofs..dofs + (dofs / 2)]) + .assign(&(1.0 / self.safety_distance / radius * &x_diff)); + } + jacobian + } + + fn measure(&mut self, state: &FactorState, x: &Vector) -> Vector { + let dofs = state.dofs.get(); + let mut h = Vector::::zeros(state.initial_measurement.len()); + let x_diff = { + let offset = dofs / 2; + let mut x_diff = x.slice(s![..offset]).sub(&x.slice(s![dofs..dofs + offset])); + // NOTE: In gbplanner, they weight this by the robot id, why they do this is + // unclear as a robot id should be unique, and not have any + // semantics of distance/weight. + for i in 0..offset { + // Add a tiny random offset to avoid div/0 errors + x_diff[i] += 1e-6 * self.connection.id_of_robot_connected_with.index() as Float; + } + x_diff + }; + + let radius = x_diff.euclidean_norm(); + let within_safety_distance = radius <= self.safety_distance; + // match (self.skip, within_safety_distance) { + // (Skip(true), true) => {} + // (Skip(true), false) => {} + // (Skip(false), true) => { + // self.skip = Skip(true); + // info!("skip = true, radius = {}", radius); + // } + // (Skip(false), false) => {} + // }; + if radius <= self.safety_distance { + if self.skip { + info!( + "within safety distance, radius = {}, setting self.skip to false", + radius + ); + } + self.skip = false; + // gbpplanner: h(0) = 1.f*(1 - r/safety_distance_); + // NOTE: in Eigen, indexing a matrix with a single index corresponds to indexing + // the matrix as a flattened array in column-major order. + // h[(0, 0)] = 1.0 * (1.0 - radius / self.safety_distance); + h[0] = 1.0 * (1.0 - radius / self.safety_distance); + // println!("h = {}", h); + } else { + if !self.skip { + // info!( + // "outside safety distance, radius = {}, setting self.skip + // to true", radius + // ); + } + self.skip = true; + } + + h + } + + #[inline(always)] + fn jacobian_delta(&self) -> Float { + 1e-2 + } + + fn skip(&mut self, state: &FactorState) -> bool { + // this->skip_flag = ( (X_(seqN(0,n_dofs_/2)) - X_(seqN(n_dofs_, + // n_dofs_/2))).squaredNorm() >= safety_distance_*safety_distance_ ); + let dofs = state.dofs.get(); + let offset = dofs / 2; + + // [..offset] is the position of the first variable + // [dofs..dofs + offset] is the position of the other variable + + let difference_between_estimated_positions = state + .linearisation_point + .slice(s![..offset]) + .sub(&state.linearisation_point.slice(s![dofs..dofs + offset])); + let squared_norm = difference_between_estimated_positions + .mapv(|x| x.powi(2)) + .sum(); + + let skip = squared_norm >= self.safety_distance.powi(2); + // let skip = squared_norm >= Float::powi(self.safety_distance, 2); + if self.skip != skip { + // warn!( + // "skip = {}, squared_norm = {} safety_distance^2 = {}", + // skip, + // squared_norm, + // Float::powi(self.safety_distance, 2) + // ); + } + self.skip = skip; + // self.skip = squared_norm >= Float::powi(self.safety_distance, 2); + self.skip + } + + #[inline(always)] + fn linear(&self) -> bool { + false + } +} diff --git a/crates/gbpplanner-rs/src/factorgraph/factor/marginalise_factor_distance.rs b/crates/gbpplanner-rs/src/factorgraph/factor/marginalise_factor_distance.rs new file mode 100644 index 00000000..99a870ad --- /dev/null +++ b/crates/gbpplanner-rs/src/factorgraph/factor/marginalise_factor_distance.rs @@ -0,0 +1,335 @@ +use gbp_linalg::prelude::*; +use ndarray::prelude::*; +use ndarray_inverse::Inverse; + +use super::message::Message; +use crate::planner::message::{Eta, Lam, Mu}; + +/// Utility function to create `start..start + n` +/// Similar to `Eigen::seqN` +const fn seq_n(start: usize, n: usize) -> std::ops::Range { + start..start + n +} + +type Aa<'a, T> = MatrixView<'a, T>; +type Ab<'a, T> = MatrixView<'a, T>; +type Ba<'a, T> = MatrixView<'a, T>; +type Bb<'a, T> = MatrixView<'a, T>; + +fn extract_submatrices_from_precision_matrix( + precision_matrix: &Matrix, + dofs: usize, + marg_idx: usize, +) -> (Aa, Ab, Ba, Bb) { + debug_assert!(precision_matrix.is_square()); + debug_assert_eq!(precision_matrix.nrows() % dofs, 0); + debug_assert_eq!(precision_matrix.ncols() % dofs, 0); + + let aa = precision_matrix.slice(s![seq_n(marg_idx, dofs), seq_n(marg_idx, dofs)]); + + let ab = if marg_idx == 0 { + precision_matrix.slice(s![seq_n(marg_idx, dofs), marg_idx + dofs..]) + } else { + precision_matrix.slice(s![seq_n(marg_idx, dofs), ..marg_idx]) + }; + + let ba = if marg_idx == 0 { + precision_matrix.slice(s![marg_idx + dofs.., seq_n(marg_idx, dofs)]) + } else { + precision_matrix.slice(s![..marg_idx, seq_n(marg_idx, dofs)]) + }; + + let bb = if marg_idx == 0 { + precision_matrix.slice(s![marg_idx + dofs.., marg_idx + dofs..]) + } else { + precision_matrix.slice(s![..marg_idx, ..marg_idx]) + }; + + (aa, ab, ba, bb) +} + +pub fn marginalise_factor_distance( + information_vector: Vector, + precision_matrix: Matrix, + variable_dofs: usize, + marginalisation_idx: usize, +) -> Result { + let ndofs = variable_dofs; + let marg_idx = marginalisation_idx; + + debug_assert_eq!(information_vector.len(), precision_matrix.nrows()); + debug_assert_eq!(precision_matrix.nrows(), precision_matrix.ncols()); + // pretty_print_vector!(&information_vector); + // pretty_print_matrix!(&precision_matrix); + + let factor_only_connected_to_one_variable = information_vector.len() == variable_dofs; + if factor_only_connected_to_one_variable { + let mu = Vector::::zeros(information_vector.len()); + return Ok(Message::new( + Eta(information_vector), + Lam(precision_matrix), + Mu(mu), + )); + // dbg!(&information_vector); + // dbg!(&precision_matrix); + // TODO: return None + // let mvn = MultivariateNormal::from_information_and_precision( + // information_vector, + // precision_matrix, + // ) + // .inspect_err(|_| { + // // pretty_print_matrix!(&precision_matrix); + // }) + // .expect( + // "the given information vector and precision matrix is a valid + // multivariate gaussian", ); + // return Ok(Message::new(mvn)); + } + + // eprintln!( + // "information_vector shape = {:?}, ndofs = {:?}", + // information_vector.shape(), + // variable_dofs + // ); + // eprintln!("precision_matrix shape = {:?}", precision_matrix.shape()); + + // eprintln!("show me precision_matrix = \n{:?}", precision_matrix); + + // let iv = &information_vector; + // let pm = &precision_matrix; + + let eta_a = information_vector.slice(s![seq_n(marg_idx, ndofs)]); + assert_eq!(eta_a.len(), ndofs); + + let eta_b = if marg_idx == 0 { + information_vector.slice(s![ndofs..]) + } else { + information_vector.slice(s![..marg_idx]) + }; + assert_eq!(eta_b.len(), information_vector.len() - ndofs); + + let (lam_aa, lam_ab, lam_ba, lam_bb) = + extract_submatrices_from_precision_matrix(&precision_matrix, ndofs, marg_idx); + + // let lam_aa = precision_matrix.slice(s![seq_n(marg_idx, ndofs), + // seq_n(marg_idx, ndofs)]); + + // let lam_ab = if marg_idx == 0 { + // precision_matrix.slice(s![seq_n(marg_idx, ndofs), marg_idx + ndofs..]) + // } else { + // precision_matrix.slice(s![seq_n(marg_idx, ndofs), ..marg_idx]) + // }; + + // assert_eq!(lam_ab.shape(), &[ndofs, precision_matrix.ncols() - ndofs]); + + // // eprintln!("margin_idx = {}", marg_idx); + + // let lam_ba = if marg_idx == 0 { + // precision_matrix.slice(s![marg_idx + ndofs.., seq_n(marg_idx, ndofs)]) + // } else { + // precision_matrix.slice(s![..marg_idx, seq_n(marg_idx, ndofs)]) + // }; + + // let lam_bb = if marg_idx == 0 { + // precision_matrix.slice(s![marg_idx + ndofs.., marg_idx + ndofs..]) + // } else { + // precision_matrix.slice(s![..marg_idx, ..marg_idx]) + // }; + + // assert_eq!( + // lam_bb.shape(), + // &[ + // precision_matrix.shape()[0] - ndofs, + // precision_matrix.shape()[1] - ndofs + // ] + // ); + + // eprintln!("lam_bb = {:?}", lam_bb); + + let Some(lam_bb_inv) = lam_bb.to_owned().inv() else { + return Ok(Message::empty(ndofs)); + }; + + // let lam_bb_inv = lam_bb.to_owned().inv().expect("should have an inverse"); + // let information_vector = (&eta_a - &lam_ab).dot(&lam_bb_inv).dot(&eta_b); + let information_vector = &eta_a - &lam_ab.dot(&lam_bb_inv).dot(&eta_b); + // eprintln!("information_vector = {:?}", information_vector); + let precision_matrix = &lam_aa - &lam_ab.dot(&lam_bb_inv).dot(&lam_ba); + // eprintln!("precision_matrix = {:?}", precision_matrix); + + if precision_matrix.iter().any(|elem| elem.is_infinite()) { + Ok(Message::empty(information_vector.len())) + // Message::with_dofs(information_vector.len()) + // Message::zeros(information_vector.len()) + } else { + let mu = Vector::::zeros(information_vector.len()); + Ok(Message::new( + Eta(information_vector), + Lam(precision_matrix), + Mu(mu), + )) + + // let mvn = MultivariateNormal::from_information_and_precision( + // information_vector, + // precision_matrix, + // ) + // .expect( + // "the given information vector and precision matrix is a valid + // multivariate gaussian", ); + // Ok(Message::new(mvn)) + } +} + +#[cfg(test)] +mod tests { + use ndarray::concatenate; + use pretty_assertions::assert_eq; + + use super::*; + + fn float_eq(lhs: f32, rhs: f32) -> bool { + f32::abs(lhs - rhs) <= f32::EPSILON + } + + macro_rules! generate_8x8_precision_matrix { + () => {{ + let upper_left = array![[1., 2., 3., 4.], [5., 6., 7., 8.], [9., 10., 11., 12.], [ + 13., 14., 15., 16. + ]]; + + let upper_right = array![ + [17., 18., 19., 20.], + [21., 22., 23., 24.], + [25., 26., 27., 28.], + [29., 30., 31., 32.] + ]; + + let lower_left = array![ + [33., 34., 35., 36.], + [37., 38., 39., 40.], + [41., 42., 43., 44.], + [45., 46., 47., 48.] + ]; + + let lower_right = array![ + [49., 50., 51., 52.], + [53., 54., 55., 56.], + [57., 58., 59., 60.], + [61., 62., 63., 64.] + ]; + + let precision_matrix = concatenate![ + Axis(0), + concatenate![Axis(1), upper_left, upper_right], + concatenate![Axis(1), lower_left, lower_right] + ]; + ( + precision_matrix, + upper_left, + upper_right, + lower_left, + lower_right, + ) + }}; + } + + #[test] + fn extract_submatrices_from_precision_matrix_with_marg_idx0_dofs4() { + let (precision_matrix, upper_left, upper_right, lower_left, lower_right) = + generate_8x8_precision_matrix!(); + + assert!(precision_matrix.is_square()); + + let (aa, ab, ba, bb) = extract_submatrices_from_precision_matrix(&precision_matrix, 4, 0); + + assert_eq!(aa, upper_left); + assert_eq!(ab, upper_right); + assert_eq!(ba, lower_left); + assert_eq!(bb, lower_right); + } + + #[test] + fn extract_submatrices_from_precision_matrix_with_marg_idx4_dofs4() { + let (precision_matrix, upper_left, upper_right, lower_left, lower_right) = + generate_8x8_precision_matrix!(); + + assert!(precision_matrix.is_square()); + + let (aa, ab, ba, bb) = extract_submatrices_from_precision_matrix(&precision_matrix, 4, 4); + + assert_eq!(aa, lower_right); + assert_eq!(ab, lower_left); + assert_eq!(ba, upper_right); + assert_eq!(bb, upper_left); + } + + #[test] + fn information_vector_length_equal_to_ndofs_do_nothing() { + #![allow(clippy::unwrap_used)] + let information_vector: Vector = array![0., 1., 2., 3.]; + let precision_matrix: Matrix = + array![[5., 0.2, 0., 0.], [0.2, 5., 0., 0.], [0., 0.0, 5., 0.3], [ + 0., 0., 0.3, 5. + ]]; + + let ndofs = 4; + let marginalisation_idx = 0; + + let mut marginalised_msg = marginalise_factor_distance( + information_vector.clone(), + precision_matrix.clone(), + ndofs, + marginalisation_idx, + ) + .unwrap(); + + let payload = marginalised_msg.take().unwrap(); + + assert_eq!(payload.eta, information_vector); + assert_eq!(payload.lam, precision_matrix); + } + + // #[test] + // fn size5x5_marg_idx1_ndofs4() { + // let information_vector: Vector = array![1., 2., 3., 4., 5.]; + // let precision_matrix: Matrix = array![ + // [0.5, 0.1, 0., 0., 0.2], + // [0.1, 0.5, 0., 0., 0.], + // [0., 0.0, 0.5, 0., 0.], + // [0., 0., 0., 0.5, 0.], + // [0.2, 0., 0., 0., 0.5] + // ]; + + // let ndofs = 4; + // let marginalisation_idx = 1; + + // let marginalised_msg = marginalise_factor_distance( + // information_vector, + // precision_matrix, + // ndofs, + // marginalisation_idx, + // ); + + // assert_eq!(marginalised_msg.information_vector().len(), ndofs); + // assert_eq!(marginalised_msg.precision_matrix().shape(), &[ndofs, + // ndofs]); + + // assert_eq!( + // marginalised_msg.information_vector(), + // array![1.8, 3., 4., 4.6] + // ); + + // let result = marginalised_msg + // .precision_matrix() + // .into_iter() + // .collect::>(); + // let expected = array![ + // [0.48, 0., 0., -0.04], + // [0., 0.5, 0., 0.,], + // [0., 0., 0.5, 0.], + // [-0.04, 0., 0., 0.42] + // ] + // .into_iter() + // .collect::>(); + // } +} diff --git a/crates/gbpplanner-rs/src/factorgraph/factor/mod.rs b/crates/gbpplanner-rs/src/factorgraph/factor/mod.rs new file mode 100644 index 00000000..970df7f0 --- /dev/null +++ b/crates/gbpplanner-rs/src/factorgraph/factor/mod.rs @@ -0,0 +1,496 @@ +use bevy::render::texture::Image; +use gbp_linalg::prelude::*; +use typed_floats::StrictlyPositiveFinite; + +use self::{ + dynamic::DynamicFactor, + interrobot::{InterRobotConnection, InterRobotFactor, InterRobotFactorError}, + obstacle::ObstacleFactor, + pose::PoseFactor, +}; +use super::{ + factorgraph::NodeIndex, + id::VariableId, + message::{MessagesToFactors, MessagesToVariables}, + prelude::Message, + MessageCount, DOFS, +}; + +mod dynamic; +mod interrobot; +mod obstacle; +mod pose; +mod marginalize_factor_distance; + +// TODO: make generic over f32 | f64 +// TODO: hide the state parameter from the public API, by having the `Factor` +// struct expose similar methods that dispatch to the `FactorState` struct. +pub trait Model { + /// The name of the factor. Used for debugging and visualization. + fn name(&self) -> &'static str; + + fn jacobian_delta(&self) -> Float; + + /// Whether to skip this factor in the update step + /// In gbpplanner, this is only used for the interrobot factor. + /// The other factors are always included in the update step. + fn skip(&mut self, state: &FactorState) -> bool; + + /// Whether the factor is linear or non-linear + fn linear(&self) -> bool; + + #[must_use] + #[inline] + fn jacobian(&mut self, state: &FactorState, x: &Vector) -> Matrix { + self.first_order_jacobian(state, x.clone()) + } + + /// Measurement function + /// **Note**: This method takes a mutable reference to self, because the + /// interrobot factor + fn measure(&mut self, state: &FactorState, x: &Vector) -> Vector; + + fn first_order_jacobian(&mut self, state: &FactorState, mut x: Vector) -> Matrix { + let h0 = self.measure(state, &x); // value at linearization point + let mut jacobian = Matrix::::zeros((h0.len(), x.len())); + + for i in 0..x.len() { + x[i] += self.jacobian_delta(); // perturb by delta + let derivatives = (self.measure(state, &x) - &h0) / self.jacobian_delta(); + jacobian.column_mut(i).assign(&derivatives); + x[i] -= self.jacobian_delta(); // reset the perturbation + } + + jacobian + } +} + +#[derive(Debug)] +pub struct Factor { + /// Unique identifier that associates the variable with the factorgraph it + /// is part of. + pub node_index: Option, + /// State common between all factor kinds + pub state: FactorState, + /// Variant storing the specialized behavior of each Factor kind. + pub kind: FactorKind, + /// Mailbox for incoming message storage + pub inbox: MessagesToFactors, + + message_count: MessageCount, +} + +impl Factor { + fn new(state: FactorState, kind: FactorKind) -> Self { + Self { + node_index: None, + state, + kind, + inbox: MessagesToFactors::new(), + message_count: MessageCount::default(), + } + } + + /// Returns the node index of the factor + /// + /// # Panics + /// + /// Panics if the node index has not been set, which should not happen. + #[inline] + pub fn node_index(&self) -> NodeIndex { + if self.node_index.is_none() { + panic!("The node index has not been set"); + } + self.node_index.unwrap() + } + + /// Sets the node index of the factor + /// + /// # Panics + /// + /// Panics if the node index has already been set. + pub fn set_node_index(&mut self, node_index: NodeIndex) { + if self.node_index.is_some() { + panic!("The node index is already set"); + } + self.node_index = Some(node_index); + } + + pub fn new_dynamic_factor(strength: Float, measurement: Vector, delta_t: Float) -> Self { + let mut state = FactorState::new(measurement, strength, DynamicFactor::NEIGHBORS); + let dynamic_factor = DynamicFactor::new(&mut state, delta_t); + let kind = FactorKind::Dynamic(dynamic_factor); + Self::new(state, kind) + } + + pub fn new_interrobot_factor( + strength: Float, + measurement: Vector, + safety_radius: StrictlyPositiveFinite, + connection: InterRobotConnection, + ) -> Result { + let interrobot_factor = InterRobotFactor::new(safety_radius.get(), connection)?; + let kind = FactorKind::InterRobot(interrobot_factor); + let state = FactorState::new(measurement, strength, InterRobotFactor::NEIGHBORS); + + Ok(Self::new(state, kind)) + } + + pub fn new_pose_factor() -> Self { + unimplemented!("the pose factor is stored in the variable") + } + + pub fn new_obstacle_factor( + strength: Float, + measurement: Vector, + obstacle_sdf: &'static Image, + world_size: Float, + ) -> Self { + let state = FactorState::new(measurement, strength, ObstacleFactor::NEIGHBORS); + let obstacle_factor = ObstacleFactor::new(obstacle_sdf, world_size); + let kind = FactorKind::Obstacle(obstacle_factor); + Self::new(state, kind) + } + + #[inline(always)] + pub fn variant(&self) -> &'static str { + self.kind.name() + } + + #[inline(always)] + fn jacobian(&mut self, x: &Vector) -> Matrix { + self.kind.jacobian(&self.state, x) + } + + fn measure(&mut self, x: &Vector) -> Vector { + self.state.cached_measurement = self.kind.measure(&self.state, x); + self.state.cached_measurement.clone() + } + + /// Check if the factor should be skipped in the update step + #[inline(always)] + fn skip(&mut self) -> bool { + self.kind.skip(&self.state) + } + + pub fn receive_message_from(&mut self, from: VariableId, message: Message) { + let _ = self.inbox.insert(from, message); + self.message_count.received += 1; + } + + #[inline(always)] + pub fn read_message_from(&mut self, from: VariableId) -> Option<&Message> { + self.inbox.get(&from) + } + + /// Calculates the residual between the current measurement and the initial + /// measurement + #[inline(always)] + #[must_use] + fn residual(&self) -> Vector { + &self.state.initial_measurement - &self.state.cached_measurement + } + + #[must_use] + pub fn update(&mut self) -> MessagesToVariables { + debug_assert_eq!( + self.state.linearisation_point.len(), + DOFS * self.inbox.len() + ); + + let zero_mean = Vector::::zeros(DOFS); + + for (i, (_, message)) in self.inbox.iter().enumerate() { + let mean = message.mean().unwrap_or(&zero_mean); + self.state + .linearisation_point + .slice_mut(s![i * DOFS..(i + 1) * DOFS]) + .assign(mean); + } + + if self.skip() { + let messages: MessagesToVariables = self + .inbox + .iter() + .map(|(variable_id, _)| (*variable_id, Message::empty())) + .collect(); + self.message_count.sent += messages.len(); + return messages; + } + + let meas = self.measure(&self.state.linearisation_point.clone()); + let jacobian = self.jacobian(&self.state.linearisation_point.clone()); + + let potential_precision_matrix = jacobian + .t() + .dot(&self.state.measurement_precision) + .dot(&jacobian); + let potential_information_vec = jacobian + .t() + .dot(&self.state.measurement_precision) + .dot(&(jacobian.dot(&self.state.linearisation_point) + self.residual())); + + self.state.initialized = true; + + let mut marginalisation_idx = 0; + let mut messages = MessagesToVariables::new(); + + // let zero_precision = Matrix::::zeros((DOFS, DOFS)); + + for variable_id in self.inbox.keys() { + let mut information_vec = potential_information_vec.clone(); + let mut precision_matrix = potential_precision_matrix.clone(); + + for (j, (other_variable_id, other_message)) in self.inbox.iter().enumerate() { + if other_variable_id == variable_id { + // Do not aggregate data from the variable we're sending to + continue; + } + + let message_eta = other_message + .information_vector() + .expect("it better be there"); + + // let message_precision = + // other_message.precision_matrix().unwrap_or(&zero_precision); + let message_precision = other_message + .precision_matrix() + .unwrap_or_else(|| &Matrix::::zeros((DOFS, DOFS))); + + information_vec + .slice_mut(s![j * DOFS..(j + 1) * DOFS]) + .add_assign(message_eta); + precision_matrix + .slice_mut(s![j * DOFS..(j + 1) * DOFS, j * DOFS..(j + 1) * DOFS]) + .add_assign(message_precision); + } + + let message = + marginalise_factor_distance(information_vec, precision_matrix, marginalisation_idx) + .expect("marginalise_factor_distance should not fail"); + messages.insert(*variable_id, message); + marginalisation_idx += DOFS; + } + + self.message_count.sent += messages.len(); + messages + } +} + +impl FactorGraphNode for Factor { + fn remove_connection_to( + &mut self, + factorgraph_id: super::factorgraph::FactorGraphId, + ) -> Result<(), super::factorgraph::RemoveConnectionToError> { + let connections_before = self.inbox.len(); + self.inbox + .retain(|variable_id, v| variable_id.factorgraph_id != factorgraph_id); + let connections_after = self.inbox.len(); + + let no_connections_removed = connections_before == connections_after; + if no_connections_removed { + Err(super::factorgraph::RemoveConnectionToError) + } else { + Ok(()) + } + } + + #[inline] + fn messages_sent(&self) -> usize { + self.message_count.sent + } + + #[inline] + fn messages_received(&self) -> usize { + self.message_count.received + } + + #[inline(always)] + fn reset_message_count(&mut self) { + self.message_count.reset(); + } +} + +#[derive(Debug, derive_more::IsVariant)] +pub enum FactorKind { + Pose(PoseFactor), + InterRobot(InterRobotFactor), + Dynamic(DynamicFactor), + Obstacle(ObstacleFactor), +} + +impl FactorKind { + /// Returns `Some(&InterRobotFactor)` if self is [`InterRobot`], otherwise + pub fn as_inter_robot(&self) -> Option<&InterRobotFactor> { + if let Self::InterRobot(v) = self { + Some(v) + } else { + None + } + } + + /// Returns `Some(&DynamicFactor)` if self is [`Dynamic`], otherwise + pub fn as_dynamic(&self) -> Option<&DynamicFactor> { + if let Self::Dynamic(v) = self { + Some(v) + } else { + None + } + } + + /// Returns `Some(&ObstacleFactor)` if self is [`Obstacle`], otherwise + pub fn as_obstacle(&self) -> Option<&ObstacleFactor> { + if let Self::Obstacle(v) = self { + Some(v) + } else { + None + } + } + + /// Returns `Some(&PoseFactor)` if self is [`Pose`], otherwise `None`. + pub fn as_pose(&self) -> Option<&PoseFactor> { + if let Self::Pose(v) = self { + Some(v) + } else { + None + } + } +} + +impl Model for FactorKind { + fn name(&self) -> &'static str { + match self { + FactorKind::Pose(f) => f.name(), + FactorKind::InterRobot(f) => f.name(), + FactorKind::Dynamic(f) => f.name(), + FactorKind::Obstacle(f) => f.name(), + } + } + + fn jacobian(&mut self, state: &FactorState, x: &Vector) -> Matrix { + match self { + FactorKind::Pose(f) => f.jacobian(state, x), + FactorKind::InterRobot(f) => f.jacobian(state, x), + FactorKind::Dynamic(f) => f.jacobian(state, x), + FactorKind::Obstacle(f) => f.jacobian(state, x), + } + } + + fn measure(&mut self, state: &FactorState, x: &Vector) -> Vector { + match self { + FactorKind::Pose(f) => f.measure(state, x), + FactorKind::InterRobot(f) => f.measure(state, x), + FactorKind::Dynamic(f) => f.measure(state, x), + FactorKind::Obstacle(f) => f.measure(state, x), + } + } + + fn skip(&mut self, state: &FactorState) -> bool { + match self { + FactorKind::Pose(f) => f.skip(state), + FactorKind::InterRobot(f) => f.skip(state), + FactorKind::Dynamic(f) => f.skip(state), + FactorKind::Obstacle(f) => f.skip(state), + } + } + + fn jacobian_delta(&self) -> Float { + match self { + FactorKind::Pose(f) => f.jacobian_delta(), + FactorKind::InterRobot(f) => f.jacobian_delta(), + FactorKind::Dynamic(f) => f.jacobian_delta(), + FactorKind::Obstacle(f) => f.jacobian_delta(), + } + } + + fn linear(&self) -> bool { + match self { + FactorKind::Pose(f) => f.linear(), + FactorKind::InterRobot(f) => f.linear(), + FactorKind::Dynamic(f) => f.linear(), + FactorKind::Obstacle(f) => f.linear(), + } + } +} + +#[derive(Debug, Clone)] +pub struct FactorState { + /// called `z_` in **gbpplanner** + pub initial_measurement: Vector, + /// called `meas_model_lambda_` in **gbpplanner** + pub measurement_precision: Matrix, + /// Stored linearisation point + /// called `X_` in **gbpplanner**, they use `Eigen::MatrixXd` instead + pub linearisation_point: Vector, + /// Strength of the factor. Called `sigma` in gbpplanner. + /// The factor precision $Lambda = sigma^-2 * Identify$ + pub strength: Float, + + /// Cached value of the factors jacobian function + /// called `J_` in **gbpplanner** + /// TODO: wrap in Option<> + pub cached_jacobian: Matrix, + + /// Cached value of the factors jacobian function + /// called `h_` in **gbpplanner** + /// TODO: wrap in Option<> + pub cached_measurement: Vector, + /// Set to true after the first call to self.update() + initialized: bool, +} + +impl FactorState { + /// Create a new [`FactorState`] + fn new(initial_measurement: Vector, strength: Float, neighbor_amount: usize) -> Self { + // Initialise precision of the measurement function + // this->meas_model_lambda_ = Eigen::MatrixXd::Identity(z_.rows(), z_.rows()) / + // pow(sigma,2.); + let measurement_precision = + Matrix::::eye(initial_measurement.len()) / Float::powi(strength, 2); + + Self { + initial_measurement, + measurement_precision, + linearisation_point: Vector::::zeros(DOFS * neighbor_amount), + strength, + cached_jacobian: array![[]], + cached_measurement: array![], + initialized: false, + } + } +} + + +impl FactorGraphNode for Factor { + fn remove_connection_to( + &mut self, + factorgraph_id: super::factorgraph::FactorGraphId, + ) -> Result<(), super::factorgraph::RemoveConnectionToError> { + let connections_before = self.inbox.len(); + self.inbox + .retain(|variable_id, v| variable_id.factorgraph_id != factorgraph_id); + let connections_after = self.inbox.len(); + + let no_connections_removed = connections_before == connections_after; + if no_connections_removed { + Err(super::factorgraph::RemoveConnectionToError) + } else { + Ok(()) + } + } + + #[inline] + fn messages_sent(&self) -> usize { + self.message_count.sent + } + + #[inline] + fn messages_received(&self) -> usize { + self.message_count.received + } + + #[inline(always)] + fn reset_message_count(&mut self) { + self.message_count.reset(); + } +} diff --git a/crates/gbpplanner-rs/src/factorgraph/factor/obstacle.rs b/crates/gbpplanner-rs/src/factorgraph/factor/obstacle.rs new file mode 100644 index 00000000..9c9cee96 --- /dev/null +++ b/crates/gbpplanner-rs/src/factorgraph/factor/obstacle.rs @@ -0,0 +1,127 @@ +//! Obstacle factor + +use bevy::render::texture::Image; +use gbp_linalg::{prelude::*, Float}; + +use super::Model; + +#[derive(Clone)] +pub struct ObstacleFactor { + /// The signed distance field of the environment + obstacle_sdf: &'static Image, + /// Copy of the `WORLD_SZ` setting from **gbpplanner**, that we store a copy + /// of here since `ObstacleFactor` needs this information to calculate + /// `.jacobian_delta()` and `.measurement()` + world_size: Float, +} + +impl std::fmt::Debug for ObstacleFactor { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.debug_struct("ObstacleFactor") + // .field("obstacle_sdf", &self.obstacle_sdf) + .field("world_size", &self.world_size) + .finish() + } +} + +impl ObstacleFactor { + pub const NEIGHBORS: usize = 1; + + /// Creates a new [`ObstacleFactor`]. + #[must_use] + pub fn new(obstacle_sdf: &'static Image, world_size: Float) -> Self { + Self { + obstacle_sdf, + world_size, + } + } +} + +impl Model for ObstacleFactor { + #[inline] + fn name(&self) -> &'static str { + "ObstacleFactor" + } + + #[inline] + fn jacobian(&mut self, state: &FactorState, x: &Vector) -> Matrix { + // Same as PoseFactor + // TODO: change to not clone x + self.first_order_jacobian(state, x.clone()) + } + + fn measure(&mut self, _state: &FactorState, x: &Vector) -> Vector { + // pretty_print_vector!(x); + debug_assert!(x.len() >= 2, "x.len() = {}", x.len()); + // White areas are obstacles, so h(0) should return a 1 for these regions. + let scale = self.obstacle_sdf.width() as Float / self.world_size; + // let offset = (self.world_size / 2.0) as usize; + let offset = self.world_size / 2.0; + if (x[0] + offset) * scale > self.obstacle_sdf.width() as Float { + // warn!( + // "x[0] + offset = {}, scale = {}, width = {}", + // (x[0] + offset) * scale, + // scale, + // self.obstacle_sdf.width() + // ); + return array![0.0]; + } + if (x[1] + offset) * scale > self.obstacle_sdf.height() as Float { + // warn!( + // "x[1] + offset = {}, scale = {}, height = {}", + // (x[1] + offset) * scale, + // scale, + // self.obstacle_sdf.height() + // ); + return array![0.0]; + } + // dbg!(offset); + let pixel_x = ((x[0] + offset) * scale) as u32; + let pixel_y = ((x[1] + offset) * scale) as u32; + // println!("pixel_x = {}, pixel_y = {}", pixel_x, pixel_y); + // dbg!(pixel_x, pixel_y); + // assert_eq!((self.obstacle_sdf.width() * self.obstacle_sdf.height() * 4) as + // usize, self.obstacle_sdf.data.len()); multiply by 4 because the image + // is in RGBA format, and we simply use th R channel to determine value, + // as the image is grayscale + // TODO: assert that the image's data is laid out in row-major order + let linear_index = ((self.obstacle_sdf.width() * pixel_y + pixel_x) * 4) as usize; + if linear_index >= self.obstacle_sdf.data.len() { + warn!( + "linear_index = {}, obstacle_sdf.data.len() = {}", + linear_index, + self.obstacle_sdf.data.len() + ); + return array![0.0]; + } + let red = self.obstacle_sdf.data[linear_index]; + // NOTE: do 1.0 - red to invert the value, as the obstacle sdf is white where + // there are obstacles in gbpplanner, they do not do the inversion here, + // but instead invert the entire image, when they load it from disk. + let hsv_value = 1.0 - red as Float / 255.0; + // let hsv_value = pixel as Float / 255.0; + // if hsv_value <= 0.5 { + // println!("image(x={}, y={}).z {} (scale = {})", pixel_x, pixel_y, + // hsv_value, scale); } + // dbg!(hsv_value); + + // println!("hsv_value = {}", hsv_value); + + array![hsv_value] + } + + #[inline(always)] + fn jacobian_delta(&self) -> Float { + self.world_size / self.obstacle_sdf.width() as Float + } + + #[inline(always)] + fn skip(&mut self, _state: &FactorState) -> bool { + false + } + + #[inline(always)] + fn linear(&self) -> bool { + false + } +} diff --git a/crates/gbpplanner-rs/src/factorgraph/factor/pose.rs b/crates/gbpplanner-rs/src/factorgraph/factor/pose.rs new file mode 100644 index 00000000..c693e5c0 --- /dev/null +++ b/crates/gbpplanner-rs/src/factorgraph/factor/pose.rs @@ -0,0 +1,37 @@ +use gbp_linalg::prelude::*; + +#[derive(Debug)] +pub struct PoseFactor; + +impl Model for PoseFactor { + #[inline(always)] + fn name(&self) -> &'static str { + "PoseFactor" + } + + /// Default jacobian is the first order taylor series jacobian + fn jacobian(&mut self, state: &FactorState, x: &Vector) -> Matrix { + self.first_order_jacobian(state, x.clone()) + } + + /// Default measurement function is the identity function + #[inline(always)] + fn measure(&mut self, _state: &FactorState, x: &Vector) -> Vector { + x.clone() + } + + #[inline(always)] + fn skip(&mut self, _state: &FactorState) -> bool { + false + } + + #[inline(always)] + fn jacobian_delta(&self) -> Float { + 1e-8 + } + + #[inline(always)] + fn linear(&self) -> bool { + false + } +} diff --git a/crates/gbpplanner-rs/src/factorgraph/factorgraph.rs b/crates/gbpplanner-rs/src/factorgraph/factorgraph.rs new file mode 100644 index 00000000..1cc24ab0 --- /dev/null +++ b/crates/gbpplanner-rs/src/factorgraph/factorgraph.rs @@ -0,0 +1,308 @@ +use bevy::{ + ecs::{component::Component, entity::Entity}, + log::debug, +}; +use petgraph::Undirected; + +use super::{ + factor::Factor, + node::{Node, NodeKind}, + variable::Variable, +}; + +/// type alias used to represent the id of the factorgraph +/// Since we use **Bevy** we can use the `Entity` id of the whatever entity the +/// the factorgraph is attached to as a Component, as its unique identifier. +pub type FactorGraphId = Entity; + +/// The type used to represent indices into the nodes of the factorgraph. +/// This is just a type alias for `petgraph::graph::NodeIndex`, but +/// we make an alias for it here, such that it is easier to use the same +/// index type across modules, as the various node index types `petgraph` +/// are not interchangeable. +pub type NodeIndex = petgraph::stable_graph::NodeIndex; +/// The type used to represent indices into the nodes of the factorgraph. +pub type EdgeIndex = petgraph::stable_graph::EdgeIndex; +/// A factorgraph is an undirected graph +pub type Graph = petgraph::stable_graph::StableGraph; + +/// A newtype used to enforce type safety of the indices of the factors in the +/// factorgraph. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, derive_more::From)] +pub struct FactorIndex(pub NodeIndex); +/// A newtype used to enforce type safety of the indices of the variables in the +/// factorgraph. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, derive_more::From)] +pub struct VariableIndex(pub NodeIndex); + +/// A factor graph is a bipartite graph consisting of two types of nodes: +/// factors and variables. +#[derive(Component, Debug)] +// #[cfg_attr(feature = "bevy", derive(Component))] +pub struct FactorGraph { + /// The id of the factorgraph. We store a copy of it here, for convenience. + /// **Invariants**: + /// - The id of the factorgraph is unique among all factorgraphs in the + /// system. + /// - The id does not change during the lifetime of the factorgraph. + id: FactorGraphId, + /// The underlying graph data structure + graph: Graph, + /// In **gbpplanner** the sequence in which variables are inserted/created + /// in the graph is meaningful. `self.graph` does not capture this + /// ordering, so we use an extra vector to manage the order in which + /// variables are inserted/removed from the graph. + /// + /// **IMPORTANT** we have to manually ensure the invariant that + /// `self.graph` and this field is consistent at all time. + variable_indices: Vec, + /// List of indices of the factors in the graph. Order is not important. + /// Used to speed up iteration over factors. + factor_indices: Vec, + + /// List of indices of the interrobot factors in the graph. Order is not + /// important. Used to speed up iteration over interrobot factors. + /// When querying for number of external messages sent + interrobot_factor_indices: Vec, +} + +impl FactorGraph { + /// Construct a new empty factorgraph with a given id + #[must_use] + pub fn new(id: FactorGraphId) -> Self { + Self { + id, + graph: Graph::with_capacity(0, 0), + variable_indices: Vec::new(), + factor_indices: Vec::new(), + interrobot_factor_indices: Vec::new(), + } + } + + /// Construct a new empty factorgraph with the specified capacity + /// for nodes and edges. + #[must_use] + pub fn with_capacity(id: FactorGraphId, nodes: usize, edges: usize) -> Self { + Self { + id, + graph: Graph::with_capacity(nodes, edges), + variable_indices: Vec::with_capacity(nodes), + factor_indices: Vec::with_capacity(edges), + interrobot_factor_indices: Vec::new(), + } + } + + /// Returns the `FactorGraphId` of the factorgraph + #[inline(always)] + pub fn id(&self) -> FactorGraphId { + self.id + } + + /// Adds a variable to the factorgraph + /// Returns the index of the variable in the factorgraph + pub fn add_variable(&mut self, variable: Variable) -> VariableIndex { + let node = Node::new(self.id, NodeKind::Variable(variable)); + let node_index = self.graph.add_node(node); + self.variable_indices.push(node_index); + self.graph[node_index] + .as_variable_mut() + .expect("just added the variable to the graph in the previous statement") + .set_node_index(node_index); + debug!( + "added a variable with node_index: {:?} to factorgraph: {:?}", + node_index, self.id + ); + node_index.into() + } + + pub fn add_factor(&mut self, factor: Factor) -> FactorIndex { + let is_interrobot = factor.is_inter_robot(); + let node = Node::new(self.id, NodeKind::Factor(factor)); + let node_index = self.graph.add_node(node); + self.graph[node_index] + .as_factor_mut() + .expect("just added the factor to the graph in the previous statement") + // .tap(|f| { + // info!( + // "adding a '{}' factor with node_index: {:?} to factorgraph: {:?}", + // f.variant(), + // node_index, + // self.id + // ); + // }) + .set_node_index(node_index); + self.factor_indices.push(node_index); + if is_interrobot { + self.interrobot_factor_indices.push(node_index); + } + + node_index.into() + } + + /// Number of nodes in the factorgraph + /// + /// **Computes in O(1) time** + #[inline] + pub fn len(&self) -> usize { + self.graph.node_count() + } + + /// A count over the number of variables and factors in the factorgraph + /// + /// **Computes in O(1) time** + #[must_use] + pub fn node_count(&self) -> NodeCount { + NodeCount { + factors: self.factor_indices.len(), + variables: self.variable_indices.len(), + } + } +} + +/// Record type used to keep track of how many factors and variables +/// there are in the factorgraph. We keep track of these counts internally in +/// the factorgraph, such a query for the counts, is **O(1)**. +#[derive(Debug, Clone, Copy, Default)] +pub struct NodeCount { + pub factors: usize, + pub variables: usize, +} + +/// Iterator over the factors in the factorgraph. +/// +/// Iterator element type is `(FactorIndex, &'a Factor)`. +/// +/// Created with [`.factors()`][1] +/// +/// [1]: struct.FactorGraph.html#method.factors +pub struct Factors<'a> { + graph: &'a Graph, + factor_indices: std::slice::Iter<'a, NodeIndex>, +} + +impl<'a> Factors<'a> { + #[must_use] + pub fn new(graph: &'a Graph, factor_indices: &'a [NodeIndex]) -> Self { + Self { + graph, + factor_indices: factor_indices.iter(), + } + } +} + +impl FactorGraph { + /// Returns an iterator over the factors in the factorgraph. + #[inline] + #[must_use] + pub fn factors(&self) -> Factors<'_> { + Factors::new(&self.graph, &self.factor_indices) + } +} + +impl<'a> Iterator for Factors<'a> { + type Item = (NodeIndex, &'a Factor); + + fn next(&mut self) -> Option { + let &index = self.factor_indices.next()?; + let node = &self.graph[index]; + node.as_factor().map(|factor| (index, factor)) + } +} + +/// Iterator over the variables in the factorgraph. +/// +/// Iterator element type is `(VariableIndex, &'a Variable)`. +/// +/// Created with [`.variables()`][1] +/// +/// [1]: struct.FactorGraph.html#method.variables +pub struct Variables<'a> { + graph: &'a Graph, + variable_indices: std::slice::Iter<'a, NodeIndex>, +} + +impl<'a> Variables<'a> { + pub fn new(graph: &'a Graph, variable_indices: &'a [NodeIndex]) -> Self { + Self { + graph, + variable_indices: variable_indices.iter(), + } + } +} + +impl<'a> Iterator for Variables<'a> { + type Item = (VariableIndex, &'a Variable); + + fn next(&mut self) -> Option { + let &index = self.variable_indices.next()?; + let node = &self.graph[index]; + node.as_variable() + .map(|variable| (VariableIndex(index), variable)) + } +} + +impl FactorGraph { + /// Returns an iterator over the variables in the factorgraph. + #[inline] + #[must_use] + pub fn variables(&self) -> Variables<'_> { + Variables::new(&self.graph, &self.variable_indices) + } +} + +/// Iterator over the interrobot factors in the factorgraph. +/// +/// Iterator element type is `(FactorIndex, &'a InterRobotFactor)`. +/// +/// Created with [`.inter_robot_factors()`][1] +pub struct InterRobotFactors<'a> { + graph: &'a Graph, + factor_indices: std::slice::Iter<'a, NodeIndex>, +} + +impl<'a> InterRobotFactors<'a> { + pub fn new(graph: &'a Graph, factor_indices: &'a [NodeIndex]) -> Self { + Self { + graph, + factor_indices: factor_indices.iter(), + } + } +} + +impl<'a> Iterator for InterRobotFactors<'a> { + type Item = (NodeIndex, &'a InterRobotFactor); + + fn next(&mut self) -> Option { + let &index = self.factor_indices.next()?; + let node = &self.graph[index]; + node.as_factor().map(|factor| (index, factor)) + } +} + +impl FactorGraph { + #[inline] + #[must_use] + pub fn inter_robot_factors(&self) -> InterRobotFactors<'_> { + InterRobotFactors::new(&self.graph, &self.interrobot_factor_indices) + } +} + +impl std::ops::Index for FactorGraph { + type Output = Factor; + + // type Output = Option; + + fn index(&self, index: FactorIndex) -> &Self::Output { + self.graph[index.into()].as_factor() + } +} + +impl std::ops::Index for FactorGraph { + type Output = Variable; + + fn index(&self, index: VariableIndex) -> &Self::Output { + self.graph[index.into()] + .as_variable() + .expect("a variable index points to a variable node in the graph") + } +} diff --git a/crates/gbpplanner-rs/src/factorgraph/graphviz.rs b/crates/gbpplanner-rs/src/factorgraph/graphviz.rs new file mode 100644 index 00000000..e69de29b diff --git a/crates/gbpplanner-rs/src/factorgraph/id.rs b/crates/gbpplanner-rs/src/factorgraph/id.rs new file mode 100644 index 00000000..742a4ade --- /dev/null +++ b/crates/gbpplanner-rs/src/factorgraph/id.rs @@ -0,0 +1,133 @@ +use super::factorgraph::{FactorGraphId, FactorIndex, VariableIndex}; + +/// Unique identifier of a factor in the world. +#[derive(Debug, Clone, Copy, PartialEq, Eq, derive_more::Display)] +#[display(fmt = "{:?}-{}", factorgraph_id, "factor_index.0.index()")] +pub struct FactorId { + /// The id of the factorgraph that the factor belongs to. + pub factorgraph_id: FactorGraphId, + /// The index of the factor in the factorgraph. + pub factor_index: FactorIndex, +} + +impl FactorId { + /// Create a new `FactorId`. + #[must_use] + pub fn new(factorgraph_id: FactorGraphId, factor_index: FactorIndex) -> Self { + Self { + factorgraph_id, + factor_index, + } + } +} + +impl std::cmp::PartialOrd for FactorId { + /// Returns `Some(std::cmp::ordering::Equal)` if `self` and `other` are + /// equal, `Some(std::cmp::ordering::Less)` if `other.factorgraph_id` is + /// greater than `self.factorgraph_id`, or if the factors belong to the + /// same factorgraph and `other.factor_index` is greater than + /// `self.factor_index`. Returns `Some(std::cmp::ordering::Greater)` + /// otherwise. + /// + /// # NOTE + /// + /// This ordering is really important for the **gbpplanner** algorithm, to + /// work correctly. + fn partial_cmp(&self, other: &Self) -> Option { + if self.factorgraph_id < other.factorgraph_id { + Some(std::cmp::Ordering::Less) + } else if self.factorgraph_id == other.factorgraph_id + && self.factor_index.0 < other.factor_index.0 + { + Some(std::cmp::Ordering::Less) + } else if self.factorgraph_id == other.factorgraph_id + && self.factor_index == other.factor_index + { + Some(std::cmp::Ordering::Equal) + } else { + Some(std::cmp::Ordering::Greater) + } + } +} + +impl std::cmp::Ord for FactorId { + #[inline] + fn cmp(&self, other: &Self) -> std::cmp::Ordering { + self.partial_cmp(other) + .expect("every branch in Self::partial_cmp() returns Some()") + } +} + +// implement PartialEq and Eq manually +// impl std::cmp::PartialEq for FactorId { +// fn eq(&self, other: &Self) -> bool { +// self.factorgraph_id == other.factorgraph_id && self.factor_index == +// other.factor_index } +// } +// +// impl std::cmp::Eq for FactorId {} + +/// Unique identifier of a variable in the world. +#[derive(Debug, Clone, Copy, PartialEq, Eq, derive_more::Display)] +#[display(fmt = "{:?}-{}", factorgraph_id, "variable_index.0.index()")] +pub struct VariableId { + /// The id of the factorgraph that the variable belongs to. + pub factorgraph_id: FactorGraphId, + /// The index of the variable in the factorgraph. + pub variable_index: VariableIndex, +} + +impl std::cmp::PartialOrd for VariableId { + /// Returns `Some(std::cmp::ordering::Equal)` if `self` and `other` are + /// equal, `Some(std::cmp::ordering::Less)` if `other.factorgraph_id` is + /// greater than `self.factorgraph_id`, or if the factors belong to the + /// same factorgraph and `other.variable_index` is greater than + /// `self.variable_index`. Returns `Some(std::cmp::ordering::Greater)` + /// otherwise. + /// + /// # NOTE + /// + /// This ordering is really important for the **gbpplanner** algorithm, to + /// work correctly. + fn partial_cmp(&self, other: &Self) -> Option { + if self.factorgraph_id < other.factorgraph_id { + Some(std::cmp::Ordering::Less) + } else if self.factorgraph_id == other.factorgraph_id + && self.variable_index.0 < other.variable_index.0 + { + Some(std::cmp::Ordering::Less) + } else if self.factorgraph_id == other.factorgraph_id + && self.variable_index == other.variable_index + { + Some(std::cmp::Ordering::Equal) + } else { + Some(std::cmp::Ordering::Greater) + } + } +} + +impl std::cmp::Ord for VariableId { + fn cmp(&self, other: &Self) -> std::cmp::Ordering { + self.partial_cmp(other) + .expect("every branch in Self::partial_cmp() returns Some()") + } +} + +// implement PartialEq and Eq manually +// impl std::cmp::PartialEq for VariableId { +// fn eq(&self, other: &Self) -> bool { +// self.factorgraph_id == other.factorgraph_id && self.variable_index == +// other.variable_index } +// } +// impl std::cmp::Eq for VariableId {} + +impl VariableId { + /// Create a new `VariableId`. + #[must_use] + pub fn new(factorgraph_id: FactorGraphId, variable_index: VariableIndex) -> Self { + Self { + factorgraph_id, + variable_index, + } + } +} diff --git a/crates/gbpplanner-rs/src/factorgraph/message.rs b/crates/gbpplanner-rs/src/factorgraph/message.rs new file mode 100644 index 00000000..c3fb508c --- /dev/null +++ b/crates/gbpplanner-rs/src/factorgraph/message.rs @@ -0,0 +1,165 @@ +#![deny(missing_docs)] +//! Message module. +//! +//! Contains the message struct that variables and factors send between each +//! other in the factorgraph. + +use std::collections::BTreeMap; + +use gbp_linalg::prelude::*; + +use super::{ + id::{FactorId, VariableId}, + DOFS, +}; + +/// Payload of a message +#[derive(Debug, Clone)] +pub struct Payload { + /// Information vector of a multivariate gaussian + pub information_factor: Vector, + /// Precision matrix of a multivariate gaussian + pub precision_matrix: Matrix, + /// Mean vector of a multivariate gaussian + /// The mean can be computed from the information vector and the precision + /// matrix but the mean vector is stored here to trade some memory, for + /// having to compute it multiple times + pub mean: Vector, +} + +/// Newtype used to make prevent the caller of `Message::new()` from mixing up +/// the information vector and mean vector argument. +pub struct InformationVec(pub Vector); +/// Newtype used to make make it clear for the caller that the matrix argument +/// for `Message::new()` has to be a precision matrix, and not a covariance. +pub struct PrecisionMatrix(pub Matrix); +/// Newtype used to make prevent the caller of `Message::new()` from mixing up +/// the information vector and mean vector argument. +pub struct Mean(pub Vector); + +#[derive(Debug, Clone)] +pub struct Message { + payload: Option, +} + +impl Message { + /// Returns a reference to the mean + /// or `None` if the message is empty. + #[inline] + pub fn mean(&self) -> Option<&Vector> { + self.payload.as_ref().map(|payload| &payload.mean) + } + + /// Returns a reference to the precision matrix + /// or `None` if the message is empty. + #[inline] + pub fn precision_matrix(&self) -> Option<&Matrix> { + self.payload + .as_ref() + .map(|payload| &payload.precision_matrix) + } + + /// Returns a reference to the information vector + /// or `None` if the message is empty. + #[inline] + pub fn information_vector(&self) -> Option<&Vector> { + self.payload + .as_ref() + .map(|payload| &payload.information_factor) + } + + /// Returns `true` if the message is [`Empty`]. + #[inline] + pub fn is_empty(&self) -> bool { + self.payload.is_none() + } + + /// Take the inner `MultivariateNormal` from the message. + /// Leaving the message in an empty state. + #[inline] + pub fn take(&mut self) -> Option { + self.payload.take() + } + + /// Access the payload of the message. + /// Returns `None` if the message is empty. + #[inline] + pub fn payload(&self) -> Option<&Payload> { + self.payload.as_ref() + } + + /// Create an empty message + #[must_use] + pub fn empty() -> Self { + Self { + payload: Some(Payload { + information_factor: Vector::::zeros(DOFS), + precision_matrix: Matrix::::zeros((DOFS, DOFS)), + mean: Vector::::zeros(DOFS), + }), + } + } + + /// Create a new message + /// + /// # Panics + /// + /// - if `eta.0.len() != DOFS` + /// - if `lam.0.nrows() != DOFS` + /// - if `lam.0.ncols() != DOFS` + /// - if `mu.0.len() != DOFS` + #[must_use] + pub fn new(eta: InformationVec, lam: PrecisionMatrix, mu: Mean) -> Self { + debug_assert_eq!(eta.0.len(), DOFS); + debug_assert_eq!(lam.0.nrows(), DOFS); + debug_assert_eq!(lam.0.ncols(), DOFS); + debug_assert_eq!(mu.0.len(), DOFS); + + Self { + payload: Some(Payload { + information_factor: eta.0, + precision_matrix: lam.0, + mean: mu.0, + }), + } + } +} + +/// A message from a factor to a variable +#[derive(Debug)] +pub struct VariableToFactorMessage { + /// The factor that sends the message + pub from: VariableId, + /// The variable that receives the message + pub to: FactorId, + /// The message + pub message: Message, +} + +/// A message from a variable to a factor +#[derive(Debug)] +pub struct FactorToVariableMessage { + /// The variable that sends the message + pub from: FactorId, + /// The factor that receives the message + pub to: VariableId, + /// The message + pub message: Message, +} + +// pub type MessagesFromVariables = BTreeMap; +// pub type MessagesFromFactors = BTreeMap; + +/// Type alias for a map of messages from a factor to a variable +/// A (`BTreeMap`)[std::collections::BTreeMap] is used, instead of a +/// (`HashMap`)[std::collections::HashMap] to ensure that the messages are +/// stored in a consistent order This is necessary for the **gbpplanner** +/// algorithm to work correctly. +pub type MessagesToVariables = BTreeMap; + +/// Type alias for a map of messages from a variable to a factor +/// A (`BTreeMap`)[std::collections::BTreeMap] is used, instead of a +/// (`HashMap`)[std::collections::HashMap] to ensure that the messages are +/// stored in a consistent order This is necessary for the **gbpplanner** +/// algorithm to work correctly. +pub type MessagesToFactors = BTreeMap; diff --git a/crates/gbpplanner-rs/src/factorgraph/mod.rs b/crates/gbpplanner-rs/src/factorgraph/mod.rs new file mode 100644 index 00000000..4e860ad2 --- /dev/null +++ b/crates/gbpplanner-rs/src/factorgraph/mod.rs @@ -0,0 +1,51 @@ +#![deny(missing_docs)] +//! ... + +use bevy::ecs::entity::Entity; +use petgraph::Undirected; + +mod factor; +mod factorgraph; +mod graphviz; +mod id; +mod message; +mod node; +mod variable; + +/// Degrees of Freedom of the ground robot. +/// The robot has 4 degrees, of freedom: +/// 1. position.x +/// 2. position.y +/// 3. velocity.x +/// 4. velocity.y +/// [x, y, x', y'] +pub const DOFS: u32 = 4; + +/// prelude module bringing entire public API into score +pub mod prelude { + pub use super::{factorgraph::FactorGraph, message::Message, DOFS}; +} + +#[derive(Debug, Default, Clone, Copy)] +pub(super) struct MessageCount { + pub sent: usize, + pub received: usize, +} + +impl MessageCount { + pub fn reset(&mut self) { + self.sent = 0; + self.received = 0; + } +} + +impl std::ops::Add for MessageCount { + type Output = Self; + + fn add(self, rhs: Self) -> Self::Output { + MessageCount { + sent: self.sent + rhs.sent, + received: self.received + rhs.received, + } + } +} diff --git a/crates/gbpplanner-rs/src/factorgraph/node.rs b/crates/gbpplanner-rs/src/factorgraph/node.rs new file mode 100644 index 00000000..02b23269 --- /dev/null +++ b/crates/gbpplanner-rs/src/factorgraph/node.rs @@ -0,0 +1,101 @@ +use super::{factorgraph::FactorGraphId, variable::Variable}; + +#[derive(Debug, derive_more::Display)] +#[display(fmt = "no connection to the given factorgraph")] +pub struct RemoveConnectionToError; + +impl std::error::Error for RemoveConnectionToError {} + +pub(super) trait FactorGraphNode { + #[must_use] + fn remove_connection_to( + &mut self, + factorgraph_id: FactorGraphId, + ) -> Result<(), RemoveConnectionToError>; + + fn messages_sent(&self) -> usize; + fn messages_received(&self) -> usize; + + fn reset_message_count(&mut self); +} + +#[derive(Debug, derive_more::IsVariant)] +pub enum NodeKind { + Factor(Factor), + // TODO: wrap in Box<> + Variable(Variable), +} + +#[derive(Debug)] +pub struct Node { + factorgraph_id: FactorGraphId, + kind: NodeKind, +} + +impl Node { + /// Construct a new node + pub fn new(factorgraph_id: FactorGraphId, kind: NodeKind) -> Self { + Self { + factorgraph_id, + kind, + } + } + + /// Returns `true` if the node is [`Factor`]. + /// + /// [`Factor`]: Node::Factor + #[must_use] + #[inline] + pub fn is_factor(&self) -> bool { + self.kind.is_factor() + } + + /// Returns `Some(&Factor)` if the node]s variant is [`Factor`], otherwise + /// `None`. + pub fn as_factor(&self) -> Option<&Factor> { + if let NodeKind::Factor(ref v) = self.kind { + Some(v) + } else { + None + } + } + + /// Returns `Some(&mut Factor)` if the node]s variant is [`Factor`], + /// otherwise `None`. + pub fn as_factor_mut(&mut self) -> Option<&mut Factor> { + if let NodeKind::Factor(ref mut v) = self.kind { + Some(v) + } else { + None + } + } + + /// Returns `true` if the node is [`Variable`]. + /// + /// [`Variable`]: Node::Variable + #[must_use] + #[inline] + pub fn is_variable(&self) -> bool { + self.kind.is_variable() + } + + /// Returns `Some(&Variable)` if the node]s variant is [`Variable`], + /// otherwise `None`. + pub fn as_variable(&self) -> Option<&Variable> { + if let NodeKind::Variable(ref v) = self.kind { + Some(v) + } else { + None + } + } + + /// Returns `Some(&mut Variable)` if the node]s variant is [`Variable`], + /// otherwise `None`. + pub fn as_variable_mut(&mut self) -> Option<&mut Variable> { + if let NodeKind::Variable(ref mut v) = self.kind { + Some(v) + } else { + None + } + } +} diff --git a/crates/gbpplanner-rs/src/factorgraph/variable.rs b/crates/gbpplanner-rs/src/factorgraph/variable.rs new file mode 100644 index 00000000..3439c472 --- /dev/null +++ b/crates/gbpplanner-rs/src/factorgraph/variable.rs @@ -0,0 +1,393 @@ +#![deny(missing_docs)] + +use bevy::log::{debug, error}; +use gbp_linalg::{pretty_print_matrix, pretty_print_vector, Float, Matrix, Vector}; +use ndarray_inverse::Inverse; +use tap::Tap; + +use super::{ + factorgraph::{ + FactorGraphNode, FactorId, MessageCount, MessagesFromVariables, MessagesToFactors, + VariableId, + }, + message::{InformationVec, Mean, Message, PrecisionMatrix}, +}; +use crate::{ + escape_codes::*, + planner::{factorgraph::VariableIndex, NodeIndex}, + pretty_print_line, pretty_print_message, pretty_print_subtitle, pretty_print_title, +}; + +#[derive(Debug, Clone)] +pub struct VariablePrior { + information_vector: Vector, + precision_matrix: Matrix, +} + +impl VariablePrior { + #[must_use] + fn new(information_vector: Vector, precision_matrix: Matrix) -> Self { + Self { + information_vector, + precision_matrix, + } + } +} + +#[derive(Debug, Clone)] +pub struct VariableBelief { + pub information_vector: Vector, + pub precision_matrix: Matrix, + pub mean: Vector, + pub covariance_matrix: Matrix, + /// Flag to indicate if the variable's covariance is finite, i.e. it does + /// not contain NaNs or Infs In gbpplanner it is used to control if a + /// variable can be rendered. + valid: bool, +} + +impl VariableBelief { + fn new( + information_vector: Vector, + precision_matrix: Matrix, + mean: Vector, + covariance_matrix: Matrix, + ) -> Self { + let valid = covariance_matrix.iter().all(|x| x.is_finite()); + Self { + information_vector, + precision_matrix, + mean, + covariance_matrix, + valid, + } + } +} + +impl From for Message { + fn from(value: VariableBelief) -> Self { + Message::new( + InformationVec(value.information_vector), + PrecisionMatrix(value.precision_matrix), + Mean(value.mean), + ) + } +} + +/// A variable in the factor graph. +#[derive(Debug)] +pub struct Variable { + pub prior: VariablePrior, + pub belief: VariableBelief, + + // / Flag to indicate if the variable's covariance is finite, i.e. it does + // / not contain NaNs or Infs In gbpplanner it is used to control if a + // / variable can be rendered. + // pub valid: bool, + /// Mailbox for incoming message storage + pub inbox: MessagesToFactors, + + /// index + node_index: Option, + + message_count: MessageCount, +} + +impl Variable { + /// Returns the node index of the variable + /// + /// # Panics + /// + /// Panics if the node index has not been set, which should not happen. + #[inline] + pub fn node_index(&self) -> NodeIndex { + if self.node_index.is_none() { + panic!("The node index has not been set"); + } + self.node_index.unwrap() + } + + /// Returns the variables belief about its position + #[inline] + pub fn estimated_position(&self) -> [Float; 2] { + [self.belief.mean[0], self.belief.mean[1]] + } + + /// Returns the variables belief about its velocity + #[inline] + pub fn estimated_velocity(&self) -> [Float; 2] { + [self.belief.mean[2], self.belief.mean[3]] + } + + #[must_use] + pub fn new( + prior_mean: Vector, + mut prior_precision_matrix: Matrix, + dofs: usize, + ) -> Self { + if !prior_precision_matrix.iter().all(|x| x.is_finite()) { + prior_precision_matrix.fill(0.0); + } + + let eta_prior = prior_precision_matrix.dot(&prior_mean); + + let sigma = prior_precision_matrix + .inv() + .unwrap_or_else(|| Matrix::::zeros((dofs, dofs))); + let eta = eta_prior.clone(); + let lam = prior_precision_matrix.clone(); + + Self { + prior: VariablePrior::new(eta_prior, prior_precision_matrix), + belief: VariableBelief::new(eta, lam, prior_mean, sigma), + inbox: MessagesToFactors::new(), + node_index: None, + message_count: MessageCount::default(), + } + } + + pub fn set_node_index(&mut self, index: NodeIndex) { + if self.node_index.is_some() { + panic!("The node index is already set"); + } + self.node_index = Some(index); + } + + // pub fn new(mut mu_prior: Vector, mut + + // pub fn set_node_index(&mut self, node_index: NodeIndex) { + // match self.node_index { + // Some(_) => panic!("The node index is already set"), + // None => self.node_index = Some(node_index), + // } + // } + // + // pub fn get_node_index(&self) -> NodeIndex { + // match self.node_index { + // Some(node_index) => node_index, + // None => panic!("The node index has not been set"), + // } + // } + + pub fn receive_message_from(&mut self, from: FactorId, message: Message) { + debug!("variable ? received message from {:?}", from); + if message.is_empty() { + // warn!("Empty message received from factor {:?}", from); + } + let _ = self.inbox.insert(from, message); + self.message_count.received += 1; + } + + // TODO: why never used? + #[inline] + pub fn read_message_from(&mut self, from: FactorId) -> Option<&Message> { + self.inbox.get(&from) + } + + /// Change the prior of the variable. + /// It updates the belief of the variable. + /// The prior acts as the pose factor + /// Called `Variable::change_variable_prior` in **gbpplanner** + pub fn change_prior(&mut self, mean: Vector) -> MessagesFromVariables { + // let subtitle = format!("{}{}{}", RED, "Changing prior", RESET); + // pretty_print_subtitle!(subtitle); + // pretty_print_matrix!(&self.prior.lambda); + // pretty_print_vector!(&mean); + self.prior.information_vector = self.prior.precision_matrix.dot(&mean); + // pretty_print_vector!(&self.prior.eta); + // pretty_print_line!(); + // self.eta_prior = self.lam_prior.dot(&mean); + self.belief.mean = mean; + // dbg!(&self.mu); + + // FIXME: forgot this line in the original code + // this->belief_ = Message {this->eta_, this->lam_, this->mu_}; + + let messages: MessagesFromVariables = self + .inbox + .keys() + .map(|factor_id| (*factor_id, self.belief.clone().into())) + .collect(); + + for message in self.inbox.values_mut() { + *message = Message::empty(self.dofs); + } + + messages + } + + pub fn prepare_message(&self) -> Message { + Message::new( + InformationVec(self.belief.information_vector.clone()), + PrecisionMatrix(self.belief.precision_matrix.clone()), + Mean(self.belief.mean.clone()), + ) + } + + // /**************************************************************************** + // *******************************/ // Variable belief update step: + // // Aggregates all the messages from its connected factors (begins with the + // prior, as this is effectively a unary factor) // The valid_ flag is + // useful for drawing the variable. // Finally the outgoing messages to + // factors is created. /**************************************************** + // *******************************************************/ + /// Variable Belief Update step (Step 1 in the GBP algorithm) + /// called `Variable::update_belief` in **gbpplanner** + pub fn update_belief_and_create_factor_responses(&mut self) -> MessagesFromVariables { + // Collect messages from all other factors, begin by "collecting message from + // pose factor prior" + self.belief.information_vector = self.prior.information_vector.clone(); + self.belief.precision_matrix = self.prior.precision_matrix.clone(); + + // let mut title = format!("{}{}{}", YELLOW, "Variable belief BEFORE update:", + // RESET); pretty_print_subtitle!(title); + // pretty_print_vector!(&self.belief.eta); + // pretty_print_matrix!(&self.belief.lambda); + // pretty_print_vector!(&self.belief.mu); + + // Go through received messages and update belief + for (_, message) in self.inbox.iter() { + let Some(payload) = message.payload() else { + // empty message + // info!("skipping empty message"); + continue; + }; + self.belief.information_vector = &self.belief.information_vector + &payload.eta; + self.belief.precision_matrix = &self.belief.precision_matrix + &payload.lam; + } + + // Update belief + // NOTE: This might not be correct, but it seems the `.inv()` method doesn't + // catch and all-zero matrix + let lam_not_zero = self.belief.precision_matrix.iter().any(|x| *x - 1e-6 > 0.0); + // println!("lam_not_zero: {}", lam_not_zero); + if lam_not_zero { + if let Some(sigma) = self.belief.precision_matrix.inv() { + // pretty_print_matrix!(&sigma); + self.belief.covariance_matrix = sigma; + self.belief.valid = self.belief.covariance_matrix.iter().all(|x| x.is_finite()); + if self.belief.valid { + self.belief.mean = self + .belief + .covariance_matrix + .dot(&self.belief.information_vector); + } else { + println!( + "{}:{},Variable covariance is not finite", + file!().split('/').last().unwrap(), + line!() + ); + } + } + } + + // let title = format!("{}{}{}", YELLOW, "Variable belief update:", RESET); + // pretty_print_subtitle!(title); + // pretty_print_vector!(&self.belief.eta); + // pretty_print_matrix!(&self.belief.lambda); + // pretty_print_vector!(&self.belief.mu); + // pretty_print_line!(); + + let messages = self + .inbox + .iter() + .map(|(&factor_id, received_message)| { + let response = received_message.payload().map_or_else( + || self.prepare_message(), + |message_from_factor| { + // pretty_print_subtitle!("BEFORE FACTOR SUBSTRACTION"); + // pretty_print_vector!(&self.belief.eta); + // pretty_print_matrix!(&self.belief.lam); + // pretty_print_vector!(&self.belief.mu); + // pretty_print_line!(); + let msg = Message::new( + InformationVec( + &self.belief.information_vector - &message_from_factor.eta, + ), + PrecisionMatrix( + &self.belief.precision_matrix - &message_from_factor.lam, + ), + Mean(&self.belief.mean - &message_from_factor.mu), + ); + // pretty_print_subtitle!("AFTER FACTOR SUBSTRACTION"); + // pretty_print_vector!(&self.belief.eta); + // pretty_print_matrix!(&self.belief.lam); + // pretty_print_vector!(&self.belief.mu); + msg + }, + ); + (factor_id, response) + }) + .collect::(); + + // messages.iter().for_each(|(factor_id, message)| { + // pretty_print_message!( + // VariableId::new( + // factor_id.get_factor_graph_id(), + // self.node_index.unwrap().into() + // ), + // factor_id, + // "" + // ); + // pretty_print_vector!(message.information_vector().unwrap()); + // pretty_print_matrix!(message.precision_matrix().unwrap()); + // pretty_print_vector!(message.mean().unwrap()); + // }); + + self.message_count.sent += messages.len(); + + messages + + // self.inbox + // .iter() + // .map(|(&factor_id, received_message)| { + // let response = Message::new( + // Eta(&self.eta - &received_message.eta), + // Lam(&self.lam - &received_message.lam), + // Mu(&self.mu - &received_message.mu), + // ); + // (factor_id, response) + // }) + // .collect() + } + + /// Returns `true` if the covariance matrix is finite, `false` otherwise. + #[inline] + pub fn finite_covariance(&self) -> bool { + self.belief.valid + } +} + +impl FactorGraphNode for Variable { + fn remove_connection_to( + &mut self, + factorgraph_id: super::factorgraph::FactorGraphId, + ) -> Result<(), super::factorgraph::RemoveConnectionToError> { + let connections_before = self.inbox.len(); + self.inbox + .retain(|factor_id, v| factor_id.factorgraph_id != factorgraph_id); + let connections_after = self.inbox.len(); + + let no_connections_removed = connections_before == connections_after; + if no_connections_removed { + Err(super::factorgraph::RemoveConnectionToError) + } else { + Ok(()) + } + } + + #[inline(always)] + fn messages_sent(&self) -> usize { + self.message_count.sent + } + + #[inline(always)] + fn messages_received(&self) -> usize { + self.message_count.received + } + + #[inline(always)] + fn reset_message_count(&mut self) { + self.message_count.reset(); + } +} From 739d53a22329a726486f28c303d3b4efef81551c Mon Sep 17 00:00:00 2001 From: kpbaks Date: Sat, 13 Apr 2024 18:02:49 +0200 Subject: [PATCH 2/6] chore: squash me --- .pre-commit-config.yaml | 2 - Cargo.lock | 119 +++++++- Cargo.toml | 2 +- crates/bevy_notify/src/lib.rs | 3 +- crates/gbp_linalg/src/lib.rs | 2 + crates/gbpplanner-rs/Cargo.toml | 11 +- crates/gbpplanner-rs/src/bevy_utils.rs | 23 ++ crates/gbpplanner-rs/src/bin/console.rs | 2 + crates/gbpplanner-rs/src/bin/derive_more.rs | 18 -- crates/gbpplanner-rs/src/bin/draw2d.rs | 2 + crates/gbpplanner-rs/src/bin/gizmos.rs | 2 +- crates/gbpplanner-rs/src/bin/hover-ui.rs | 199 ------------- .../src/bin/many-animated-sprites.rs | 152 ---------- crates/gbpplanner-rs/src/bin/notify.rs | 275 ------------------ .../gbpplanner-rs/src/bin/repeating-timers.rs | 2 + crates/gbpplanner-rs/src/config/mod.rs | 3 + crates/gbpplanner-rs/src/diagnostic/robot.rs | 14 + .../src/factorgraph/factor/dynamic.rs | 5 +- .../src/factorgraph/factor/interrobot.rs | 95 +++--- .../factor/marginalise_factor_distance.rs | 156 +++------- .../src/factorgraph/factor/mod.rs | 78 +++-- .../src/factorgraph/factor/obstacle.rs | 7 +- .../src/factorgraph/factor/pose.rs | 6 +- .../src/factorgraph/factorgraph.rs | 16 +- .../gbpplanner-rs/src/factorgraph/graphviz.rs | 1 + .../gbpplanner-rs/src/factorgraph/message.rs | 8 +- crates/gbpplanner-rs/src/factorgraph/mod.rs | 16 +- crates/gbpplanner-rs/src/factorgraph/node.rs | 6 +- .../gbpplanner-rs/src/factorgraph/variable.rs | 48 +-- crates/gbpplanner-rs/src/main.rs | 1 + crates/gbpplanner-rs/src/planner/factor.rs | 197 ------------- .../gbpplanner-rs/src/planner/factorgraph.rs | 2 - crates/gbpplanner-rs/src/ui/mod.rs | 4 +- crates/gbpplanner-rs/src/ui/settings.rs | 27 +- 34 files changed, 401 insertions(+), 1103 deletions(-) delete mode 100644 crates/gbpplanner-rs/src/bin/derive_more.rs delete mode 100644 crates/gbpplanner-rs/src/bin/hover-ui.rs delete mode 100644 crates/gbpplanner-rs/src/bin/many-animated-sprites.rs delete mode 100644 crates/gbpplanner-rs/src/bin/notify.rs diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 97809a45..08348b91 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -22,5 +22,3 @@ repos: - id: conventional-pre-commit stages: [commit-msg] args: [] - - diff --git a/Cargo.lock b/Cargo.lock index 74383f0c..c0262595 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -964,6 +964,13 @@ dependencies = [ "rfd", ] +[[package]] +name = "bevy_fullscreen" +version = "1.0.0" +dependencies = [ + "bevy", +] + [[package]] name = "bevy_gilrs" version = "0.13.2" @@ -2386,6 +2393,27 @@ dependencies = [ "crypto-common", ] +[[package]] +name = "directories" +version = "5.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a49173b84e034382284f27f1af4dcbbd231ffa358c0fe316541a7337f376a35" +dependencies = [ + "dirs-sys", +] + +[[package]] +name = "dirs-sys" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "520f05a5cbd335fae5a99ff7a6ab8627577660ee5cfd6a94a6a929b52ff0321c" +dependencies = [ + "libc", + "option-ext", + "redox_users", + "windows-sys 0.48.0", +] + [[package]] name = "dispatch" version = "0.2.0" @@ -2416,6 +2444,16 @@ version = "1.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "75b325c5dbd37f80359721ad39aca5a29fb04c89279657cffdda8736d0c0b9d2" +[[package]] +name = "duplicate" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "de78e66ac9061e030587b2a2e75cc88f22304913c907b11307bca737141230cb" +dependencies = [ + "heck 0.4.1", + "proc-macro-error", +] + [[package]] name = "ecolor" version = "0.26.2" @@ -2447,6 +2485,17 @@ dependencies = [ "egui", ] +[[package]] +name = "egui_dock" +version = "0.11.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a062ac200c9f3ddf120ffcc5582f9fbd5d8fbd046d2eed215ed5426f56513d0" +dependencies = [ + "duplicate", + "egui", + "paste", +] + [[package]] name = "egui_extras" version = "0.26.2" @@ -2483,6 +2532,19 @@ dependencies = [ "egui", ] +[[package]] +name = "egui_tiles" +version = "0.7.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1bb10cef7bdbd1adb158aec9cca20f34779fd40ea126e02662ab558189f1c435" +dependencies = [ + "ahash", + "egui", + "itertools", + "log", + "serde", +] + [[package]] name = "ehttp" version = "0.4.0" @@ -2941,6 +3003,7 @@ dependencies = [ "bevy_egui 0.26.0", "bevy_eventlistener", "bevy_file_dialog", + "bevy_fullscreen", "bevy_infinite_grid", "bevy_mod_picking", "bevy_more_shapes", @@ -2951,9 +3014,12 @@ dependencies = [ "clap", "colored", "derive_more", + "directories", + "egui_dock", "egui_extras", "egui_graphs", "egui_plot", + "egui_tiles", "embed-resource 2.4.2", "gbp_linalg", "gbp_multivariate_normal", @@ -3649,6 +3715,16 @@ dependencies = [ "redox_syscall 0.4.1", ] +[[package]] +name = "libredox" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c0ff37bd590ca25063e35af745c343cb7a0271906fb7b37e4813e8f79f00268d" +dependencies = [ + "bitflags 2.5.0", + "libc", +] + [[package]] name = "libudev-sys" version = "0.1.4" @@ -4194,13 +4270,19 @@ dependencies = [ "pathdiff", ] +[[package]] +name = "option-ext" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "04744f49eae99ab78e0d5c0b603ab218f515ea8cfe5a456d7629ad883a3b6e7d" + [[package]] name = "orbclient" version = "0.3.47" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "52f0d54bde9774d3a51dcf281a5def240c71996bc6ca05d2c847ec8b2b216166" dependencies = [ - "libredox", + "libredox 0.0.2", ] [[package]] @@ -4424,6 +4506,30 @@ dependencies = [ "toml_edit 0.21.1", ] +[[package]] +name = "proc-macro-error" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c" +dependencies = [ + "proc-macro-error-attr", + "proc-macro2", + "quote", + "syn 1.0.109", + "version_check", +] + +[[package]] +name = "proc-macro-error-attr" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869" +dependencies = [ + "proc-macro2", + "quote", + "version_check", +] + [[package]] name = "proc-macro2" version = "1.0.79" @@ -4584,6 +4690,17 @@ version = "0.1.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "20145670ba436b55d91fc92d25e71160fbfbdd57831631c8d7d36377a476f1cb" +[[package]] +name = "redox_users" +version = "0.4.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bd283d9651eeda4b2a83a43c1c91b266c40fd76ecd39a50a8c630ae69dc72891" +dependencies = [ + "getrandom", + "libredox 0.1.3", + "thiserror", +] + [[package]] name = "regex" version = "1.10.4" diff --git a/Cargo.toml b/Cargo.toml index 13259766..0d7430f8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,7 +13,7 @@ members = [ "crates/angle", # "crates/bevy_undo_redo", "crates/gbpplanner-rs", "crates/bevy_notify", - "crates/units", + "crates/units", "crates/bevy_fullscreen", ] [workspace.package] diff --git a/crates/bevy_notify/src/lib.rs b/crates/bevy_notify/src/lib.rs index d23cebfd..63f1e380 100644 --- a/crates/bevy_notify/src/lib.rs +++ b/crates/bevy_notify/src/lib.rs @@ -45,6 +45,7 @@ pub mod prelude { /// Adds event `ToastEvent` to be used in systems. /// Uses a `Update` system to render the toasts on the screen at the specified /// anchor +#[derive(Debug)] pub struct NotifyPlugin { /// Anchor for where to render the toasts in the main window /// Defaults to `Anchor::TopCenter` @@ -108,7 +109,7 @@ impl Toasts { } /// Event for creating a toast -#[derive(Event)] +#[derive(Debug, Event)] pub struct ToastEvent { /// The caption of the toast pub caption: String, diff --git a/crates/gbp_linalg/src/lib.rs b/crates/gbp_linalg/src/lib.rs index 7039ce17..4bb6bee1 100644 --- a/crates/gbp_linalg/src/lib.rs +++ b/crates/gbp_linalg/src/lib.rs @@ -5,6 +5,8 @@ pub mod pretty_print; /// `use gbp_linalg::prelude::*` to import all the common symbols from this /// crate pub mod prelude { + // pub use ndarray::{array, concatenate, s, Axis}; + pub use super::{ pretty_print::*, Float, GbpFloat, Matrix, MatrixView, NdarrayVectorExt, Vector, VectorNorm, VectorView, diff --git a/crates/gbpplanner-rs/Cargo.toml b/crates/gbpplanner-rs/Cargo.toml index f3807773..b082d3ca 100644 --- a/crates/gbpplanner-rs/Cargo.toml +++ b/crates/gbpplanner-rs/Cargo.toml @@ -20,6 +20,7 @@ min_len_vec = { path = "../min_len_vec" } gbp_linalg = { path = "../gbp_linalg" } gbp_multivariate_normal = { path = "../gbp_multivariate_normal" } bevy_notify = { path = "../bevy_notify" } +bevy_fullscreen = { path = "../bevy_fullscreen" } units = { path = "../units" } glob = "0.3.1" @@ -28,8 +29,11 @@ bevy = { version = "0.13.0", default-features = true, features = [ # "wayland", # "dynamic_linking", ] } + bevy-inspector-egui = "0.23.2" -bevy_egui = "0.26.0" +bevy_egui = "0.26.0" +egui_dock = "0.11.4" +egui_tiles = "0.7.2" # bevy_egui = { git = "https://github.com/mvlabat/bevy_egui", branch = "main" } # 0.26 does not support egui 0.27, which egui-notify needs @@ -99,7 +103,10 @@ paste = "1.0.14" egui_plot = "0.26" egui_graphs = "0.19.0" serde_yaml = "0.9.34" -colored = "2.1.0" +colored = "2.1.0" + +directories = "5.0" + # font-kit = { version = "0.13.0", features = ["freetype"] } diff --git a/crates/gbpplanner-rs/src/bevy_utils.rs b/crates/gbpplanner-rs/src/bevy_utils.rs index 44bee5ef..1c508bb5 100644 --- a/crates/gbpplanner-rs/src/bevy_utils.rs +++ b/crates/gbpplanner-rs/src/bevy_utils.rs @@ -35,3 +35,26 @@ pub fn despawn_entities_with_component( commands.entity(entity).despawn_recursive(); } } + +pub mod run_conditions { + use bevy::{ + ecs::system::Res, + input::{keyboard::KeyCode, ButtonInput}, + }; + + // pub fn any_input_just_pressed( + // // inputs: impl IntoIterator>, + // // inputs: impl IntoIterator, + // // inputs: Vec, + // ) -> impl Fn(Res>) -> bool + // // where + // // T: Copy + Eq + Send + Sync + 'static, + // { + // move |keyboard_input: Res>| + // keyboard_input.any_pressed(inputs) + + // // move |keyboard_input: Res>| { + // // inputs.into_iter().any(|it| + // keyboard_input.just_pressed(it)) // } + // } +} diff --git a/crates/gbpplanner-rs/src/bin/console.rs b/crates/gbpplanner-rs/src/bin/console.rs index 84ca2503..64daba06 100644 --- a/crates/gbpplanner-rs/src/bin/console.rs +++ b/crates/gbpplanner-rs/src/bin/console.rs @@ -1,3 +1,5 @@ +#![allow(missing_docs)] + use bevy::{log::LogPlugin, prelude::*}; use bevy_dev_console::prelude::*; diff --git a/crates/gbpplanner-rs/src/bin/derive_more.rs b/crates/gbpplanner-rs/src/bin/derive_more.rs deleted file mode 100644 index 0453bf25..00000000 --- a/crates/gbpplanner-rs/src/bin/derive_more.rs +++ /dev/null @@ -1,18 +0,0 @@ -#[derive(derive_more::Display)] -#[display(fmt = "hello")] -struct Foo; - -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, derive_more::From)] -struct VariableIndex(usize); - -fn main() -> Result<(), Box> { - let foo = Foo; - - println!("{}", foo); - - let vi: VariableIndex = 42usize.into(); - - println!("{:?}", vi); - - Ok(()) -} diff --git a/crates/gbpplanner-rs/src/bin/draw2d.rs b/crates/gbpplanner-rs/src/bin/draw2d.rs index 89cc067a..baf0578e 100644 --- a/crates/gbpplanner-rs/src/bin/draw2d.rs +++ b/crates/gbpplanner-rs/src/bin/draw2d.rs @@ -1,3 +1,5 @@ +#![allow(missing_docs)] + use bevy::{prelude::*, window::WindowResized}; // use std::f32::consts::{PI, TAU}; diff --git a/crates/gbpplanner-rs/src/bin/gizmos.rs b/crates/gbpplanner-rs/src/bin/gizmos.rs index 2836a2e1..645e1fd4 100644 --- a/crates/gbpplanner-rs/src/bin/gizmos.rs +++ b/crates/gbpplanner-rs/src/bin/gizmos.rs @@ -1,4 +1,4 @@ -#![allow(dead_code, unused_macros)] +#![allow(dead_code, unused_macros, missing_docs)] use bevy::{prelude::*, window::WindowResized}; // use std::f32::consts::{PI, TAU}; diff --git a/crates/gbpplanner-rs/src/bin/hover-ui.rs b/crates/gbpplanner-rs/src/bin/hover-ui.rs deleted file mode 100644 index 93b8b7d5..00000000 --- a/crates/gbpplanner-rs/src/bin/hover-ui.rs +++ /dev/null @@ -1,199 +0,0 @@ -#![allow(missing_docs)] - -use bevy::{prelude::*, window::PrimaryWindow}; - -fn main() -> Result<(), Box> { - let mut app = App::new(); - - app.add_plugins(DefaultPlugins) - .add_systems(Startup, setup) - .add_systems(Update, move_hover_window); - - app.run(); - Ok(()) -} - -// #[inline] -// fn px(logical_pixels: f32) -> Val { -// Val::Px(logical_pixels) -// } - -trait ValExt { - fn px(self) -> Val; - fn percent(self) -> Val; -} - -impl ValExt for f32 { - fn px(self) -> Val { - Val::Px(self) - } - - fn percent(self) -> Val { - if !(0.0..=100.0).contains(&self) { - panic!("value outside interval [0.0, 100.0]"); - } - Val::Percent(self) - } -} - -impl ValExt for i32 { - fn px(self) -> Val { - Val::Px(self as f32) - } - - fn percent(self) -> Val { - if !(0..=100).contains(&self) { - panic!("value outside interval [0, 100]"); - } - - Val::Percent(self as f32) - } -} - -fn relative_cursor_position_in_window(window: &Window) -> Option { - let cursor_position = window.cursor_position()?; - let relative_cursor_position = Vec2::new( - cursor_position.x / window.width(), - cursor_position.y / window.height(), - ); - - Some(relative_cursor_position) -} - -#[derive(Component)] -pub struct HoverWindow; - -fn setup(mut commands: Commands, query_window: Query<&Window, With>) { - let Ok(window) = query_window.get_single() else { - error!("no primary window"); - return; - }; - - let Some(relative_cursor_position) = relative_cursor_position_in_window(window) else { - warn!("cursor position is outside primary window"); - return; - }; - - commands.spawn(Camera2dBundle::default()); - - let root = commands - .spawn((HoverWindow, NodeBundle { - style: Style { - position_type: PositionType::Absolute, - top: 5.0.percent(), - // top: percent(5.0), - left: 5.0.percent(), - - width: 200.px(), - height: 400.px(), - ..default() - }, - background_color: Color::RED.into(), - border_color: Color::GREEN.into(), - ..default() - })) - .id(); - - // all the different combinations of border edges - // these correspond to the labels above - let borders = [ - UiRect::default(), - UiRect::all(Val::Px(10.)), - UiRect::left(Val::Px(10.)), - UiRect::right(Val::Px(10.)), - UiRect::top(Val::Px(10.)), - UiRect::bottom(Val::Px(10.)), - UiRect::horizontal(Val::Px(10.)), - UiRect::vertical(Val::Px(10.)), - UiRect { - left: Val::Px(10.), - top: Val::Px(10.), - ..Default::default() - }, - UiRect { - left: Val::Px(10.), - bottom: Val::Px(10.), - ..Default::default() - }, - UiRect { - right: Val::Px(10.), - top: Val::Px(10.), - ..Default::default() - }, - UiRect { - right: Val::Px(10.), - bottom: Val::Px(10.), - ..Default::default() - }, - UiRect { - right: Val::Px(10.), - top: Val::Px(10.), - bottom: Val::Px(10.), - ..Default::default() - }, - UiRect { - left: Val::Px(10.), - top: Val::Px(10.), - bottom: Val::Px(10.), - ..Default::default() - }, - UiRect { - left: Val::Px(10.), - right: Val::Px(10.), - top: Val::Px(10.), - ..Default::default() - }, - UiRect { - left: Val::Px(10.), - right: Val::Px(10.), - bottom: Val::Px(10.), - ..Default::default() - }, - ]; - - let title = commands - .spawn(TextBundle::from_section("variable", TextStyle { - font_size: 10.0, - ..Default::default() - })) - .id(); - - let container = commands - .spawn(NodeBundle { - style: Style { - flex_direction: FlexDirection::Column, - align_items: AlignItems::Center, - ..Default::default() - }, - ..Default::default() - }) - .push_children(&[title]) - .id(); - - commands.entity(root).add_child(container); -} - -fn primary_window_exists(query_window: Query<&Window, With>) -> bool { - query_window.get_single().is_ok() -} - -fn move_hover_window( - mut query_hover_window: Query<&mut Transform, With>, - query_window: Query<&Window, With>, -) { - let window = query_window.single(); - - let Some(relative_cursor_position) = relative_cursor_position_in_window(window) else { - warn!("cursor outside primary window"); - return; - }; - - let mut transform = query_hover_window.single_mut(); - - let x = relative_cursor_position.x * window.width(); - let y = relative_cursor_position.y * window.height(); - - info!("x = {}, y = {}", x, y); - - transform.translation = Vec3::new(x, 0.0, y); -} diff --git a/crates/gbpplanner-rs/src/bin/many-animated-sprites.rs b/crates/gbpplanner-rs/src/bin/many-animated-sprites.rs deleted file mode 100644 index 0b63fd12..00000000 --- a/crates/gbpplanner-rs/src/bin/many-animated-sprites.rs +++ /dev/null @@ -1,152 +0,0 @@ -//! Renders a lot of animated sprites to allow performance testing. -//! -//! This example sets up many animated sprites in different sizes, rotations, -//! and scales in the world. It also moves the camera over them to see how well -//! frustum culling works. - -use std::time::Duration; - -use bevy::{ - diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}, - math::Quat, - prelude::*, - render::camera::Camera, - window::{PresentMode, WindowResolution}, - winit::{UpdateMode, WinitSettings}, -}; -use rand::Rng; - -const CAMERA_SPEED: f32 = 1000.0; - -fn main() { - App::new() - // Since this is also used as a benchmark, we want it to display performance data. - .add_plugins(( - LogDiagnosticsPlugin::default(), - FrameTimeDiagnosticsPlugin, - DefaultPlugins.set(WindowPlugin { - primary_window: Some(Window { - present_mode: PresentMode::AutoNoVsync, - resolution: WindowResolution::new(1920.0, 1080.0) - .with_scale_factor_override(1.0), - ..default() - }), - ..default() - }), - )) - .insert_resource(WinitSettings { - focused_mode: UpdateMode::Continuous, - unfocused_mode: UpdateMode::Continuous, - }) - .add_systems(Startup, setup) - .add_systems( - Update, - ( - animate_sprite, - print_sprite_count, - move_camera.after(print_sprite_count), - ), - ) - .run(); -} - -fn setup( - mut commands: Commands, - assets: Res, - mut texture_atlases: ResMut>, -) { - // warn!(include_str!("warning_string.txt")); - - let mut rng = rand::thread_rng(); - - let tile_size = Vec2::splat(64.0); - let map_size = Vec2::splat(320.0); - - let half_x = (map_size.x / 2.0) as i32; - let half_y = (map_size.y / 2.0) as i32; - - let texture_handle = assets.load("textures/rpg/chars/gabe/gabe-idle-run.png"); - let texture_atlas = TextureAtlasLayout::from_grid(Vec2::new(24.0, 24.0), 7, 1, None, None); - let texture_atlas_handle = texture_atlases.add(texture_atlas); - - // Spawns the camera - - commands.spawn(Camera2dBundle::default()); - - // Builds and spawns the sprites - for y in -half_y..half_y { - for x in -half_x..half_x { - let position = Vec2::new(x as f32, y as f32); - let translation = (position * tile_size).extend(rng.gen::()); - let rotation = Quat::from_rotation_z(rng.gen::()); - let scale = Vec3::splat(rng.gen::() * 2.0); - let mut timer = Timer::from_seconds(0.1, TimerMode::Repeating); - timer.set_elapsed(Duration::from_secs_f32(rng.gen::())); - - commands.spawn(( - SpriteSheetBundle { - texture: texture_handle.clone(), - atlas: TextureAtlas { - layout: texture_atlas_handle.clone(), - ..Default::default() - }, - transform: Transform { - translation, - rotation, - scale, - }, - sprite: Sprite { - custom_size: Some(tile_size), - ..default() - }, - ..default() - }, - AnimationTimer(timer), - )); - } - } -} - -// System for rotating and translating the camera -fn move_camera(time: Res