Skip to content

Commit 8127c1e

Browse files
contagoncontagon
committed
Make core module to pair with trait module
1 parent 7b7b3f1 commit 8127c1e

File tree

15 files changed

+101
-87
lines changed

15 files changed

+101
-87
lines changed

‎examples/g2o.rs

+6-7
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,16 @@
1+
#[cfg(feature = "rerun")]
2+
use std::net::{SocketAddr, SocketAddrV4};
13
use std::{env, time::Instant};
24

5+
#[cfg(feature = "rerun")]
6+
use factrs::rerun::RerunObserver;
37
use factrs::{
4-
optimizers::{GaussNewton, GraphOptimizer, Optimizer},
8+
core::{GaussNewton, SE2, SE3},
9+
traits::Optimizer,
510
utils::load_g20,
6-
variables::*,
711
};
8-
9-
#[cfg(feature = "rerun")]
10-
use factrs::rerun::RerunObserver;
1112
#[cfg(feature = "rerun")]
1213
use rerun::{Arrows2D, Arrows3D, Points2D, Points3D};
13-
#[cfg(feature = "rerun")]
14-
use std::net::{SocketAddr, SocketAddrV4};
1514

1615
// Setups rerun and a callback for iteratively sending to rerun
1716
// Must run with --features rerun for it to work

‎examples/gps.rs

+21-21
Original file line numberDiff line numberDiff line change
@@ -10,21 +10,18 @@ A simple 2D pose slam example with "GPS" measurements
1010
*/
1111

1212
#![allow(unused_imports)]
13+
// Our state will be represented by SE2 -> theta, x, y
14+
// VectorVar2 is a newtype around Vector2 for optimization purposes
15+
use factrs::variables::{VectorVar2, SE2};
1316
use factrs::{
1417
assign_symbols,
15-
containers::{Graph, Values},
18+
core::{BetweenResidual, GaussNewton, GaussianNoise, Graph, Values},
1619
dtype, fac,
1720
linalg::{Const, ForwardProp, Numeric, NumericalDiff, VectorX},
18-
noise::GaussianNoise,
19-
optimizers::GaussNewton,
20-
residuals::{BetweenResidual, Residual1},
21-
traits::{GraphOptimizer, Optimizer, Variable},
21+
residuals::Residual1,
22+
traits::*,
2223
};
2324

24-
// Our state will be represented by SE2 -> theta, x, y
25-
// VectorVar2 is a newtype around Vector2 for optimization purposes
26-
use factrs::variables::{VectorVar2, SE2};
27-
2825
#[derive(Clone, Debug)]
2926
// Enable serialization if it's desired
3027
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
@@ -45,10 +42,11 @@ impl GpsResidual {
4542
impl Residual1 for GpsResidual {
4643
// Use forward propagation for differentiation
4744
type Differ = ForwardProp<<Self as Residual1>::DimIn>;
48-
// Alternatively, could use numerical differentiation (6 => 10^-6 as denominator)
49-
// type Differ = NumericalDiff<6>;
45+
// Alternatively, could use numerical differentiation (6 => 10^-6 as
46+
// denominator) type Differ = NumericalDiff<6>;
5047

51-
// The input variable type, input dimension of variable(s), and output dimension of residual
48+
// The input variable type, input dimension of variable(s), and output dimension
49+
// of residual
5250
type V1 = SE2;
5351
type DimIn = Const<3>;
5452
type DimOut = Const<2>;
@@ -64,8 +62,8 @@ impl Residual1 for GpsResidual {
6462
}
6563

6664
// You can also hand-code the jacobian by hand if extra efficiency is desired.
67-
// fn residual1_jacobian(&self, values: &Values, keys: &[Key]) -> DiffResult<VectorX, MatrixX> {
68-
// let p: &SE2 = values
65+
// fn residual1_jacobian(&self, values: &Values, keys: &[Key]) ->
66+
// DiffResult<VectorX, MatrixX> { let p: &SE2 = values
6967
// .get_unchecked(keys[0])
7068
// .expect("got wrong variable type");
7169
// let s = p.theta().sin();
@@ -76,8 +74,9 @@ impl Residual1 for GpsResidual {
7674
// diff,
7775
// }
7876
// }
79-
// As a note - the above jacobian is only valid if running with the "left" feature disabled
80-
// Switching to the left feature will change the jacobian used
77+
// As a note - the above jacobian is only valid if running with the "left"
78+
// feature disabled Switching to the left feature will change the jacobian
79+
// used
8180
}
8281

8382
// Here we assign X to always represent SE2 variables
@@ -111,11 +110,12 @@ fn main() {
111110

112111
// These will all compile-time error
113112
// values.insert(X(5), VectorVar2::identity()); // wrong variable type
114-
// let f = fac![GpsResidual::new(0.0, 0.0), (X(0), X(1))]; // wrong number of keys
115-
// let n = GaussianNoise::<5>::from_scalar_sigma(0.1);
116-
// let f = fac![GpsResidual::new(0.0, 0.0), X(0), n]; // wrong noise-model dimension
117-
// assign_symbols!(Y : VectorVar2);
118-
// let f = fac![GpsResidual::new(0.0, 0.0), Y(0), 0.1 as std]; // wrong variable type
113+
// let f = fac![GpsResidual::new(0.0, 0.0), (X(0), X(1))]; // wrong number of
114+
// keys let n = GaussianNoise::<5>::from_scalar_sigma(0.1);
115+
// let f = fac![GpsResidual::new(0.0, 0.0), X(0), n]; // wrong noise-model
116+
// dimension assign_symbols!(Y : VectorVar2);
117+
// let f = fac![GpsResidual::new(0.0, 0.0), Y(0), 0.1 as std]; // wrong variable
118+
// type
119119

120120
// optimize
121121
let mut opt: GaussNewton = GaussNewton::new(graph);

‎rustfmt.toml

+6
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
imports_granularity = "Crate"
2+
group_imports = "StdExternalCrate"
3+
# imports_layout = "HorizontalVertical"
4+
5+
format_code_in_doc_comments = true
6+
wrap_comments = true

‎src/containers/factor.rs

+1-1
Original file line numberDiff line numberDiff line change
@@ -136,7 +136,7 @@ impl<'f, KF> FactorFormatter<'f, KF> {
136136
}
137137
}
138138

139-
impl<'f, KF: KeyFormatter> fmt::Debug for FactorFormatter<'f, KF> {
139+
impl<KF: KeyFormatter> fmt::Debug for FactorFormatter<'_, KF> {
140140
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
141141
if f.alternate() {
142142
f.write_str("Factor {\n")?;

‎src/containers/graph.rs

+1-1
Original file line numberDiff line numberDiff line change
@@ -131,7 +131,7 @@ impl<'g, KF> GraphFormatter<'g, KF> {
131131
}
132132
}
133133

134-
impl<'g, KF: KeyFormatter> Debug for GraphFormatter<'g, KF> {
134+
impl<KF: KeyFormatter> Debug for GraphFormatter<'_, KF> {
135135
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
136136
if f.alternate() {
137137
f.write_str("Graph [\n")?;

‎src/containers/values.rs

+2-2
Original file line numberDiff line numberDiff line change
@@ -225,7 +225,7 @@ impl<'v, KF> ValuesFormatter<'v, KF> {
225225
}
226226
}
227227

228-
impl<'v, KF: KeyFormatter> fmt::Display for ValuesFormatter<'v, KF> {
228+
impl<KF: KeyFormatter> fmt::Display for ValuesFormatter<'_, KF> {
229229
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
230230
let precision = f.precision().unwrap_or(3);
231231
if f.alternate() {
@@ -246,7 +246,7 @@ impl<'v, KF: KeyFormatter> fmt::Display for ValuesFormatter<'v, KF> {
246246
}
247247
}
248248

249-
impl<'v, KF: KeyFormatter> fmt::Debug for ValuesFormatter<'v, KF> {
249+
impl<KF: KeyFormatter> fmt::Debug for ValuesFormatter<'_, KF> {
250250
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
251251
let precision = f.precision().unwrap_or(3);
252252
if f.alternate() {

‎src/lib.rs

+23-8
Original file line numberDiff line numberDiff line change
@@ -127,10 +127,26 @@ pub mod symbols {
127127
/// ```
128128
pub mod traits {
129129
pub use crate::{
130-
linalg::Diff,
131-
optimizers::{GraphOptimizer, Optimizer},
132-
residuals::Residual,
133-
variables::Variable,
130+
linalg::Diff, noise::NoiseModel, optimizers::Optimizer, residuals::Residual,
131+
robust::RobustCost, variables::Variable,
132+
};
133+
}
134+
135+
/// Helper module to group together common types
136+
///
137+
/// Specifically, this contains everything that would be needed to implement a
138+
/// simple pose graph. While we recommend against it, it can be all imported
139+
/// using
140+
/// ```
141+
/// use factrs::core::*;
142+
/// ```
143+
pub mod core {
144+
pub use crate::{
145+
containers::{Factor, Graph, Values},
146+
noise::{GaussianNoise, UnitNoise},
147+
optimizers::{GaussNewton, LevenMarquardt},
148+
residuals::{BetweenResidual, PriorResidual},
149+
variables::{VectorVar, VectorVar1, VectorVar2, VectorVar3, SE2, SE3, SO2, SO3},
134150
};
135151
}
136152

@@ -139,8 +155,7 @@ pub mod rerun;
139155

140156
#[cfg(feature = "serde")]
141157
pub mod serde {
142-
pub use crate::noise::tag_noise;
143-
pub use crate::residuals::tag_residual;
144-
pub use crate::robust::tag_robust;
145-
pub use crate::variables::tag_variable;
158+
pub use crate::{
159+
noise::tag_noise, residuals::tag_residual, robust::tag_robust, variables::tag_variable,
160+
};
146161
}

‎src/optimizers/gauss_newton.rs

+4-4
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
use faer_ext::IntoNalgebra;
22

3-
use super::{GraphOptimizer, OptObserverVec, OptParams, OptResult, Optimizer};
3+
use super::{OptObserverVec, OptParams, OptResult, Optimizer};
44
use crate::{
55
containers::{Graph, GraphOrder, Values, ValuesOrder},
66
linalg::DiffResult,
@@ -26,8 +26,8 @@ pub struct GaussNewton<S: LinearSolver = CholeskySolver> {
2626
graph_order: Option<GraphOrder>,
2727
}
2828

29-
impl<S: LinearSolver> GraphOptimizer for GaussNewton<S> {
30-
fn new(graph: Graph) -> Self {
29+
impl<S: LinearSolver> GaussNewton<S> {
30+
pub fn new(graph: Graph) -> Self {
3131
Self {
3232
graph,
3333
solver: S::default(),
@@ -37,7 +37,7 @@ impl<S: LinearSolver> GraphOptimizer for GaussNewton<S> {
3737
}
3838
}
3939

40-
fn graph(&self) -> &Graph {
40+
pub fn graph(&self) -> &Graph {
4141
&self.graph
4242
}
4343
}

‎src/optimizers/levenberg_marquardt.rs

+4-4
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@ use std::ops::Mul;
33
use faer::{scale, sparse::SparseColMat};
44
use faer_ext::IntoNalgebra;
55

6-
use super::{GraphOptimizer, OptError, OptObserverVec, OptParams, OptResult, Optimizer};
6+
use super::{OptError, OptObserverVec, OptParams, OptResult, Optimizer};
77
use crate::{
88
containers::{Graph, GraphOrder, Values, ValuesOrder},
99
dtype,
@@ -51,8 +51,8 @@ pub struct LevenMarquardt<S: LinearSolver = CholeskySolver> {
5151
graph_order: Option<GraphOrder>,
5252
}
5353

54-
impl<S: LinearSolver> GraphOptimizer for LevenMarquardt<S> {
55-
fn new(graph: Graph) -> Self {
54+
impl<S: LinearSolver> LevenMarquardt<S> {
55+
pub fn new(graph: Graph) -> Self {
5656
Self {
5757
graph,
5858
solver: S::default(),
@@ -64,7 +64,7 @@ impl<S: LinearSolver> GraphOptimizer for LevenMarquardt<S> {
6464
}
6565
}
6666

67-
fn graph(&self) -> &Graph {
67+
pub fn graph(&self) -> &Graph {
6868
&self.graph
6969
}
7070
}

‎src/optimizers/macros.rs

+15-7
Original file line numberDiff line numberDiff line change
@@ -5,35 +5,43 @@
55
/// - Between optimization for VectorVar3, SO3, and SE3
66
#[macro_export]
77
macro_rules! test_optimizer {
8-
($optimizer:ty) => {
8+
($o:ty) => {
99
#[test]
1010
fn priorvector3() {
11-
$crate::optimizers::test::optimize_prior::<$optimizer, 3, $crate::variables::VectorVar3>()
11+
let f = |graph| <$o>::new(graph);
12+
$crate::optimizers::test::optimize_prior::<$o, 3, $crate::variables::VectorVar3>(&f)
1213
}
1314

1415
#[test]
1516
fn priorso3() {
16-
$crate::optimizers::test::optimize_prior::<$optimizer, 3, $crate::variables::SO3>();
17+
let f = |graph| <$o>::new(graph);
18+
$crate::optimizers::test::optimize_prior::<$o, 3, $crate::variables::SO3>(&f);
1719
}
1820

1921
#[test]
2022
fn priorse3() {
21-
$crate::optimizers::test::optimize_prior::<$optimizer, 6, $crate::variables::SE3>();
23+
let f = |graph| <$o>::new(graph);
24+
$crate::optimizers::test::optimize_prior::<$o, 6, $crate::variables::SE3>(&f);
2225
}
2326

2427
#[test]
2528
fn betweenvector3() {
26-
$crate::optimizers::test::optimize_between::<$optimizer, 3, 6, $crate::variables::VectorVar3>();
29+
let f = |graph| <$o>::new(graph);
30+
$crate::optimizers::test::optimize_between::<$o, 3, 6, $crate::variables::VectorVar3>(
31+
&f,
32+
);
2733
}
2834

2935
#[test]
3036
fn betweenso3() {
31-
$crate::optimizers::test::optimize_between::<$optimizer, 3, 6, $crate::variables::SO3>();
37+
let f = |graph| <$o>::new(graph);
38+
$crate::optimizers::test::optimize_between::<$o, 3, 6, $crate::variables::SO3>(&f);
3239
}
3340

3441
#[test]
3542
fn betweense3() {
36-
$crate::optimizers::test::optimize_between::<$optimizer, 6, 12, $crate::variables::SE3>();
43+
let f = |graph| <$o>::new(graph);
44+
$crate::optimizers::test::optimize_between::<$o, 6, 12, $crate::variables::SE3>(&f);
3745
}
3846
};
3947
}

‎src/optimizers/mod.rs

+11-14
Original file line numberDiff line numberDiff line change
@@ -35,9 +35,7 @@
3535
//! using the [test_optimizer](crate::test_optimizer) macro to run a handful of
3636
//! simple tests over a few different variable types to ensure correctness.
3737
mod traits;
38-
pub use traits::{
39-
GraphOptimizer, OptError, OptObserver, OptObserverVec, OptParams, OptResult, Optimizer,
40-
};
38+
pub use traits::{OptError, OptObserver, OptObserverVec, OptParams, OptResult, Optimizer};
4139

4240
mod macros;
4341

@@ -58,7 +56,6 @@ pub mod test {
5856
containers::{FactorBuilder, Graph, Values},
5957
dtype,
6058
linalg::{AllocatorBuffer, Const, DualAllocator, DualVector, VectorX},
61-
noise::{NoiseModel, UnitNoise},
6259
residuals::{BetweenResidual, PriorResidual, Residual},
6360
symbols::X,
6461
variables::VariableDtype,
@@ -69,11 +66,11 @@ pub mod test {
6966
const DIM: usize,
7067
#[cfg(feature = "serde")] T: VariableDtype<Dim = nalgebra::Const<DIM>> + 'static + typetag::Tagged,
7168
#[cfg(not(feature = "serde"))] T: VariableDtype<Dim = nalgebra::Const<DIM>> + 'static,
72-
>()
73-
where
74-
UnitNoise<DIM>: NoiseModel,
69+
>(
70+
new: &dyn Fn(Graph) -> O,
71+
) where
7572
PriorResidual<T>: Residual,
76-
O: Optimizer<Input = Values> + GraphOptimizer,
73+
O: Optimizer<Input = Values>,
7774
{
7875
let t = VectorX::from_fn(T::DIM, |_, i| ((i + 1) as dtype) / 10.0);
7976
let p = T::exp(t.as_view());
@@ -86,7 +83,7 @@ pub mod test {
8683
let factor = FactorBuilder::new1_unchecked(res, X(0)).build();
8784
graph.add_factor(factor);
8885

89-
let mut opt = O::new(graph);
86+
let mut opt = new(graph);
9087
values = opt.optimize(values).expect("Optimization failed");
9188

9289
let out: &T = values.get_unchecked(X(0)).expect("Missing X(0)");
@@ -104,12 +101,12 @@ pub mod test {
104101
const DIM_DOUBLE: usize,
105102
#[cfg(feature = "serde")] T: VariableDtype<Dim = nalgebra::Const<DIM>> + 'static + typetag::Tagged,
106103
#[cfg(not(feature = "serde"))] T: VariableDtype<Dim = nalgebra::Const<DIM>> + 'static,
107-
>()
108-
where
109-
UnitNoise<DIM>: NoiseModel,
104+
>(
105+
new: &dyn Fn(Graph) -> O,
106+
) where
110107
PriorResidual<T>: Residual,
111108
BetweenResidual<T>: Residual,
112-
O: Optimizer<Input = Values> + GraphOptimizer,
109+
O: Optimizer<Input = Values>,
113110
Const<DIM>: ToTypenum,
114111
AllocatorBuffer<DimNameSum<Const<DIM>, Const<DIM>>>: Sync + Send,
115112
DefaultAllocator: DualAllocator<DimNameSum<Const<DIM>, Const<DIM>>>,
@@ -136,7 +133,7 @@ pub mod test {
136133
let factor = FactorBuilder::new2_unchecked(res, X(0), X(1)).build();
137134
graph.add_factor(factor);
138135

139-
let mut opt = O::new(graph);
136+
let mut opt = new(graph);
140137
values = opt.optimize(values).expect("Optimization failed");
141138

142139
let out1: &T = values.get_unchecked(X(0)).expect("Missing X(0)");

‎src/optimizers/traits.rs

+1-8
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
use crate::{containers::Graph, dtype};
1+
use crate::dtype;
22

33
/// Error types for optimizers
44
#[derive(Debug)]
@@ -165,10 +165,3 @@ pub trait Optimizer {
165165
Err(OptError::MaxIterations(values))
166166
}
167167
}
168-
169-
/// Small trait for optimizers that work on a graph
170-
pub trait GraphOptimizer: Optimizer {
171-
fn new(graph: Graph) -> Self;
172-
173-
fn graph(&self) -> &Graph;
174-
}

0 commit comments

Comments
 (0)