Add manifold (#36)

* huge refactor

* save

* save

* manifold

* read g2o

* add parking

* add docs

* rename

Author: powei-lin

Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "tiny-solver"
-version = "0.13.0"
+version = "0.14.0"
 edition = "2021"
 authors = ["Powei Lin <poweilin1994@gmail.com>"]
 readme = "README.md"
@@ -15,7 +15,7 @@ exclude = ["/.github/*", "*.ipynb", "./scripts/*", "examples/*", "tests/"]
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
 [dependencies]
-faer = "0.20.1"
+faer = "0.20.2"
 faer-ext = { version = "0.4.1", features = ["nalgebra"] }
 log = "0.4.21"
 nalgebra = "0.33.2"
@@ -25,22 +25,28 @@ numpy = { version = "0.23.0", features = ["nalgebra"], optional = true }
 pyo3 = { version = "0.23.3", features = ["abi3", "abi3-py38"], optional = true }
 # pyo3-log = { version = "0.9.0", optional = true }
 rayon = "1.9.0"
+simba = "0.9.0"
 
 [[example]]
 name = "m3500_benchmark"
 path = "examples/m3500_benchmark.rs"
 
+[[example]]
+name = "sphere2500"
+
+[[example]]
+name = "parking-garage"
+
 [features]
 python = ["num-dual/python", "numpy", "pyo3"]
 
 [dev-dependencies]
 env_logger = "0.11.6"
-itertools = "0.13.0"
+itertools = "0.14.0"
 plotters = "0.3.6"
 
 [profile.dev.package.faer]
 opt-level = 3
 
 [lib]
 name = "tiny_solver"
-# crate-type = ["staticlib"]

README.md

@@ -23,7 +23,7 @@ cargo add tiny-solver
 - [x] LevenbergMarquardtOptimizer
 - [x] Multithreading jacobian
 - [x] loss functions (Huber, CauchyLoss, ArctanLoss)
-- [x] Define factor in python
+- [x] Parameter on manifold (SO3, SE3)
 
 #### TODO
 - [ ] information matrix
@@ -70,14 +70,14 @@ fn main() {
     // add residual x needs to be close to 3.0
     problem.add_residual_block(
         1,
-        &[("x", 1)],
+        &["x"],
         Box::new(tiny_solver::factors::PriorFactor {
             v: na::dvector![3.0],
         }),
         None,
     );
     // add custom residual for x and yz
-    problem.add_residual_block(2, &[("x", 1), ("yz", 2)], Box::new(CustomFactor {}), None);
+    problem.add_residual_block(2, &["x", "yz"], Box::new(CustomFactor), None);
 
     // the initial values for x is 0.7 and yz is [-30.2, 123.4]
     let initial_values = HashMap::<String, na::DVector<f64>>::from([
@@ -167,4 +167,15 @@ cargo run -r --example m3500_benchmark
 # run python version
 pip install tiny-solver matplotlib
 python3 examples/python/m3500.py
-```
+```
+### Sphere 2500 dataset
+```
+cargo run -r --example sphere2500
+```
+<img src="docs/sphere2500_rs.png" width="300" alt="sp2500 dataset rust result.">
+
+### Parking garage dataset
+```
+cargo run -r --example parking-garage
+```
+<img src="docs/parking_garage.png" width="300" alt="parking dataset rust result.">

docs/parking_garage.png

@@ -1,58 +1,8 @@
-use std::collections::HashMap;
-use std::fs::read_to_string;
 use std::time::Instant;
 
-use nalgebra as na;
 use plotters::prelude::*;
 
-use tiny_solver::{
-    factors, loss_functions::HuberLoss, optimizer::Optimizer, problem, GaussNewtonOptimizer,
-};
-
-fn read_g2o(filename: &str) -> (problem::Problem, HashMap<String, na::DVector<f64>>) {
-    let mut problem = problem::Problem::new();
-    let mut init_values = HashMap::<String, na::DVector<f64>>::new();
-    for line in read_to_string(filename).unwrap().lines() {
-        let line: Vec<&str> = line.split(' ').collect();
-        match line[0] {
-            "VERTEX_SE2" => {
-                let x = line[2].parse::<f64>().unwrap();
-                let y = line[3].parse::<f64>().unwrap();
-                let theta = line[4].parse::<f64>().unwrap();
-                init_values.insert(format!("x{}", line[1]), na::dvector![theta, x, y]);
-            }
-            "EDGE_SE2" => {
-                let id0 = format!("x{}", line[1]);
-                let id1 = format!("x{}", line[2]);
-                let dx = line[3].parse::<f64>().unwrap();
-                let dy = line[4].parse::<f64>().unwrap();
-                let dtheta = line[5].parse::<f64>().unwrap();
-                // todo add info matrix
-                let edge = factors::BetweenFactorSE2 { dx, dy, dtheta };
-                problem.add_residual_block(
-                    3,
-                    &[(&id0, 3), (&id1, 3)],
-                    Box::new(edge),
-                    Some(Box::new(HuberLoss::new(1.0))),
-                );
-            }
-            _ => {
-                println!("err");
-                break;
-            }
-        }
-    }
-    let origin_factor = factors::PriorFactor {
-        v: na::dvector![0.0, 0.0, 0.0],
-    };
-    problem.add_residual_block(
-        3,
-        &[("x0", 3)],
-        Box::new(origin_factor),
-        Some(Box::new(HuberLoss::new(1.0))),
-    );
-    (problem, init_values)
-}
+use tiny_solver::{helper::read_g2o, optimizer::Optimizer, GaussNewtonOptimizer};
 
 fn main() {
     // init logger

docs/sphere2500_rs.png

@@ -0,0 +1,50 @@
+use std::time::Instant;
+
+use plotters::prelude::*;
+
+use tiny_solver::helper::read_g2o;
+use tiny_solver::{optimizer::Optimizer, GaussNewtonOptimizer};
+
+fn main() {
+    // init logger
+    env_logger::init();
+
+    let (problem, init_values) = read_g2o("tests/data/parking-garage.g2o");
+    let init_points: Vec<(f64, f64)> = init_values.values().map(|v| (v[4], v[5])).collect();
+    let root_drawing_area =
+        BitMapBackend::new("parking_garage.png", (1024, 1024)).into_drawing_area();
+
+    root_drawing_area.fill(&WHITE).unwrap();
+
+    let mut scatter_ctx = ChartBuilder::on(&root_drawing_area)
+        .x_label_area_size(40)
+        .y_label_area_size(40)
+        .build_cartesian_2d(-220f64..80f64, -20f64..280f64)
+        .unwrap();
+    scatter_ctx
+        .configure_mesh()
+        .disable_x_mesh()
+        .disable_y_mesh()
+        .draw()
+        .unwrap();
+    scatter_ctx
+        .draw_series(
+            init_points
+                .iter()
+                .map(|(x, y)| Circle::new((*x, *y), 2, GREEN.filled())),
+        )
+        .unwrap();
+    let gn = GaussNewtonOptimizer::new();
+    let start = Instant::now();
+    let result = gn.optimize(&problem, &init_values, None);
+    let duration = start.elapsed();
+    println!("Time elapsed in total is: {:?}", duration);
+    let result_points: Vec<(f64, f64)> = result.unwrap().values().map(|v| (v[4], v[5])).collect();
+    scatter_ctx
+        .draw_series(
+            result_points
+                .iter()
+                .map(|(x, y)| Circle::new((*x, *y), 2, BLUE.filled())),
+        )
+        .unwrap();
+}

examples/m3500_benchmark.rs

@@ -3,7 +3,7 @@ use std::collections::HashMap;
 use nalgebra as na;
 use tiny_solver::{self, Optimizer};
 
-struct CustomFactor {}
+struct CustomFactor;
 // define your own residual function and the jacobian will be auto generated
 impl<T: na::RealField> tiny_solver::factors::Factor<T> for CustomFactor {
     fn residual_func(&self, params: &[nalgebra::DVector<T>]) -> nalgebra::DVector<T> {
@@ -31,14 +31,14 @@ fn main() {
     // add residual x needs to be close to 3.0
     problem.add_residual_block(
         1,
-        &[("x", 1)],
+        &["x"],
         Box::new(tiny_solver::factors::PriorFactor {
             v: na::dvector![3.0],
         }),
         None,
     );
     // add custom residual for x and yz
-    problem.add_residual_block(2, &[("x", 1), ("yz", 2)], Box::new(CustomFactor {}), None);
+    problem.add_residual_block(2, &["x", "yz"], Box::new(CustomFactor), None);
 
     // the initial values for x is 0.7 and yz is [-30.2, 123.4]
     let initial_values = HashMap::<String, na::DVector<f64>>::from([

examples/parking-garage.rs

@@ -0,0 +1,50 @@
+use std::time::Instant;
+
+use plotters::prelude::*;
+
+use tiny_solver::helper::read_g2o;
+use tiny_solver::{optimizer::Optimizer, GaussNewtonOptimizer};
+
+fn main() {
+    // init logger
+    env_logger::init();
+
+    let (problem, init_values) = read_g2o("tests/data/sphere2500.g2o");
+    let init_points: Vec<(f64, f64)> = init_values.values().map(|v| (v[4], v[6])).collect();
+    let root_drawing_area =
+        BitMapBackend::new("sphere2500_rs.png", (1024, 1024)).into_drawing_area();
+
+    root_drawing_area.fill(&WHITE).unwrap();
+
+    let mut scatter_ctx = ChartBuilder::on(&root_drawing_area)
+        .x_label_area_size(40)
+        .y_label_area_size(40)
+        .build_cartesian_2d(-60f64..60f64, -110f64..10f64)
+        .unwrap();
+    scatter_ctx
+        .configure_mesh()
+        .disable_x_mesh()
+        .disable_y_mesh()
+        .draw()
+        .unwrap();
+    scatter_ctx
+        .draw_series(
+            init_points
+                .iter()
+                .map(|(x, y)| Circle::new((*x, *y), 2, GREEN.filled())),
+        )
+        .unwrap();
+    let gn = GaussNewtonOptimizer::new();
+    let start = Instant::now();
+    let result = gn.optimize(&problem, &init_values, None);
+    let duration = start.elapsed();
+    println!("Time elapsed in total is: {:?}", duration);
+    let result_points: Vec<(f64, f64)> = result.unwrap().values().map(|v| (v[4], v[6])).collect();
+    scatter_ctx
+        .draw_series(
+            result_points
+                .iter()
+                .map(|(x, y)| Circle::new((*x, *y), 2, BLUE.filled())),
+        )
+        .unwrap();
+}

examples/small_problem.rs

@@ -1,5 +1,7 @@
 use nalgebra as na;
 
+use crate::manifold::se3::SE3;
+
 pub trait Factor<T: na::RealField>: Send + Sync {
     fn residual_func(&self, params: &[na::DVector<T>]) -> na::DVector<T>;
 }
@@ -63,6 +65,35 @@ impl<T: na::RealField> Factor<T> for BetweenFactorSE2 {
     }
 }
 
+#[derive(Debug, Clone)]
+pub struct BetweenFactorSE3 {
+    pub dtx: f64,
+    pub dty: f64,
+    pub dtz: f64,
+    pub dqx: f64,
+    pub dqy: f64,
+    pub dqz: f64,
+    pub dqw: f64,
+}
+impl<T: na::RealField> Factor<T> for BetweenFactorSE3 {
+    fn residual_func(&self, params: &[na::DVector<T>]) -> na::DVector<T> {
+        let t_origin_k0 = &params[0];
+        let t_origin_k1 = &params[1];
+        let se3_origin_k0 = SE3::from_vec(t_origin_k0.as_view());
+        let se3_origin_k1 = SE3::from_vec(t_origin_k1.as_view());
+
+        let se3_k0_k1 = SE3::from_vec(
+            na::dvector![self.dqx, self.dqy, self.dqz, self.dqw, self.dtx, self.dty, self.dtz,]
+                .as_view(),
+        )
+        .cast::<T>();
+
+        let se3_diff = se3_origin_k1.inverse() * se3_origin_k0 * se3_k0_k1.cast();
+
+        se3_diff.log()
+    }
+}
+
 #[derive(Debug, Clone)]
 pub struct PriorFactor {
     pub v: na::DVector<f64>,