├── .gitattributes ├── .github └── workflows │ └── CI.yml ├── .gitignore ├── .pre-commit-config.yaml ├── Cargo.lock ├── Cargo.toml ├── LICENSE ├── README.md ├── benches ├── graph_slam.rs └── kalman_filter.rs ├── dataset ├── g2o │ ├── dlr.g2o │ ├── input_M3500_g2o.g2o │ ├── intel.g2o │ ├── parking-garage.g2o │ ├── simulation-pose-landmark.g2o │ ├── simulation-pose-pose.g2o │ ├── sphere2500.g2o │ └── torus3D.g2o ├── slam_course │ ├── sensor_data.dat │ └── world.dat └── utias0 │ ├── Barcodes.csv │ ├── Groundtruth.csv │ ├── Landmark_Groundtruth.csv │ ├── Measurement.csv │ └── Odometry.csv ├── deny.toml ├── examples ├── control │ └── inverted_pendulum.rs ├── localization │ ├── bayesian_filter.rs │ └── localization_landmarks.rs └── mapping │ └── pose_graph_optimization.rs ├── pyproject.toml ├── rust-toolchain.toml └── src ├── control ├── lqr.rs └── mod.rs ├── data ├── mod.rs ├── slam_course.rs └── utias.rs ├── lib.rs ├── localization ├── bayesian_filter.rs ├── extended_kalman_filter.rs ├── mod.rs ├── particle_filter.rs └── unscented_kalman_filter.rs ├── main.rs ├── mapping ├── ekf_slam_known.rs ├── g2o.rs ├── mod.rs ├── pose_graph_optimization.rs └── se2_se3.rs ├── models ├── measurement.rs ├── mod.rs └── motion.rs └── utils ├── mod.rs ├── mvn.rs ├── plot.rs └── state.rs /.gitattributes: -------------------------------------------------------------------------------- 1 | dataset/* filter=lfs diff=lfs merge=lfs -text 2 | -------------------------------------------------------------------------------- /.github/workflows/CI.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | branches: 6 | - main 7 | - master 8 | pull_request: 9 | workflow_dispatch: 10 | 11 | jobs: 12 | linux_build_and_test: 13 | name: Rust project - latest 14 | runs-on: ubuntu-latest 15 | strategy: 16 | matrix: 17 | toolchain: 18 | - stable 19 | # - beta 20 | # - nightly 21 | steps: 22 | - uses: actions/checkout@v3 23 | - run: sudo apt-get install liblapacke-dev libmumps-seq-dev libopenblas-dev libsuitesparse-dev 24 | - run: rustup update ${{ matrix.toolchain }} && rustup default ${{ matrix.toolchain }} 25 | - run: cargo build --verbose 26 | - run: cargo test --verbose 27 | # skip this maturin jobs for the moment 28 | # linux: 29 | # runs-on: ubuntu-latest 30 | # steps: 31 | # - uses: actions/checkout@v3 32 | # - uses: PyO3/maturin-action@v1 33 | # with: 34 | # manylinux: auto 35 | # command: build 36 | # args: --release --sdist -o dist --find-interpreter 37 | # - name: Upload wheels 38 | # uses: actions/upload-artifact@v3 39 | # with: 40 | # name: wheels 41 | # path: dist 42 | 43 | # windows: 44 | # runs-on: windows-latest 45 | # steps: 46 | # - uses: actions/checkout@v3 47 | # - uses: PyO3/maturin-action@v1 48 | # with: 49 | # command: build 50 | # args: --release -o dist --find-interpreter 51 | # - name: Upload wheels 52 | # uses: actions/upload-artifact@v3 53 | # with: 54 | # name: wheels 55 | # path: dist 56 | 57 | # macos: 58 | # runs-on: macos-latest 59 | # steps: 60 | # - uses: actions/checkout@v3 61 | # - uses: PyO3/maturin-action@v1 62 | # with: 63 | # command: build 64 | # args: --release -o dist --universal2 --find-interpreter 65 | # - name: Upload wheels 66 | # uses: actions/upload-artifact@v3 67 | # with: 68 | # name: wheels 69 | # path: dist 70 | 71 | # release: 72 | # name: Release 73 | # runs-on: ubuntu-latest 74 | # if: "startsWith(github.ref, 'refs/tags/')" 75 | # needs: [ macos, windows, linux ] 76 | # steps: 77 | # - uses: actions/download-artifact@v3 78 | # with: 79 | # name: wheels 80 | # - name: Publish to PyPI 81 | # uses: PyO3/maturin-action@v1 82 | # env: 83 | # MATURIN_PYPI_TOKEN: ${{ secrets.PYPI_API_TOKEN }} 84 | # with: 85 | # command: upload 86 | # args: --skip-existing * -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | img/ 2 | target/ 3 | dataset/ 4 | 5 | # fleet 6 | .idea/ 7 | .fleet/ 8 | .cargo/ 9 | fleet.toml 10 | rust-toolchain.toml 11 | .vscode/ 12 | 13 | # flamegraph 14 | # https://superuser.com/questions/980632/run-perf-without-root-rights 15 | # sudo sh -c 'echo 1 >/proc/sys/kernel/perf_event_paranoid' 16 | perf.data 17 | perf.data.old 18 | flamegraph.svg -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | rev: v4.4.0 4 | hooks: 5 | - id: check-yaml 6 | - id: check-toml 7 | - id: check-json 8 | - id: check-added-large-files 9 | - repo: https://github.com/doublify/pre-commit-rust 10 | rev: v1.0 11 | hooks: 12 | - id: fmt 13 | - id: cargo-check 14 | - id: clippy 15 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "robotics" 3 | version = "0.1.5" 4 | edition = "2021" 5 | license = "MIT" 6 | description = "Rust implementation of robotics algorithms" 7 | authors = [ "Jean-Gabriel Simard " ] 8 | repository = "https://github.com/jgsimard/RustRobotics" 9 | exclude = ["dataset/*"] 10 | 11 | 12 | # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html 13 | 14 | [dependencies] 15 | nalgebra = {version = "0.32", features=["rand-no-std"]} 16 | plotters = "0.3" 17 | csv = "1.3" 18 | serde = { version = "1.0", features = ["derive"] } 19 | rustc-hash = "1.1" 20 | rand = "0.8" 21 | rand_distr = "0.4" 22 | russell_lab = "0.7" 23 | russell_sparse = "0.7" 24 | plotpy = "0.5" 25 | rayon = "1.7" 26 | faer = "0.18" # fast linear algebra 27 | # # python 28 | # pyo3 = { version = "0.18", features = ["extension-module"] } 29 | # numpy = {version = "0.18", features = ["nalgebra"] } 30 | 31 | [dev-dependencies] 32 | criterion = "0.5" 33 | approx = "0.5" 34 | dialoguer = "0.11" 35 | 36 | [profile.dev.package.faer] 37 | opt-level = 3 38 | 39 | [lib] 40 | name = "robotics" 41 | # crate-type = ["cdylib", "lib"] # cdylib=python, lib=rust 42 | crate-type = ["lib"] # cdylib=python, lib=rust 43 | 44 | [profile.dev] 45 | opt-level = 1 46 | debug = 2 47 | incremental = true 48 | codegen-units = 512 49 | 50 | [profile.release] 51 | debug = true 52 | 53 | [[example]] 54 | name = "localization" 55 | path = "examples/localization/bayesian_filter.rs" 56 | 57 | [[example]] 58 | name = "localization_landmarks" 59 | path = "examples/localization/localization_landmarks.rs" 60 | 61 | [[example]] 62 | name = "pose_graph_optimization" 63 | path = "examples/mapping/pose_graph_optimization.rs" 64 | 65 | [[example]] 66 | name = "inverted_pendulum" 67 | path = "examples/control/inverted_pendulum.rs" 68 | 69 | 70 | [[bench]] 71 | name = "kalman_filter" 72 | harness = false 73 | 74 | [[bench]] 75 | name = "graph_slam" 76 | harness = false -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Jean-Gabriel Simard 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RustRobotics 2 | 3 | This package is a rust implementation of robotics algorithms. So far, the main source is the book [Probabilistic Robotics](https://mitpress.mit.edu/9780262201629/probabilistic-robotics/). I plan to have algorithms implementations in the `src` folder and the algorithms use cases in the `examples` folder. I plan to have python bindings using [pyo3](https://github.com/PyO3/pyo3)/[maturin](https://github.com/PyO3/maturin). I am also implementing the algorithms in python using [JAX](https://jax.readthedocs.io/en/latest/) in this [repo](https://github.com/jgsimard/jaxrobot). 4 | 5 | ## Table of Contents 6 | 7 | - [RustRobotics](#rustrobotics) 8 | - [Table of Contents](#table-of-contents) 9 | - [Localization](#localization) 10 | - [Extended Kalman Filter + Unscented Kalman Filter + Particle Filter](#extended-kalman-filter--unscented-kalman-filter--particle-filter) 11 | - [EKF/PF With Landmarks](#ekfpf-with-landmarks) 12 | - [Mapping](#mapping) 13 | - [Pose Graph Optimization](#pose-graph-optimization) 14 | - [Todo](#todo) 15 | - [Sources](#sources) 16 | 17 | ## Localization 18 | 19 | ### Extended Kalman Filter + Unscented Kalman Filter + Particle Filter 20 | 21 | [EKF](src/localization/extended_kalman_filter.rs), [UKF](src/localization/unscented_kalman_filter.rs), [PF](src/localization/particle_filter.rs), [Example](examples/localization/bayesian_filter.rs) 22 | 23 | ```bash 24 | cargo run --example localization 25 | ``` 26 | 27 | ### EKF/PF With Landmarks 28 | 29 | [Example](examples/localization/localization_landmarks.rs) 30 | 31 | ```bash 32 | cargo run --example localization_landmarks 33 | ``` 34 | 35 | ## Mapping 36 | 37 | ### Pose Graph Optimization 38 | 39 | This algorithm uses the sparse solver in [Russel](https://github.com/cpmech/russell/tree/main/russell_sparse) which wraps [SuiteSparse](https://people.engr.tamu.edu/davis/suitesparse.html) so follow the installation instructions. [Algorithm](src/mapping/pose_graph_optimization.rs), [Example](examples/mapping/pose_graph_optimization.rs), [Source](https://www.researchgate.net/profile/Mohamed-Mourad-Lafifi/post/What_is_the_relationship_between_GraphSLAM_and_Pose_Graph_SLAM/attachment/613b3f63647f3906fc978272/AS%3A1066449581928450%401631272802870/download/A+tutorial+on+graph-based+SLAM+_+Grisetti2010.pdf) 40 | 41 | ```bash 42 | cargo run --example pose_graph_optimization 43 | ``` 44 | 45 | ## Todo 46 | 47 | - Bayesian Filters 48 | - Information filter 49 | - Histogram filter 50 | - Pose Graph Optimization 51 | - PGO on manifold (3D) 52 | - Robust Kernels / Adaptive Kernels 53 | - Mapping 54 | - Occupancy Grid 55 | - Iterative Closest Point 56 | - EKF-SLAM 57 | - FastSLAM 1.0 58 | - FastSLAM 2.0 59 | - Camera Calibration 60 | - Direct Linear Transform (DLT) 61 | - Zhang’s Method 62 | - Projective 3-Point (P3P) 63 | - Bundle Adjustement 64 | - Triangulation 65 | - Optimal Control 66 | - LQR 67 | - LQG 68 | - etc 69 | 70 | ## Sources 71 | 72 | - [Probabilistic Robotics](https://mitpress.mit.edu/9780262201629/probabilistic-robotics/) 73 | - [PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics) 74 | - [Underactuated Robotics](https://underactuated.mit.edu/index.html) 75 | - [Probabilistic-Robotics-Algorithms](https://github.com/ChengeYang/Probabilistic-Robotics-Algorithms) 76 | - [A tutorial on Graph-Based SLAM](https://www.researchgate.net/profile/Mohamed-Mourad-Lafifi/post/What_is_the_relationship_between_GraphSLAM_and_Pose_Graph_SLAM/attachment/613b3f63647f3906fc978272/AS%3A1066449581928450%401631272802870/download/A+tutorial+on+graph-based+SLAM+_+Grisetti2010.pdf) 77 | - [A tutorial on SE(3) transformation parameterizations and on-manifold optimization](https://arxiv.org/abs/2103.15980) 78 | - [Courses from Dr. Cyrill Stachniss](https://www.ipb.uni-bonn.de/teaching) 79 | 80 | ## Compilation issues 81 | 82 | For plotters 83 | 84 | ```bash 85 | sudo apt install fontconfig libfontconfig libfontconfig1-dev 86 | ``` 87 | 88 | For Russel 89 | 90 | ```bash 91 | sudo apt install liblapacke-dev libmumps-seq-dev libopenblas-dev libsuitesparse-dev 92 | ``` 93 | -------------------------------------------------------------------------------- /benches/graph_slam.rs: -------------------------------------------------------------------------------- 1 | use criterion::{criterion_group, criterion_main, Criterion}; 2 | 3 | extern crate robotics; 4 | use robotics::mapping::{PoseGraph, PoseGraphSolver}; 5 | 6 | fn graph_slam_intel(b: &mut Criterion) { 7 | b.bench_function("graph_slam_intel", |b| { 8 | b.iter(|| { 9 | PoseGraph::new("dataset/g2o/intel.g2o", PoseGraphSolver::GaussNewton)? 10 | .optimize(10, false, false) 11 | }) 12 | }); 13 | } 14 | 15 | criterion_group!(benches, graph_slam_intel); 16 | criterion_main!(benches); 17 | -------------------------------------------------------------------------------- /benches/kalman_filter.rs: -------------------------------------------------------------------------------- 1 | use criterion::{criterion_group, criterion_main, Criterion}; 2 | 3 | use nalgebra::{Const, Matrix4, Vector2, Vector4}; 4 | extern crate robotics; 5 | use robotics::localization::{BayesianFilter, ExtendedKalmanFilter, UnscentedKalmanFilter}; 6 | use robotics::models::measurement::SimpleProblemMeasurementModel; 7 | use robotics::models::motion::SimpleProblemMotionModel; 8 | use robotics::utils::deg2rad; 9 | use robotics::utils::state::GaussianState; 10 | 11 | fn ekf(b: &mut Criterion) { 12 | // setup ekf 13 | let q = Matrix4::::from_diagonal(&Vector4::new(0.1, 0.1, deg2rad(1.0), 1.0)); 14 | let r = nalgebra::Matrix2::identity(); 15 | let motion_model = SimpleProblemMotionModel::new(); 16 | let measurement_model = SimpleProblemMeasurementModel::new(); 17 | let initial_state = GaussianState { 18 | x: Vector4::::new(0., 0., 0., 0.), 19 | cov: Matrix4::::identity(), 20 | }; 21 | let mut ekf = ExtendedKalmanFilter::, Const<2>, Const<2>>::new( 22 | q, 23 | r, 24 | measurement_model, 25 | motion_model, 26 | initial_state, 27 | ); 28 | 29 | let dt = 0.1; 30 | let u: Vector2 = Default::default(); 31 | let z: Vector2 = Default::default(); 32 | 33 | b.bench_function("ekf", |b| b.iter(|| ekf.update_estimate(&u, &z, dt))); 34 | } 35 | 36 | fn ukf(b: &mut Criterion) { 37 | // setup ukf 38 | let dt = 0.1; 39 | let q = Matrix4::::from_diagonal(&Vector4::new(0.1, 0.1, deg2rad(1.0), 1.0)); 40 | let r = nalgebra::Matrix2::identity(); //Observation x,y position covariance 41 | let initial_state = GaussianState { 42 | x: Vector4::::new(0., 0., 0., 0.), 43 | cov: Matrix4::::identity(), 44 | }; 45 | let mut ukf = UnscentedKalmanFilter::, Const<2>, Const<2>>::new( 46 | q, 47 | r, 48 | Box::new(SimpleProblemMeasurementModel {}), 49 | Box::new(SimpleProblemMotionModel {}), 50 | 0.1, 51 | 2.0, 52 | 0.0, 53 | initial_state, 54 | ); 55 | 56 | let u: Vector2 = Default::default(); 57 | let z: Vector2 = Default::default(); 58 | 59 | b.bench_function("ukf", |b| b.iter(|| ukf.update_estimate(&u, &z, dt))); 60 | } 61 | 62 | criterion_group!(benches, ekf, ukf); 63 | criterion_main!(benches); 64 | -------------------------------------------------------------------------------- /dataset/g2o/simulation-pose-landmark.g2o: -------------------------------------------------------------------------------- 1 | VERTEX_XY 1 8.766820 -2.356790 2 | VERTEX_XY 4 0.470347 -0.026934 3 | VERTEX_XY 6 -0.483767 -10.932700 4 | VERTEX_XY 9 2.272910 -2.120260 5 | VERTEX_XY 15 3.484110 -8.456790 6 | VERTEX_XY 16 3.909760 -1.616410 7 | VERTEX_XY 26 -1.179070 -9.841120 8 | VERTEX_XY 27 -0.090121 2.445970 9 | VERTEX_XY 34 7.802900 -3.552070 10 | VERTEX_XY 38 2.338060 -5.254240 11 | VERTEX_XY 43 -1.153190 -11.302900 12 | VERTEX_XY 48 -0.684117 3.654230 13 | VERTEX_XY 50 -3.216620 -3.022020 14 | VERTEX_XY 53 -2.688430 -2.131060 15 | VERTEX_XY 54 4.641900 -10.747800 16 | VERTEX_XY 55 9.346370 -4.965970 17 | VERTEX_XY 59 2.923280 -6.976690 18 | VERTEX_XY 60 -1.604410 3.914350 19 | VERTEX_XY 61 6.393350 1.277030 20 | VERTEX_XY 64 2.299830 -2.062770 21 | VERTEX_XY 67 8.183010 -1.104190 22 | VERTEX_XY 68 -0.434014 0.000412 23 | VERTEX_XY 69 0.177968 -6.523700 24 | VERTEX_XY 71 -2.938300 -6.826110 25 | VERTEX_XY 74 7.514110 -1.664350 26 | VERTEX_XY 75 3.938300 -9.485920 27 | VERTEX_XY 77 -2.713120 1.731110 28 | VERTEX_XY 78 4.910290 -0.398389 29 | VERTEX_XY 81 -4.840960 1.702820 30 | VERTEX_XY 84 0.850685 -8.880190 31 | VERTEX_XY 86 -0.895229 -9.765950 32 | VERTEX_XY 88 -0.178745 -1.343300 33 | VERTEX_XY 90 6.443940 -7.693240 34 | VERTEX_XY 91 -0.103525 -9.338990 35 | VERTEX_XY 92 2.279870 -9.617040 36 | VERTEX_XY 94 4.846660 3.396620 37 | VERTEX_SE2 100 0.000000 0.000000 0.000000 38 | VERTEX_SE2 101 0.112639 0.023666 -1.570660 39 | VERTEX_SE2 102 0.214228 -0.962739 -1.608030 40 | VERTEX_SE2 103 0.162134 -0.593629 -0.051150 41 | VERTEX_SE2 104 1.131140 -0.721783 -0.050961 42 | VERTEX_SE2 105 2.018290 -0.858984 -0.102944 43 | VERTEX_SE2 106 2.938090 -0.985912 -0.107239 44 | VERTEX_SE2 107 4.049660 -1.122950 -0.080536 45 | VERTEX_SE2 108 5.103160 -1.312120 -0.063951 46 | VERTEX_SE2 109 6.026750 -1.177160 -0.054834 47 | VERTEX_SE2 110 5.934210 -1.220510 1.506750 48 | VERTEX_SE2 111 5.865880 -1.192130 3.089660 49 | VERTEX_SE2 112 5.040540 -1.202350 3.103650 50 | VERTEX_SE2 113 4.139540 -1.162920 -3.102600 51 | VERTEX_SE2 114 4.118370 -1.300600 -1.564630 52 | VERTEX_SE2 115 4.139750 -1.268970 -3.122990 53 | VERTEX_SE2 116 3.105270 -1.270390 3.137980 54 | VERTEX_SE2 117 2.149100 -1.384200 3.132540 55 | VERTEX_SE2 118 1.108750 -1.365550 3.118810 56 | VERTEX_SE2 119 0.053260 -1.214910 3.068740 57 | VERTEX_SE2 120 -0.869202 -1.260490 3.045960 58 | VERTEX_SE2 121 -1.769580 -1.054030 3.024020 59 | VERTEX_SE2 122 -1.795350 -1.199830 1.461140 60 | VERTEX_SE2 123 -1.632270 -0.145652 1.526280 61 | VERTEX_SE2 124 -1.479510 -0.157290 -0.032089 62 | VERTEX_SE2 125 -1.619390 -0.012726 -1.642650 63 | VERTEX_SE2 126 -1.632390 -0.070816 -0.061489 64 | VERTEX_SE2 127 -0.666610 -0.054117 -0.053950 65 | VERTEX_SE2 128 0.378644 -0.239880 -0.033300 66 | VERTEX_SE2 129 1.553750 -0.371699 -0.020439 67 | VERTEX_SE2 130 2.286690 -0.293061 0.000597 68 | VERTEX_SE2 131 2.139080 -0.295857 -1.622020 69 | VERTEX_SE2 132 2.081040 -1.332190 -1.638020 70 | VERTEX_SE2 133 2.141230 -2.329460 -1.643950 71 | VERTEX_SE2 134 1.905200 -3.311560 -1.641340 72 | VERTEX_SE2 135 1.910990 -4.344790 -1.684170 73 | VERTEX_SE2 136 1.918420 -5.395240 -1.694810 74 | VERTEX_SE2 137 1.803640 -6.410800 -1.701710 75 | VERTEX_SE2 138 1.582390 -7.440070 -1.688030 76 | VERTEX_SE2 139 1.640240 -7.394220 -0.074381 77 | VERTEX_SE2 140 1.629070 -7.278060 -1.584880 78 | EDGE_SE2 100 101 0.112639 0.023666 -1.570660 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 79 | EDGE_SE2_XY 101 4 0.050648 0.357701 100.000000 0.000000 100.000000 80 | EDGE_SE2_XY 101 9 2.144220 2.159980 100.000000 0.000000 100.000000 81 | EDGE_SE2_XY 101 16 1.640590 3.796900 100.000000 0.000000 100.000000 82 | EDGE_SE2_XY 101 50 3.045230 -3.329670 100.000000 0.000000 100.000000 83 | EDGE_SE2_XY 101 53 2.154340 -2.801360 100.000000 0.000000 100.000000 84 | EDGE_SE2_XY 101 64 2.086730 2.186910 100.000000 0.000000 100.000000 85 | EDGE_SE2_XY 101 68 0.023179 -0.546656 100.000000 0.000000 100.000000 86 | EDGE_SE2_XY 101 78 0.422709 4.797590 100.000000 0.000000 100.000000 87 | EDGE_SE2_XY 101 88 1.366930 -0.291570 100.000000 0.000000 100.000000 88 | EDGE_SE2 101 102 0.986418 0.101455 -0.037369 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 89 | EDGE_SE2_XY 102 9 1.151430 2.142290 100.000000 0.000000 100.000000 90 | EDGE_SE2_XY 102 16 0.742774 3.712520 100.000000 0.000000 100.000000 91 | EDGE_SE2_XY 102 50 2.059600 -3.211420 100.000000 0.000000 100.000000 92 | EDGE_SE2_XY 102 53 0.700505 -2.563650 100.000000 0.000000 100.000000 93 | EDGE_SE2_XY 102 64 1.246650 2.190280 100.000000 0.000000 100.000000 94 | EDGE_SE2_XY 102 88 0.282219 -0.288983 100.000000 0.000000 100.000000 95 | EDGE_SE2 102 103 -0.366915 -0.065799 1.556880 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 96 | EDGE_SE2_XY 103 4 0.250282 1.001620 100.000000 0.000000 100.000000 97 | EDGE_SE2_XY 103 9 2.135610 -0.993277 100.000000 0.000000 100.000000 98 | EDGE_SE2_XY 103 16 3.649580 -0.605277 100.000000 0.000000 100.000000 99 | EDGE_SE2_XY 103 64 2.309610 -1.315970 100.000000 0.000000 100.000000 100 | EDGE_SE2_XY 103 78 4.956740 0.728739 100.000000 0.000000 100.000000 101 | EDGE_SE2 103 104 0.974291 -0.078444 0.000188 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 102 | EDGE_SE2_XY 104 9 1.133030 -1.135390 100.000000 0.000000 100.000000 103 | EDGE_SE2_XY 104 16 2.673870 -0.689828 100.000000 0.000000 100.000000 104 | EDGE_SE2_XY 104 38 1.436230 -4.465090 100.000000 0.000000 100.000000 105 | EDGE_SE2_XY 104 64 1.247530 -1.288060 100.000000 0.000000 100.000000 106 | EDGE_SE2_XY 104 78 3.917730 0.602721 100.000000 0.000000 100.000000 107 | EDGE_SE2 104 105 0.892991 -0.091832 -0.051982 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 108 | EDGE_SE2_XY 105 9 0.334295 -1.048790 100.000000 0.000000 100.000000 109 | EDGE_SE2_XY 105 16 1.565830 -0.667554 100.000000 0.000000 100.000000 110 | EDGE_SE2_XY 105 38 0.353493 -4.333130 100.000000 0.000000 100.000000 111 | EDGE_SE2_XY 105 64 0.138794 -1.376160 100.000000 0.000000 100.000000 112 | EDGE_SE2_XY 105 78 3.032360 0.598071 100.000000 0.000000 100.000000 113 | EDGE_SE2 105 106 0.927972 -0.031735 -0.004295 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 114 | EDGE_SE2_XY 106 16 0.611426 -0.777877 100.000000 0.000000 100.000000 115 | EDGE_SE2_XY 106 61 3.193200 2.619770 100.000000 0.000000 100.000000 116 | EDGE_SE2_XY 106 74 4.622350 -0.184755 100.000000 0.000000 100.000000 117 | EDGE_SE2_XY 106 78 2.027660 0.438725 100.000000 0.000000 100.000000 118 | EDGE_SE2_XY 106 94 1.428530 4.561640 100.000000 0.000000 100.000000 119 | EDGE_SE2 106 107 1.119850 -0.017272 0.026703 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 120 | EDGE_SE2_XY 107 1 4.801130 -0.850361 100.000000 0.000000 100.000000 121 | EDGE_SE2_XY 107 34 3.936490 -2.119310 100.000000 0.000000 100.000000 122 | EDGE_SE2_XY 107 61 2.118150 2.495020 100.000000 0.000000 100.000000 123 | EDGE_SE2_XY 107 67 4.118440 0.351219 100.000000 0.000000 100.000000 124 | EDGE_SE2_XY 107 74 3.646630 -0.246854 100.000000 0.000000 100.000000 125 | EDGE_SE2_XY 107 78 1.037980 0.509298 100.000000 0.000000 100.000000 126 | EDGE_SE2_XY 107 94 0.116959 4.486480 100.000000 0.000000 100.000000 127 | EDGE_SE2 107 108 1.065300 -0.103804 0.016584 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 128 | EDGE_SE2_XY 108 1 3.962890 -0.938155 100.000000 0.000000 100.000000 129 | EDGE_SE2_XY 108 34 2.937850 -1.822160 100.000000 0.000000 100.000000 130 | EDGE_SE2_XY 108 61 1.356190 2.687290 100.000000 0.000000 100.000000 131 | EDGE_SE2_XY 108 67 3.315140 0.262478 100.000000 0.000000 100.000000 132 | EDGE_SE2_XY 108 74 2.449980 -0.042387 100.000000 0.000000 100.000000 133 | EDGE_SE2 108 109 0.913077 0.193701 0.009118 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 134 | EDGE_SE2_XY 109 1 2.708970 -0.850427 100.000000 0.000000 100.000000 135 | EDGE_SE2_XY 109 34 1.846600 -1.988320 100.000000 0.000000 100.000000 136 | EDGE_SE2_XY 109 55 3.522280 -3.601180 100.000000 0.000000 100.000000 137 | EDGE_SE2_XY 109 61 -0.102777 2.457150 100.000000 0.000000 100.000000 138 | EDGE_SE2_XY 109 67 2.320670 0.440339 100.000000 0.000000 100.000000 139 | EDGE_SE2_XY 109 74 1.692470 -0.345207 100.000000 0.000000 100.000000 140 | EDGE_SE2 109 110 -0.090025 -0.048352 1.561580 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 141 | EDGE_SE2_XY 110 61 2.746980 -0.296250 100.000000 0.000000 100.000000 142 | EDGE_SE2_XY 110 67 0.432055 -2.300700 100.000000 0.000000 100.000000 143 | EDGE_SE2_XY 110 78 0.463640 1.175350 100.000000 0.000000 100.000000 144 | EDGE_SE2_XY 110 94 4.530620 1.820750 100.000000 0.000000 100.000000 145 | EDGE_SE2 110 111 0.023951 0.070007 1.582910 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 146 | EDGE_SE2_XY 111 9 3.794580 1.083680 100.000000 0.000000 100.000000 147 | EDGE_SE2_XY 111 16 2.356860 0.793858 100.000000 0.000000 100.000000 148 | EDGE_SE2_XY 111 64 3.553000 1.198040 100.000000 0.000000 100.000000 149 | EDGE_SE2_XY 111 78 1.228690 -0.430377 100.000000 0.000000 100.000000 150 | EDGE_SE2_XY 111 94 1.663610 -4.560930 100.000000 0.000000 100.000000 151 | EDGE_SE2 111 112 0.823693 0.053056 0.013994 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 152 | EDGE_SE2_XY 112 4 4.694840 -0.931923 100.000000 0.000000 100.000000 153 | EDGE_SE2_XY 112 9 2.735620 0.943596 100.000000 0.000000 100.000000 154 | EDGE_SE2_XY 112 16 1.184240 0.678194 100.000000 0.000000 100.000000 155 | EDGE_SE2_XY 112 64 2.916440 1.188940 100.000000 0.000000 100.000000 156 | EDGE_SE2_XY 112 78 -0.062406 -0.487601 100.000000 0.000000 100.000000 157 | EDGE_SE2_XY 112 94 0.830293 -4.557260 100.000000 0.000000 100.000000 158 | EDGE_SE2 112 113 0.901845 -0.005228 0.076932 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 159 | EDGE_SE2_XY 113 4 3.592920 -1.229680 100.000000 0.000000 100.000000 160 | EDGE_SE2_XY 113 9 1.775480 1.059410 100.000000 0.000000 100.000000 161 | EDGE_SE2_XY 113 16 0.184329 0.583824 100.000000 0.000000 100.000000 162 | EDGE_SE2_XY 113 38 1.677650 4.279340 100.000000 0.000000 100.000000 163 | EDGE_SE2_XY 113 64 1.740720 1.117390 100.000000 0.000000 100.000000 164 | EDGE_SE2_XY 113 68 4.473040 -0.962370 100.000000 0.000000 100.000000 165 | EDGE_SE2_XY 113 88 4.247980 0.309290 100.000000 0.000000 100.000000 166 | EDGE_SE2 113 114 0.026523 0.136752 1.537970 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 167 | EDGE_SE2_XY 114 1 0.892507 4.780980 100.000000 0.000000 100.000000 168 | EDGE_SE2_XY 114 9 1.008280 -1.955550 100.000000 0.000000 100.000000 169 | EDGE_SE2_XY 114 16 0.757650 -0.232404 100.000000 0.000000 100.000000 170 | EDGE_SE2_XY 114 34 1.980970 3.937600 100.000000 0.000000 100.000000 171 | EDGE_SE2_XY 114 38 4.347810 -1.473450 100.000000 0.000000 100.000000 172 | EDGE_SE2_XY 114 64 1.094530 -1.781520 100.000000 0.000000 100.000000 173 | EDGE_SE2_XY 114 74 0.258633 3.503790 100.000000 0.000000 100.000000 174 | EDGE_SE2_XY 114 88 0.268932 -4.355080 100.000000 0.000000 100.000000 175 | EDGE_SE2 114 115 -0.031496 0.021572 -1.558360 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 176 | EDGE_SE2_XY 115 4 3.545330 -0.875093 100.000000 0.000000 100.000000 177 | EDGE_SE2_XY 115 9 1.964280 1.067710 100.000000 0.000000 100.000000 178 | EDGE_SE2_XY 115 16 0.251742 0.651880 100.000000 0.000000 100.000000 179 | EDGE_SE2_XY 115 38 1.612860 4.457780 100.000000 0.000000 100.000000 180 | EDGE_SE2_XY 115 64 1.621790 1.196050 100.000000 0.000000 100.000000 181 | EDGE_SE2_XY 115 68 4.396850 -0.924717 100.000000 0.000000 100.000000 182 | EDGE_SE2_XY 115 88 4.230160 0.208377 100.000000 0.000000 100.000000 183 | EDGE_SE2 115 116 1.034320 -0.017821 -0.022214 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 184 | EDGE_SE2_XY 116 4 2.459420 -0.984438 100.000000 0.000000 100.000000 185 | EDGE_SE2_XY 116 9 0.784888 1.057820 100.000000 0.000000 100.000000 186 | EDGE_SE2_XY 116 38 0.591497 4.407990 100.000000 0.000000 100.000000 187 | EDGE_SE2_XY 116 64 0.744106 1.029550 100.000000 0.000000 100.000000 188 | EDGE_SE2_XY 116 68 3.486860 -0.941213 100.000000 0.000000 100.000000 189 | EDGE_SE2_XY 116 88 3.308750 0.382677 100.000000 0.000000 100.000000 190 | EDGE_SE2 116 117 0.955761 0.117263 -0.005442 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 191 | EDGE_SE2_XY 117 4 1.489020 -0.926827 100.000000 0.000000 100.000000 192 | EDGE_SE2_XY 117 27 2.273810 -3.809730 100.000000 0.000000 100.000000 193 | EDGE_SE2_XY 117 53 4.782690 0.698023 100.000000 0.000000 100.000000 194 | EDGE_SE2_XY 117 68 2.342010 -0.894894 100.000000 0.000000 100.000000 195 | EDGE_SE2_XY 117 88 2.198140 0.186304 100.000000 0.000000 100.000000 196 | EDGE_SE2 117 118 1.040470 -0.009221 -0.013729 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 197 | EDGE_SE2_XY 118 4 0.565958 -0.907369 100.000000 0.000000 100.000000 198 | EDGE_SE2_XY 118 27 1.363150 -3.785620 100.000000 0.000000 100.000000 199 | EDGE_SE2_XY 118 50 3.991830 2.007720 100.000000 0.000000 100.000000 200 | EDGE_SE2_XY 118 53 3.759230 0.761217 100.000000 0.000000 100.000000 201 | EDGE_SE2_XY 118 68 1.537520 -0.943006 100.000000 0.000000 100.000000 202 | EDGE_SE2_XY 118 77 3.891430 -3.008790 100.000000 0.000000 100.000000 203 | EDGE_SE2_XY 118 88 1.200380 0.139439 100.000000 0.000000 100.000000 204 | EDGE_SE2 118 119 1.058650 -0.126561 -0.050072 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 205 | EDGE_SE2_XY 119 48 1.089860 -4.802540 100.000000 0.000000 100.000000 206 | EDGE_SE2_XY 119 27 0.387343 -3.666600 100.000000 0.000000 100.000000 207 | EDGE_SE2_XY 119 50 3.251720 2.109280 100.000000 0.000000 100.000000 208 | EDGE_SE2_XY 119 53 2.608410 0.865697 100.000000 0.000000 100.000000 209 | EDGE_SE2_XY 119 68 0.370099 -0.980259 100.000000 0.000000 100.000000 210 | EDGE_SE2_XY 119 77 2.786620 -2.977340 100.000000 0.000000 100.000000 211 | EDGE_SE2_XY 119 88 0.214231 0.087875 100.000000 0.000000 100.000000 212 | EDGE_SE2 119 120 0.916697 0.112613 -0.022777 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 213 | EDGE_SE2_XY 120 48 -0.051164 -4.826530 100.000000 0.000000 100.000000 214 | EDGE_SE2_XY 120 50 2.244350 2.010960 100.000000 0.000000 100.000000 215 | EDGE_SE2_XY 120 53 1.782520 0.584407 100.000000 0.000000 100.000000 216 | EDGE_SE2_XY 120 77 1.636450 -2.976230 100.000000 0.000000 100.000000 217 | EDGE_SE2_XY 120 81 4.236570 -2.570510 100.000000 0.000000 100.000000 218 | EDGE_SE2 120 121 0.915983 -0.119545 -0.021943 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 219 | EDGE_SE2_XY 121 50 1.262870 2.189160 100.000000 0.000000 100.000000 220 | EDGE_SE2_XY 121 53 0.850342 0.646895 100.000000 0.000000 100.000000 221 | EDGE_SE2_XY 121 77 0.660390 -3.118950 100.000000 0.000000 100.000000 222 | EDGE_SE2_XY 121 81 2.906620 -2.554390 100.000000 0.000000 100.000000 223 | EDGE_SE2 121 122 0.008486 0.147815 -1.562880 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 224 | EDGE_SE2_XY 122 4 1.083600 -2.481120 100.000000 0.000000 100.000000 225 | EDGE_SE2_XY 122 48 4.867300 -1.072060 100.000000 0.000000 100.000000 226 | EDGE_SE2_XY 122 27 3.616910 -1.928230 100.000000 0.000000 100.000000 227 | EDGE_SE2_XY 122 68 0.910387 -1.415240 100.000000 0.000000 100.000000 228 | EDGE_SE2_XY 122 77 3.236130 0.929183 100.000000 0.000000 100.000000 229 | EDGE_SE2_XY 122 81 2.543800 3.054440 100.000000 0.000000 100.000000 230 | EDGE_SE2 122 123 1.065690 -0.046734 0.065145 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 231 | EDGE_SE2_XY 123 48 3.837270 -0.865770 100.000000 0.000000 100.000000 232 | EDGE_SE2_XY 123 27 2.804220 -1.517170 100.000000 0.000000 100.000000 233 | EDGE_SE2_XY 123 60 4.057220 0.152842 100.000000 0.000000 100.000000 234 | EDGE_SE2_XY 123 77 2.059240 0.722308 100.000000 0.000000 100.000000 235 | EDGE_SE2_XY 123 81 1.565250 3.093450 100.000000 0.000000 100.000000 236 | EDGE_SE2 123 124 -0.004828 -0.153128 -1.558370 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 237 | EDGE_SE2_XY 124 4 2.193330 -0.274996 100.000000 0.000000 100.000000 238 | EDGE_SE2_XY 124 9 4.273750 -2.066930 100.000000 0.000000 100.000000 239 | EDGE_SE2_XY 124 48 0.979423 3.922390 100.000000 0.000000 100.000000 240 | EDGE_SE2_XY 124 27 1.660180 2.782560 100.000000 0.000000 100.000000 241 | EDGE_SE2_XY 124 64 4.273040 -2.210080 100.000000 0.000000 100.000000 242 | EDGE_SE2_XY 124 68 1.631340 -0.145572 100.000000 0.000000 100.000000 243 | EDGE_SE2_XY 124 88 1.931800 -1.298520 100.000000 0.000000 100.000000 244 | EDGE_SE2 124 125 -0.144447 0.140001 -1.610560 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 245 | EDGE_SE2_XY 125 4 -0.125593 2.367140 100.000000 0.000000 100.000000 246 | EDGE_SE2_XY 125 9 2.068630 4.044220 100.000000 0.000000 100.000000 247 | EDGE_SE2_XY 125 50 3.201480 -1.121160 100.000000 0.000000 100.000000 248 | EDGE_SE2_XY 125 53 1.777360 -0.875294 100.000000 0.000000 100.000000 249 | EDGE_SE2_XY 125 64 2.204380 4.228380 100.000000 0.000000 100.000000 250 | EDGE_SE2_XY 125 68 0.320428 1.711290 100.000000 0.000000 100.000000 251 | EDGE_SE2_XY 125 88 1.288530 1.688520 100.000000 0.000000 100.000000 252 | EDGE_SE2 125 126 0.058873 -0.008795 1.581160 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 253 | EDGE_SE2_XY 126 4 2.344530 -0.148493 100.000000 0.000000 100.000000 254 | EDGE_SE2_XY 126 9 4.247190 -2.079220 100.000000 0.000000 100.000000 255 | EDGE_SE2_XY 126 48 0.973120 3.829770 100.000000 0.000000 100.000000 256 | EDGE_SE2_XY 126 27 1.733780 2.808210 100.000000 0.000000 100.000000 257 | EDGE_SE2_XY 126 64 4.307790 -2.002440 100.000000 0.000000 100.000000 258 | EDGE_SE2_XY 126 68 1.604830 0.059405 100.000000 0.000000 100.000000 259 | EDGE_SE2_XY 126 88 1.777050 -1.220080 100.000000 0.000000 100.000000 260 | EDGE_SE2 126 127 0.962925 0.076015 0.007539 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 261 | EDGE_SE2_XY 127 4 1.275800 -0.004918 100.000000 0.000000 100.000000 262 | EDGE_SE2_XY 127 9 3.159110 -2.081160 100.000000 0.000000 100.000000 263 | EDGE_SE2_XY 127 16 4.647960 -1.533330 100.000000 0.000000 100.000000 264 | EDGE_SE2_XY 127 27 0.778770 2.975750 100.000000 0.000000 100.000000 265 | EDGE_SE2_XY 127 64 3.179680 -2.207380 100.000000 0.000000 100.000000 266 | EDGE_SE2_XY 127 68 0.474571 -0.185126 100.000000 0.000000 100.000000 267 | EDGE_SE2_XY 127 88 0.826055 -1.063630 100.000000 0.000000 100.000000 268 | EDGE_SE2 127 128 1.053750 -0.129128 0.020651 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 269 | EDGE_SE2_XY 128 4 0.343894 -0.072998 100.000000 0.000000 100.000000 270 | EDGE_SE2_XY 128 9 2.073930 -2.209070 100.000000 0.000000 100.000000 271 | EDGE_SE2_XY 128 16 3.626410 -1.581700 100.000000 0.000000 100.000000 272 | EDGE_SE2_XY 128 64 2.237400 -2.295850 100.000000 0.000000 100.000000 273 | EDGE_SE2_XY 128 78 5.152660 -0.648907 100.000000 0.000000 100.000000 274 | EDGE_SE2 128 129 1.178840 -0.092623 0.012861 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 275 | EDGE_SE2_XY 129 9 1.316700 -2.098990 100.000000 0.000000 100.000000 276 | EDGE_SE2_XY 129 16 2.779280 -1.634810 100.000000 0.000000 100.000000 277 | EDGE_SE2_XY 129 64 1.300540 -2.109600 100.000000 0.000000 100.000000 278 | EDGE_SE2_XY 129 78 4.071950 -0.529102 100.000000 0.000000 100.000000 279 | EDGE_SE2_XY 129 94 3.246870 3.553900 100.000000 0.000000 100.000000 280 | EDGE_SE2 129 130 0.731183 0.093600 0.021036 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 281 | EDGE_SE2_XY 130 9 0.242102 -2.148760 100.000000 0.000000 100.000000 282 | EDGE_SE2_XY 130 16 1.813770 -1.625730 100.000000 0.000000 100.000000 283 | EDGE_SE2_XY 130 61 4.135860 1.690180 100.000000 0.000000 100.000000 284 | EDGE_SE2_XY 130 64 0.284090 -2.297150 100.000000 0.000000 100.000000 285 | EDGE_SE2_XY 130 78 2.926380 -0.412580 100.000000 0.000000 100.000000 286 | EDGE_SE2_XY 130 94 1.986160 3.523330 100.000000 0.000000 100.000000 287 | EDGE_SE2 130 131 -0.147613 -0.002708 -1.622620 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 288 | EDGE_SE2_XY 131 4 0.235783 -1.659100 100.000000 0.000000 100.000000 289 | EDGE_SE2_XY 131 9 2.049600 0.166393 100.000000 0.000000 100.000000 290 | EDGE_SE2_XY 131 16 1.724000 1.649120 100.000000 0.000000 100.000000 291 | EDGE_SE2_XY 131 64 2.115850 0.357862 100.000000 0.000000 100.000000 292 | EDGE_SE2_XY 131 68 0.005063 -2.476030 100.000000 0.000000 100.000000 293 | EDGE_SE2_XY 131 78 0.343556 2.883140 100.000000 0.000000 100.000000 294 | EDGE_SE2_XY 131 88 1.176080 -2.185350 100.000000 0.000000 100.000000 295 | EDGE_SE2 131 132 1.037940 -0.004894 -0.015993 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 296 | EDGE_SE2_XY 132 9 1.094510 0.424532 100.000000 0.000000 100.000000 297 | EDGE_SE2_XY 132 16 0.721689 1.811990 100.000000 0.000000 100.000000 298 | EDGE_SE2_XY 132 38 4.425980 0.366432 100.000000 0.000000 100.000000 299 | EDGE_SE2_XY 132 53 0.963088 -4.704460 100.000000 0.000000 100.000000 300 | EDGE_SE2_XY 132 64 1.199420 0.111833 100.000000 0.000000 100.000000 301 | EDGE_SE2_XY 132 88 0.179749 -2.158550 100.000000 0.000000 100.000000 302 | EDGE_SE2 132 133 0.990985 0.127033 -0.005935 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 303 | EDGE_SE2_XY 133 9 -0.333788 0.159970 100.000000 0.000000 100.000000 304 | EDGE_SE2_XY 133 38 3.354580 0.423673 100.000000 0.000000 100.000000 305 | EDGE_SE2_XY 133 59 4.577640 1.119630 100.000000 0.000000 100.000000 306 | EDGE_SE2_XY 133 64 0.162632 0.223420 100.000000 0.000000 100.000000 307 | EDGE_SE2_XY 133 69 4.326510 -1.651450 100.000000 0.000000 100.000000 308 | EDGE_SE2 133 134 0.996723 -0.163617 0.002607 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 309 | EDGE_SE2_XY 134 38 2.291390 0.353856 100.000000 0.000000 100.000000 310 | EDGE_SE2_XY 134 59 3.549550 1.389490 100.000000 0.000000 100.000000 311 | EDGE_SE2_XY 134 69 3.409460 -1.610470 100.000000 0.000000 100.000000 312 | EDGE_SE2 134 135 1.030250 0.078613 -0.042826 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 313 | EDGE_SE2_XY 135 15 3.907630 2.028220 100.000000 0.000000 100.000000 314 | EDGE_SE2_XY 135 38 1.405520 0.540134 100.000000 0.000000 100.000000 315 | EDGE_SE2_XY 135 59 2.852630 1.276450 100.000000 0.000000 100.000000 316 | EDGE_SE2_XY 135 69 2.384560 -1.602860 100.000000 0.000000 100.000000 317 | EDGE_SE2_XY 135 84 4.626240 -0.540402 100.000000 0.000000 100.000000 318 | EDGE_SE2 135 136 1.042860 0.126216 -0.010643 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 319 | EDGE_SE2_XY 136 15 3.069380 1.973360 100.000000 0.000000 100.000000 320 | EDGE_SE2_XY 136 38 0.502463 0.274055 100.000000 0.000000 100.000000 321 | EDGE_SE2_XY 136 59 1.779260 1.266560 100.000000 0.000000 100.000000 322 | EDGE_SE2_XY 136 69 1.419560 -1.807590 100.000000 0.000000 100.000000 323 | EDGE_SE2_XY 136 71 2.020660 -4.642420 100.000000 0.000000 100.000000 324 | EDGE_SE2_XY 136 75 3.809410 2.510390 100.000000 0.000000 100.000000 325 | EDGE_SE2_XY 136 84 3.447580 -0.557298 100.000000 0.000000 100.000000 326 | EDGE_SE2_XY 136 90 1.720550 4.775030 100.000000 0.000000 100.000000 327 | EDGE_SE2_XY 136 91 4.163580 -1.518570 100.000000 0.000000 100.000000 328 | EDGE_SE2_XY 136 92 4.144670 0.880919 100.000000 0.000000 100.000000 329 | EDGE_SE2 136 137 1.021960 0.011729 -0.006900 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 330 | EDGE_SE2_XY 137 15 2.093300 1.827650 100.000000 0.000000 100.000000 331 | EDGE_SE2_XY 137 26 3.790340 -2.509380 100.000000 0.000000 100.000000 332 | EDGE_SE2_XY 137 59 0.613190 1.474080 100.000000 0.000000 100.000000 333 | EDGE_SE2_XY 137 69 0.393518 -1.716760 100.000000 0.000000 100.000000 334 | EDGE_SE2_XY 137 71 0.959921 -4.403530 100.000000 0.000000 100.000000 335 | EDGE_SE2_XY 137 75 2.773380 2.253600 100.000000 0.000000 100.000000 336 | EDGE_SE2_XY 137 84 2.320730 -0.689835 100.000000 0.000000 100.000000 337 | EDGE_SE2_XY 137 86 3.678760 -2.237780 100.000000 0.000000 100.000000 338 | EDGE_SE2_XY 137 90 0.625952 4.738230 100.000000 0.000000 100.000000 339 | EDGE_SE2_XY 137 91 3.141510 -1.596470 100.000000 0.000000 100.000000 340 | EDGE_SE2_XY 137 92 3.205130 0.901206 100.000000 0.000000 100.000000 341 | EDGE_SE2 137 138 1.049350 -0.084995 0.013683 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 342 | EDGE_SE2_XY 138 6 3.710350 -1.643450 100.000000 0.000000 100.000000 343 | EDGE_SE2_XY 138 15 0.970135 2.198810 100.000000 0.000000 100.000000 344 | EDGE_SE2_XY 138 26 2.907280 -2.604260 100.000000 0.000000 100.000000 345 | EDGE_SE2_XY 138 43 4.156240 -2.264980 100.000000 0.000000 100.000000 346 | EDGE_SE2_XY 138 54 2.927120 3.425400 100.000000 0.000000 100.000000 347 | EDGE_SE2_XY 138 71 -0.096328 -4.665840 100.000000 0.000000 100.000000 348 | EDGE_SE2_XY 138 75 1.878670 2.575350 100.000000 0.000000 100.000000 349 | EDGE_SE2_XY 138 84 1.548550 -0.601182 100.000000 0.000000 100.000000 350 | EDGE_SE2_XY 138 86 2.722100 -2.217540 100.000000 0.000000 100.000000 351 | EDGE_SE2_XY 138 91 2.066180 -1.466910 100.000000 0.000000 100.000000 352 | EDGE_SE2_XY 138 92 1.837800 1.044950 100.000000 0.000000 100.000000 353 | EDGE_SE2 138 139 -0.052304 0.052098 1.613650 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 354 | EDGE_SE2_XY 139 9 -0.022085 4.923620 100.000000 0.000000 100.000000 355 | EDGE_SE2_XY 139 15 2.008080 -0.990410 100.000000 0.000000 100.000000 356 | EDGE_SE2_XY 139 38 0.569294 1.628350 100.000000 0.000000 100.000000 357 | EDGE_SE2_XY 139 54 3.673130 -2.654320 100.000000 0.000000 100.000000 358 | EDGE_SE2_XY 139 59 1.110840 0.356639 100.000000 0.000000 100.000000 359 | EDGE_SE2_XY 139 64 0.336079 4.719980 100.000000 0.000000 100.000000 360 | EDGE_SE2_XY 139 75 2.500080 -1.882420 100.000000 0.000000 100.000000 361 | EDGE_SE2_XY 139 90 4.577750 0.319868 100.000000 0.000000 100.000000 362 | EDGE_SE2_XY 139 92 0.858560 -1.891930 100.000000 0.000000 100.000000 363 | EDGE_SE2 139 140 -0.019771 0.115010 -1.510500 100.000000 0.000000 0.000000 100.000000 0.000000 1000.000000 364 | EDGE_SE2_XY 140 6 3.787530 -1.868020 100.000000 0.000000 100.000000 365 | EDGE_SE2_XY 140 15 0.919415 2.168730 100.000000 0.000000 100.000000 366 | EDGE_SE2_XY 140 26 2.723340 -2.535050 100.000000 0.000000 100.000000 367 | EDGE_SE2_XY 140 43 4.115400 -2.243150 100.000000 0.000000 100.000000 368 | EDGE_SE2_XY 140 54 2.817830 3.209560 100.000000 0.000000 100.000000 369 | EDGE_SE2_XY 140 71 -0.106658 -4.606880 100.000000 0.000000 100.000000 370 | EDGE_SE2_XY 140 75 1.900440 2.435410 100.000000 0.000000 100.000000 371 | EDGE_SE2_XY 140 84 1.409600 -0.651416 100.000000 0.000000 100.000000 372 | EDGE_SE2_XY 140 86 2.775200 -2.285800 100.000000 0.000000 100.000000 373 | EDGE_SE2_XY 140 91 2.285430 -1.276190 100.000000 0.000000 100.000000 374 | EDGE_SE2_XY 140 92 2.065690 0.830742 100.000000 0.000000 100.000000 375 | -------------------------------------------------------------------------------- /dataset/slam_course/world.dat: -------------------------------------------------------------------------------- 1 | 1 2 1 2 | 2 0 4 3 | 3 2 7 4 | 4 9 2 5 | 5 10 5 6 | 6 9 8 7 | 7 5 5 8 | 8 5 3 9 | 9 5 9 10 | -------------------------------------------------------------------------------- /dataset/utias0/Barcodes.csv: -------------------------------------------------------------------------------- 1 | subject_nb,barcode_nb 2 | 1,5 3 | 2,14 4 | 3,41 5 | 4,32 6 | 5,23 7 | 6,45 8 | 7,90 9 | 8,72 10 | 9,9 11 | 10,63 12 | 11,18 13 | 12,81 14 | 13,27 15 | 14,61 16 | 15,7 17 | 16,16 18 | 17,36 19 | 18,54 20 | 19,25 21 | 20,70 -------------------------------------------------------------------------------- /dataset/utias0/Landmark_Groundtruth.csv: -------------------------------------------------------------------------------- 1 | subject_nb,x,y,x_std_dev,y_std_dev 2 | 6,0.48704624,-4.95127346,0.0000302,0.00017939 3 | 7,3.12907696,-5.5581163,0.00003312,0.00015935 4 | 8,2.65345619,-3.75123336,0.00004179,0.00023282 5 | 9,0.97259759,-3.19151915,0.00007947,0.00022909 6 | 10,0.84527678,-1.61673856,0.00006391,0.00026239 7 | 11,3.72073503,-1.98025384,0.00013685,0.0002793 8 | 12,1.91856554,-0.82058089,0.00018287,0.00020971 9 | 13,0.91765949,0.59631939,0.00006368,0.00033948 10 | 14,2.76327261,0.24394752,0.00009005,0.00017049 11 | 15,4.6723925,0.8440939,0.00014602,0.00027795 12 | 16,0.95289638,2.7093334,0.00033774,0.0001696 13 | 17,3.26730988,2.52731607,0.00107665,0.00298502 14 | 18,0.8891764,4.40906195,0.00005517,0.00010895 15 | 19,2.39221778,3.80018838,0.00025545,0.00256092 16 | 20,4.13634588,3.60883503,0.00007982,0.00050874 17 | -------------------------------------------------------------------------------- /deny.toml: -------------------------------------------------------------------------------- 1 | # This template contains all of the possible sections and their default values 2 | 3 | # Note that all fields that take a lint level have these possible values: 4 | # * deny - An error will be produced and the check will fail 5 | # * warn - A warning will be produced, but the check will not fail 6 | # * allow - No warning or error will be produced, though in some cases a note 7 | # will be 8 | 9 | # The values provided in this template are the default values that will be used 10 | # when any section or field is not specified in your own configuration 11 | 12 | # Root options 13 | 14 | # If 1 or more target triples (and optionally, target_features) are specified, 15 | # only the specified targets will be checked when running `cargo deny check`. 16 | # This means, if a particular package is only ever used as a target specific 17 | # dependency, such as, for example, the `nix` crate only being used via the 18 | # `target_family = "unix"` configuration, that only having windows targets in 19 | # this list would mean the nix crate, as well as any of its exclusive 20 | # dependencies not shared by any other crates, would be ignored, as the target 21 | # list here is effectively saying which targets you are building for. 22 | targets = [ 23 | # The triple can be any string, but only the target triples built in to 24 | # rustc (as of 1.40) can be checked against actual config expressions 25 | #{ triple = "x86_64-unknown-linux-musl" }, 26 | # You can also specify which target_features you promise are enabled for a 27 | # particular target. target_features are currently not validated against 28 | # the actual valid features supported by the target architecture. 29 | #{ triple = "wasm32-unknown-unknown", features = ["atomics"] }, 30 | ] 31 | # When creating the dependency graph used as the source of truth when checks are 32 | # executed, this field can be used to prune crates from the graph, removing them 33 | # from the view of cargo-deny. This is an extremely heavy hammer, as if a crate 34 | # is pruned from the graph, all of its dependencies will also be pruned unless 35 | # they are connected to another crate in the graph that hasn't been pruned, 36 | # so it should be used with care. The identifiers are [Package ID Specifications] 37 | # (https://doc.rust-lang.org/cargo/reference/pkgid-spec.html) 38 | #exclude = [] 39 | # If true, metadata will be collected with `--all-features`. Note that this can't 40 | # be toggled off if true, if you want to conditionally enable `--all-features` it 41 | # is recommended to pass `--all-features` on the cmd line instead 42 | all-features = false 43 | # If true, metadata will be collected with `--no-default-features`. The same 44 | # caveat with `all-features` applies 45 | no-default-features = false 46 | # If set, these feature will be enabled when collecting metadata. If `--features` 47 | # is specified on the cmd line they will take precedence over this option. 48 | #features = [] 49 | # When outputting inclusion graphs in diagnostics that include features, this 50 | # option can be used to specify the depth at which feature edges will be added. 51 | # This option is included since the graphs can be quite large and the addition 52 | # of features from the crate(s) to all of the graph roots can be far too verbose. 53 | # This option can be overridden via `--feature-depth` on the cmd line 54 | feature-depth = 1 55 | 56 | # This section is considered when running `cargo deny check advisories` 57 | # More documentation for the advisories section can be found here: 58 | # https://embarkstudios.github.io/cargo-deny/checks/advisories/cfg.html 59 | [advisories] 60 | # The path where the advisory database is cloned/fetched into 61 | db-path = "~/.cargo/advisory-db" 62 | # The url(s) of the advisory databases to use 63 | db-urls = ["https://github.com/rustsec/advisory-db"] 64 | # The lint level for security vulnerabilities 65 | vulnerability = "deny" 66 | # The lint level for unmaintained crates 67 | unmaintained = "warn" 68 | # The lint level for crates that have been yanked from their source registry 69 | yanked = "warn" 70 | # The lint level for crates with security notices. Note that as of 71 | # 2019-12-17 there are no security notice advisories in 72 | # https://github.com/rustsec/advisory-db 73 | notice = "warn" 74 | # A list of advisory IDs to ignore. Note that ignored advisories will still 75 | # output a note when they are encountered. 76 | ignore = [ 77 | #"RUSTSEC-0000-0000", 78 | ] 79 | # Threshold for security vulnerabilities, any vulnerability with a CVSS score 80 | # lower than the range specified will be ignored. Note that ignored advisories 81 | # will still output a note when they are encountered. 82 | # * None - CVSS Score 0.0 83 | # * Low - CVSS Score 0.1 - 3.9 84 | # * Medium - CVSS Score 4.0 - 6.9 85 | # * High - CVSS Score 7.0 - 8.9 86 | # * Critical - CVSS Score 9.0 - 10.0 87 | #severity-threshold = 88 | 89 | # If this is true, then cargo deny will use the git executable to fetch advisory database. 90 | # If this is false, then it uses a built-in git library. 91 | # Setting this to true can be helpful if you have special authentication requirements that cargo-deny does not support. 92 | # See Git Authentication for more information about setting up git authentication. 93 | #git-fetch-with-cli = true 94 | 95 | # This section is considered when running `cargo deny check licenses` 96 | # More documentation for the licenses section can be found here: 97 | # https://embarkstudios.github.io/cargo-deny/checks/licenses/cfg.html 98 | [licenses] 99 | # The lint level for crates which do not have a detectable license 100 | unlicensed = "deny" 101 | # List of explicitly allowed licenses 102 | # See https://spdx.org/licenses/ for list of possible licenses 103 | # [possible values: any SPDX 3.11 short identifier (+ optional exception)]. 104 | allow = [ 105 | "MIT", 106 | "Apache-2.0", # nalgebra -> simba, numpy -> pyo3-macros/pyo3-ffi/pyo3-build-config/pyo3, etc 107 | "Apache-2.0 WITH LLVM-exception", # pyo3 -> target-lexicon 108 | "BSD-2-Clause", # numpy 109 | "BSD-3-Clause", # nalgebra, nalgebra-lapack, 110 | "Unicode-DFS-2016" # unicode-ident 111 | ] 112 | # List of explicitly disallowed licenses 113 | # See https://spdx.org/licenses/ for list of possible licenses 114 | # [possible values: any SPDX 3.11 short identifier (+ optional exception)]. 115 | deny = [ 116 | #"Nokia", 117 | ] 118 | # Lint level for licenses considered copyleft 119 | copyleft = "warn" 120 | # Blanket approval or denial for OSI-approved or FSF Free/Libre licenses 121 | # * both - The license will be approved if it is both OSI-approved *AND* FSF 122 | # * either - The license will be approved if it is either OSI-approved *OR* FSF 123 | # * osi-only - The license will be approved if is OSI-approved *AND NOT* FSF 124 | # * fsf-only - The license will be approved if is FSF *AND NOT* OSI-approved 125 | # * neither - This predicate is ignored and the default lint level is used 126 | allow-osi-fsf-free = "neither" 127 | # Lint level used when no other predicates are matched 128 | # 1. License isn't in the allow or deny lists 129 | # 2. License isn't copyleft 130 | # 3. License isn't OSI/FSF, or allow-osi-fsf-free = "neither" 131 | default = "deny" 132 | # The confidence threshold for detecting a license from license text. 133 | # The higher the value, the more closely the license text must be to the 134 | # canonical license text of a valid SPDX license file. 135 | # [possible values: any between 0.0 and 1.0]. 136 | confidence-threshold = 0.8 137 | # Allow 1 or more licenses on a per-crate basis, so that particular licenses 138 | # aren't accepted for every possible crate as with the normal allow list 139 | exceptions = [ 140 | # Each entry is the crate and version constraint, and its specific allow 141 | # list 142 | #{ allow = ["Zlib"], name = "adler32", version = "*" }, 143 | ] 144 | 145 | # Some crates don't have (easily) machine readable licensing information, 146 | # adding a clarification entry for it allows you to manually specify the 147 | # licensing information 148 | #[[licenses.clarify]] 149 | # The name of the crate the clarification applies to 150 | #name = "ring" 151 | # The optional version constraint for the crate 152 | #version = "*" 153 | # The SPDX expression for the license requirements of the crate 154 | #expression = "MIT AND ISC AND OpenSSL" 155 | # One or more files in the crate's source used as the "source of truth" for 156 | # the license expression. If the contents match, the clarification will be used 157 | # when running the license check, otherwise the clarification will be ignored 158 | # and the crate will be checked normally, which may produce warnings or errors 159 | # depending on the rest of your configuration 160 | #license-files = [ 161 | # Each entry is a crate relative path, and the (opaque) hash of its contents 162 | #{ path = "LICENSE", hash = 0xbd0eed23 } 163 | #] 164 | 165 | [licenses.private] 166 | # If true, ignores workspace crates that aren't published, or are only 167 | # published to private registries. 168 | # To see how to mark a crate as unpublished (to the official registry), 169 | # visit https://doc.rust-lang.org/cargo/reference/manifest.html#the-publish-field. 170 | ignore = false 171 | # One or more private registries that you might publish crates to, if a crate 172 | # is only published to private registries, and ignore is true, the crate will 173 | # not have its license(s) checked 174 | registries = [ 175 | #"https://sekretz.com/registry 176 | ] 177 | 178 | # This section is considered when running `cargo deny check bans`. 179 | # More documentation about the 'bans' section can be found here: 180 | # https://embarkstudios.github.io/cargo-deny/checks/bans/cfg.html 181 | [bans] 182 | # Lint level for when multiple versions of the same crate are detected 183 | multiple-versions = "warn" 184 | # Lint level for when a crate version requirement is `*` 185 | wildcards = "allow" 186 | # The graph highlighting used when creating dotgraphs for crates 187 | # with multiple versions 188 | # * lowest-version - The path to the lowest versioned duplicate is highlighted 189 | # * simplest-path - The path to the version with the fewest edges is highlighted 190 | # * all - Both lowest-version and simplest-path are used 191 | highlight = "all" 192 | # The default lint level for `default` features for crates that are members of 193 | # the workspace that is being checked. This can be overriden by allowing/denying 194 | # `default` on a crate-by-crate basis if desired. 195 | workspace-default-features = "allow" 196 | # The default lint level for `default` features for external crates that are not 197 | # members of the workspace. This can be overriden by allowing/denying `default` 198 | # on a crate-by-crate basis if desired. 199 | external-default-features = "allow" 200 | # List of crates that are allowed. Use with care! 201 | allow = [ 202 | #{ name = "ansi_term", version = "=0.11.0" }, 203 | ] 204 | # List of crates to deny 205 | deny = [ 206 | # Each entry the name of a crate and a version range. If version is 207 | # not specified, all versions will be matched. 208 | #{ name = "ansi_term", version = "=0.11.0" }, 209 | # 210 | # Wrapper crates can optionally be specified to allow the crate when it 211 | # is a direct dependency of the otherwise banned crate 212 | #{ name = "ansi_term", version = "=0.11.0", wrappers = [] }, 213 | ] 214 | 215 | # List of features to allow/deny 216 | # Each entry the name of a crate and a version range. If version is 217 | # not specified, all versions will be matched. 218 | #[[bans.features]] 219 | #name = "reqwest" 220 | # Features to not allow 221 | #deny = ["json"] 222 | # Features to allow 223 | #allow = [ 224 | # "rustls", 225 | # "__rustls", 226 | # "__tls", 227 | # "hyper-rustls", 228 | # "rustls", 229 | # "rustls-pemfile", 230 | # "rustls-tls-webpki-roots", 231 | # "tokio-rustls", 232 | # "webpki-roots", 233 | #] 234 | # If true, the allowed features must exactly match the enabled feature set. If 235 | # this is set there is no point setting `deny` 236 | #exact = true 237 | 238 | # Certain crates/versions that will be skipped when doing duplicate detection. 239 | skip = [ 240 | #{ name = "ansi_term", version = "=0.11.0" }, 241 | ] 242 | # Similarly to `skip` allows you to skip certain crates during duplicate 243 | # detection. Unlike skip, it also includes the entire tree of transitive 244 | # dependencies starting at the specified crate, up to a certain depth, which is 245 | # by default infinite. 246 | skip-tree = [ 247 | #{ name = "ansi_term", version = "=0.11.0", depth = 20 }, 248 | ] 249 | 250 | # This section is considered when running `cargo deny check sources`. 251 | # More documentation about the 'sources' section can be found here: 252 | # https://embarkstudios.github.io/cargo-deny/checks/sources/cfg.html 253 | [sources] 254 | # Lint level for what to happen when a crate from a crate registry that is not 255 | # in the allow list is encountered 256 | unknown-registry = "warn" 257 | # Lint level for what to happen when a crate from a git repository that is not 258 | # in the allow list is encountered 259 | unknown-git = "warn" 260 | # List of URLs for allowed crate registries. Defaults to the crates.io index 261 | # if not specified. If it is specified but empty, no registries are allowed. 262 | allow-registry = ["https://github.com/rust-lang/crates.io-index"] 263 | # List of URLs for allowed Git repositories 264 | allow-git = [] 265 | 266 | [sources.allow-org] 267 | # 1 or more github.com organizations to allow git sources for 268 | github = [""] 269 | # 1 or more gitlab.com organizations to allow git sources for 270 | gitlab = [""] 271 | # 1 or more bitbucket.org organizations to allow git sources for 272 | bitbucket = [""] 273 | -------------------------------------------------------------------------------- /examples/control/inverted_pendulum.rs: -------------------------------------------------------------------------------- 1 | use dialoguer::{theme::ColorfulTheme, Select}; 2 | use nalgebra::{Const, Matrix1, Matrix4, Matrix4x1, Vector4}; 3 | use plotpy::{Curve, Plot}; 4 | use std::error::Error; 5 | 6 | extern crate robotics; 7 | use robotics::control::lqr::{lqr, LinearTimeInvariantModel}; 8 | 9 | /// [x, x_dot, theta, thetha_dot] 10 | struct InvertedPendulumModel { 11 | da: Matrix4, 12 | db: Matrix4x1, 13 | r: Matrix1, 14 | q: Matrix4, 15 | } 16 | impl InvertedPendulumModel { 17 | fn new(l_bar: f64, mass_cart: f64, mass_ball: f64, g: f64) -> InvertedPendulumModel { 18 | let q = Matrix4::from_diagonal(&Vector4::new(10.00, 1.0, 10.0, 1.0)); 19 | let r = Matrix1::new(0.01); 20 | 21 | #[rustfmt::skip] 22 | let da = Matrix4::new( 23 | 0.0, 1.0, 0.0, 0.0, 24 | 0.0, 0.0, mass_ball * g / mass_cart, 0.0, 25 | 0.0, 0.0, 0.0, 1.0, 26 | 0.0, 0.0, g * (mass_cart + mass_ball) / (l_bar * mass_cart), 0.0 27 | ); 28 | 29 | let db = Matrix4x1::new(0.0, 1.0 / mass_cart, 0.0, 1.0 / (l_bar * mass_cart)); 30 | 31 | InvertedPendulumModel { da, db, r, q } 32 | } 33 | 34 | fn linearize(&self, dt: f64) -> LinearTimeInvariantModel, Const<1>> { 35 | LinearTimeInvariantModel { 36 | A: Matrix4::identity() + dt * self.da, 37 | B: dt * self.db, 38 | Q: self.q.clone(), 39 | R: self.r.clone(), 40 | } 41 | } 42 | } 43 | 44 | fn run() -> Option<(Vec>, Vec)> { 45 | let sim_time = 5.0; 46 | let dt = 0.01; 47 | let mut time = 0.; 48 | let max_iter = 500; 49 | let epsilon = 0.01; 50 | 51 | let l_bar = 2.0; // length of bar 52 | let mass_cart = 1.0; // [kg] 53 | let mass_ball = 0.3; // [kg] 54 | let g = 9.8; // [m/s^2] 55 | 56 | let model = InvertedPendulumModel::new(l_bar, mass_cart, mass_ball, g); 57 | let linear_model = model.linearize(dt); 58 | let k_lqr = lqr(&linear_model, max_iter, epsilon)?; 59 | 60 | let mut x = Vector4::new(0.0, 0.0, -0.2, 0.0); 61 | let x_goal = Vector4::new(0.0, 0.0, 0.0, 0.0); 62 | 63 | let mut states = vec![x]; 64 | let mut commands = vec![0.0]; 65 | 66 | while time < sim_time { 67 | time += dt; 68 | let u = -k_lqr * (&x - &x_goal); 69 | x = &linear_model.A * x + &linear_model.B * u; 70 | 71 | states.push(x.clone()); 72 | commands.push(u.x); 73 | } 74 | Some((states, commands)) 75 | } 76 | 77 | fn main() -> Result<(), Box> { 78 | let algos = &[ 79 | "Linear Quadratic Regulator (LQR)", 80 | "Model Predictive Control (MPC)", 81 | ]; 82 | let algo_idx = Select::with_theme(&ColorfulTheme::default()) 83 | .with_prompt("Pick control algorithm") 84 | .default(0) 85 | .items(&algos[..]) 86 | .interact() 87 | .unwrap(); 88 | // let algo = algos[algo_idx]; 89 | 90 | // get data 91 | let (states, commands) = run().expect("unable to run"); 92 | 93 | // Create output directory if it didnt exist 94 | std::fs::create_dir_all("./img")?; 95 | 96 | let path = match algo_idx { 97 | 0 => "lqr", 98 | 1 => "mpc", 99 | _ => unreachable!(), 100 | }; 101 | 102 | let time = (0..states.len()) 103 | .map(|i| (i as f64) / (states.len() as f64) * 5.0) 104 | .collect::>(); 105 | 106 | let mut curve_x = Curve::new(); 107 | curve_x 108 | .set_label("x") 109 | .draw(&time, &states.iter().map(|s| s.x).collect()); 110 | 111 | let mut curve_theta = Curve::new(); 112 | curve_theta 113 | .set_label("theta") 114 | .draw(&time, &states.iter().map(|s| s.z).collect()); 115 | 116 | let mut curve_x_dot = Curve::new(); 117 | curve_x_dot 118 | .set_label("x dot") 119 | .draw(&time, &states.iter().map(|s| s.y).collect()); 120 | 121 | let mut curve_theta_dot = Curve::new(); 122 | curve_theta_dot 123 | .set_label("theta dot") 124 | .draw(&time, &states.iter().map(|s| s.w).collect()); 125 | 126 | let mut curve_u = Curve::new(); 127 | curve_u.set_label("u").draw(&time, &commands); 128 | 129 | // add features to plot 130 | let mut plot = Plot::new(); 131 | 132 | plot.add(&curve_x) 133 | .add(&curve_theta) 134 | .add(&curve_x_dot) 135 | .add(&curve_theta_dot) 136 | // .add(&curve_u) 137 | .legend() 138 | .set_equal_axes(true) // save figure 139 | .set_figure_size_points(1000.0, 1000.0) 140 | .save(format!("img/{}-{}.svg", path, "done").as_str())?; 141 | Ok(()) 142 | } 143 | -------------------------------------------------------------------------------- /examples/localization/bayesian_filter.rs: -------------------------------------------------------------------------------- 1 | use dialoguer::{theme::ColorfulTheme, Select}; 2 | use nalgebra::{Const, Matrix2, Matrix4, Vector2, Vector4}; 3 | use plotters::prelude::*; 4 | use rand_distr::{Distribution, Normal}; 5 | use std::error::Error; 6 | 7 | extern crate robotics; 8 | use robotics::localization::{ 9 | BayesianFilter, ExtendedKalmanFilter, ParticleFilter, ResamplingScheme, UnscentedKalmanFilter, 10 | }; 11 | use robotics::models::measurement::{MeasurementModel, SimpleProblemMeasurementModel}; 12 | use robotics::models::motion::{MotionModel, SimpleProblemMotionModel}; 13 | use robotics::utils::deg2rad; 14 | use robotics::utils::plot::{chart, History}; 15 | use robotics::utils::state::GaussianState; 16 | 17 | /// State 18 | /// [x, y, yaw, v] 19 | /// 20 | /// Observation 21 | /// [x,y] 22 | struct SimpleProblem { 23 | pub gps_noise: Matrix2, 24 | pub input_noise: Matrix2, 25 | pub motion_model: SimpleProblemMotionModel, 26 | pub observation_model: SimpleProblemMeasurementModel, 27 | } 28 | 29 | impl SimpleProblem { 30 | fn observation( 31 | &self, 32 | x_true: &Vector4, 33 | x_deterministic: &Vector4, 34 | u: &Vector2, 35 | dt: f64, 36 | ) -> (Vector4, Vector2, Vector4, Vector2) { 37 | let mut rng = rand::thread_rng(); 38 | let normal = Normal::new(0., 1.).unwrap(); 39 | 40 | let x_true_next = self.motion_model.prediction(x_true, u, dt); 41 | 42 | // add noise to gps x-y 43 | let observation_noise = 44 | self.gps_noise * Vector2::new(normal.sample(&mut rng), normal.sample(&mut rng)); 45 | let observation = self.observation_model.prediction(&x_true_next, None) + observation_noise; 46 | 47 | // add noise to input 48 | let u_noise = 49 | self.input_noise * Vector2::new(normal.sample(&mut rng), normal.sample(&mut rng)); 50 | let ud = u + u_noise; 51 | 52 | let x_deterministic_next = self.motion_model.prediction(x_deterministic, &ud, dt); 53 | 54 | (x_true_next, observation, x_deterministic_next, ud) 55 | } 56 | } 57 | 58 | fn run(algo: &str) -> History { 59 | let sim_time = 50.0; 60 | let dt = 0.1; 61 | let mut time = 0.; 62 | 63 | // state : [x, y, yaw, v] 64 | let mut q = Matrix4::::from_diagonal(&Vector4::new(0.1, 0.1, deg2rad(1.0), 1.0)); 65 | q = q * q; // predict state covariance 66 | 67 | let r = nalgebra::Matrix2::identity(); //Observation x,y position covariance 68 | 69 | let initial_state = GaussianState { 70 | x: Vector4::::new(0., 0., 0., 0.), 71 | cov: Matrix4::::identity(), 72 | }; 73 | let mut bayesian_filter: Box, Const<2>, Const<2>>> = match algo 74 | { 75 | "Extended Kalman Filter (EKF)" => Box::new(ExtendedKalmanFilter::new( 76 | q, 77 | r, 78 | SimpleProblemMeasurementModel::new(), 79 | SimpleProblemMotionModel::new(), 80 | initial_state, 81 | )), 82 | "Unscented Kalman Filter (UKF)" => Box::new(UnscentedKalmanFilter::new( 83 | q, 84 | r, 85 | SimpleProblemMeasurementModel::new(), 86 | SimpleProblemMotionModel::new(), 87 | 0.1, 88 | 2.0, 89 | 0.0, 90 | initial_state, 91 | )), 92 | "Particle Filter (PF)" => Box::new(ParticleFilter::new( 93 | q, 94 | r, 95 | SimpleProblemMeasurementModel::new(), 96 | SimpleProblemMotionModel::new(), 97 | initial_state, 98 | 300, 99 | ResamplingScheme::Stratified, 100 | )), 101 | _ => unimplemented!("{}", algo), 102 | }; 103 | 104 | let simple_problem = SimpleProblem { 105 | input_noise: Matrix2::new(1., 0., 0., deg2rad(30.0).powi(2)), 106 | gps_noise: Matrix2::new(0.25, 0., 0., 0.25), 107 | observation_model: SimpleProblemMeasurementModel {}, 108 | motion_model: SimpleProblemMotionModel {}, 109 | }; 110 | 111 | let u = Vector2::::new(1.0, 0.1); 112 | let mut ud: Vector2; 113 | let mut x_dr = Vector4::::new(0., 0., 0., 0.); 114 | let mut x_true = Vector4::::new(0., 0., 0., 0.); 115 | let mut z: Vector2; 116 | 117 | let mut history = History::default(); 118 | 119 | while time < sim_time { 120 | time += dt; 121 | (x_true, z, x_dr, ud) = simple_problem.observation(&x_true, &x_dr, &u, dt); 122 | bayesian_filter.update_estimate(&ud, &z, dt); 123 | let gaussian_state = bayesian_filter.gaussian_estimate(); 124 | 125 | // record step 126 | history.z.push((z[0] as f64, z[1] as f64)); 127 | history.x_true.push((x_true[0] as f64, x_true[1] as f64)); 128 | history.x_dr.push((x_dr[0] as f64, x_dr[1] as f64)); 129 | history 130 | .x_est 131 | .push((gaussian_state.x[0] as f64, gaussian_state.x[1] as f64)); 132 | history.gaussian_state.push(gaussian_state.clone()); 133 | } 134 | history 135 | } 136 | 137 | fn main() -> Result<(), Box> { 138 | let algos = &[ 139 | "Extended Kalman Filter (EKF)", 140 | "Unscented Kalman Filter (UKF)", 141 | "Particle Filter (PF)", 142 | ]; 143 | let algo_idx = Select::with_theme(&ColorfulTheme::default()) 144 | .with_prompt("Pick localization algorithm") 145 | .default(0) 146 | .items(&algos[..]) 147 | .interact() 148 | .unwrap(); 149 | let algo = algos[algo_idx]; 150 | 151 | // get data 152 | let history = run(algo); 153 | let len = history.z.len(); 154 | 155 | // Create output directory if it didnt exist 156 | std::fs::create_dir_all("./img")?; 157 | 158 | let path = match algo_idx { 159 | 0 => "ekf", 160 | 1 => "ukf", 161 | 2 => "pf", 162 | _ => unreachable!(), 163 | }; 164 | 165 | // Plot image 166 | let path_img = format!("./img/{path}.png"); 167 | let root = BitMapBackend::new(path_img.as_str(), (1024, 768)).into_drawing_area(); 168 | root.fill(&WHITE)?; 169 | chart(&root, &history, len - 1, algo)?; 170 | root.present()?; 171 | println!("Result has been saved to {path_img}"); 172 | 173 | // Plot GIF 174 | println!("Building a gif..."); 175 | let path_gif = format!("./img/{path}.gif"); 176 | let root = BitMapBackend::gif(path_gif.as_str(), (1024, 768), 1)?.into_drawing_area(); 177 | for i in (0..len).step_by(5) { 178 | root.fill(&WHITE)?; 179 | chart(&root, &history, i, algo)?; 180 | root.present()?; 181 | } 182 | println!("Result has been saved to {path_gif}"); 183 | 184 | Ok(()) 185 | } 186 | -------------------------------------------------------------------------------- /examples/localization/localization_landmarks.rs: -------------------------------------------------------------------------------- 1 | use dialoguer::{theme::ColorfulTheme, Select}; 2 | use nalgebra::{Const, Matrix2, Matrix3, Vector2, Vector3}; 3 | use rustc_hash::FxHashMap; 4 | use std::error::Error; 5 | 6 | extern crate robotics; 7 | use robotics::data::utias::UtiasDataset; 8 | use robotics::localization::BayesianFilterKnownCorrespondences; 9 | use robotics::localization::{ 10 | ExtendedKalmanFilterKnownCorrespondences, ParticleFilterKnownCorrespondences, 11 | }; 12 | use robotics::models::measurement::RangeBearingMeasurementModel; 13 | use robotics::models::motion::Velocity; 14 | use robotics::utils::plot::plot_landmarks; 15 | use robotics::utils::state::GaussianState; 16 | 17 | fn main() -> Result<(), Box> { 18 | let algos = &[ 19 | "Extended Kalman Filter (EKF)", 20 | "Unscented Kalman Filter (UKF)", 21 | "Particle Filter (PF)", 22 | ]; 23 | let algo_idx = Select::with_theme(&ColorfulTheme::default()) 24 | .with_prompt("Pick localization algorithm") 25 | .default(0) 26 | .items(&algos[..]) 27 | .interact() 28 | .unwrap(); 29 | // let algo = algos[algo_idx]; 30 | 31 | let dataset = UtiasDataset::new(0)?; 32 | let mut landmarks = FxHashMap::default(); 33 | for (id, lm) in &dataset.landmarks { 34 | landmarks.insert(*id, Vector3::new(lm.x, lm.y, 0.0)); 35 | } 36 | let measurement_model = RangeBearingMeasurementModel::new(); 37 | let noise = 1.0; 38 | let noise_w = 30.0; 39 | let noise_g = 10.0; 40 | let motion_model = Velocity::new([noise, noise, noise_w, noise_w, noise_g, noise_g]); 41 | let q = Matrix2::::from_diagonal(&Vector2::new(0.1, 0.2)); 42 | //Observation x,y position covariance 43 | let r = Matrix3::::from_diagonal(&Vector3::new(0.2, 0.2, 0.2)); 44 | 45 | let skip = 0; 46 | let gt_state = &dataset.groundtruth[skip]; 47 | let state = GaussianState { 48 | x: Vector3::new(gt_state.x, gt_state.y, gt_state.orientation), 49 | cov: Matrix3::::from_diagonal(&Vector3::new(1e-10, 1e-10, 1e-10)), 50 | }; 51 | 52 | let mut bayes_filter: Box< 53 | dyn BayesianFilterKnownCorrespondences, Const<2>, Const<2>>, 54 | > = match algo_idx { 55 | 0 => Box::new(ExtendedKalmanFilterKnownCorrespondences::new( 56 | q, 57 | landmarks, 58 | measurement_model, 59 | motion_model, 60 | state, 61 | )), 62 | 1 => todo!(), 63 | 2 => Box::new(ParticleFilterKnownCorrespondences::new( 64 | r, 65 | q, 66 | landmarks, 67 | measurement_model, 68 | motion_model, 69 | state, 70 | 300, 71 | )), 72 | _ => unreachable!(), 73 | }; 74 | 75 | let mut time_past = gt_state.time; 76 | 77 | let mut states = Vec::new(); 78 | let mut states_measurement = Vec::new(); 79 | 80 | for (measurements, odometry) in (&dataset).into_iter().skip(skip).take(10000) { 81 | let (time_now, measurement_update) = if let Some(m) = &measurements { 82 | (m.first().unwrap().time, true) 83 | } else if let Some(od) = &odometry { 84 | (od.time, false) 85 | } else { 86 | panic!("NOOOOOOOOO") 87 | }; 88 | let dt = time_now - time_past; 89 | time_past = time_now; 90 | 91 | let measurements = measurements.map(|ms| { 92 | ms.iter() 93 | .map(|m| (m.subject_nb, Vector2::new(m.range, m.bearing))) 94 | .collect::)>>() 95 | }); 96 | 97 | let odometry = odometry.map(|od| Vector2::new(od.forward_velocity, od.angular_velocity)); 98 | 99 | bayes_filter.update_estimate(odometry, measurements, dt); 100 | 101 | states.push(bayes_filter.gaussian_estimate()); 102 | if measurement_update { 103 | states_measurement.push(states.last().unwrap().clone()) 104 | } 105 | } 106 | println!("measurement updates = {}", states_measurement.len()); 107 | 108 | let (path, name) = match algo_idx { 109 | 0 => ("ekf_landmarks", "EKF landmarks"), 110 | 1 => todo!(), 111 | 2 => ("pf_landmarks", "Particle Filter (Monte Carlo) landmarks"), 112 | _ => unreachable!(), 113 | }; 114 | plot_landmarks( 115 | &dataset, 116 | &states, 117 | &states_measurement, 118 | time_past, 119 | format!("./img/{path}.png").as_str(), 120 | name, 121 | )?; 122 | 123 | Ok(()) 124 | } 125 | -------------------------------------------------------------------------------- /examples/mapping/pose_graph_optimization.rs: -------------------------------------------------------------------------------- 1 | use dialoguer::{theme::ColorfulTheme, Select}; 2 | use std::error::Error; 3 | 4 | use robotics::mapping::{PoseGraph, PoseGraphSolver}; 5 | 6 | fn main() -> Result<(), Box> { 7 | // Create output directory if it didnt exist 8 | std::fs::create_dir_all("./img")?; 9 | 10 | let filenames = &[ 11 | "simulation-pose-pose.g2o", 12 | "simulation-pose-landmark.g2o", 13 | "dlr.g2o", 14 | "intel.g2o", 15 | "input_M3500_g2o.g2o", 16 | "sphere2500.g2o", 17 | "torus3D.g2o", 18 | "parking-garage.g2o", 19 | ]; 20 | let filename_idx = Select::with_theme(&ColorfulTheme::default()) 21 | .with_prompt("Pick g2o file") 22 | .default(0) 23 | .items(&filenames[..]) 24 | .interact() 25 | .unwrap(); 26 | let filename = format!("dataset/g2o/{}", filenames[filename_idx]); 27 | 28 | let solvers = &["GaussNewton", "LevenbergMarquardt"]; 29 | let solver_idx = Select::with_theme(&ColorfulTheme::default()) 30 | .with_prompt("Pick Solver") 31 | .default(0) 32 | .items(&solvers[..]) 33 | .interact() 34 | .unwrap(); 35 | let solver = match solver_idx { 36 | 0 => PoseGraphSolver::GaussNewton, 37 | 1 => PoseGraphSolver::LevenbergMarquardt, 38 | _ => unreachable!(), 39 | }; 40 | 41 | let plot = Select::with_theme(&ColorfulTheme::default()) 42 | .with_prompt("Plot the resut?") 43 | .default(0) 44 | .items(&[true, false]) 45 | .interact() 46 | .unwrap(); 47 | let plot = plot == 0; 48 | 49 | let mut graph = PoseGraph::new(filename.as_str(), solver)?; 50 | graph.optimize(50, true, plot)?; 51 | Ok(()) 52 | } 53 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["maturin>=0.14,<0.15"] 3 | build-backend = "maturin" 4 | 5 | [project] 6 | name = "robotics" 7 | requires-python = ">=3.7" 8 | classifiers = [ 9 | "Programming Language :: Rust", 10 | "Programming Language :: Python :: Implementation :: CPython", 11 | "Programming Language :: Python :: Implementation :: PyPy", 12 | ] 13 | 14 | 15 | -------------------------------------------------------------------------------- /rust-toolchain.toml: -------------------------------------------------------------------------------- 1 | [toolchain] 2 | # channel = "nightly" 3 | channel = "stable" -------------------------------------------------------------------------------- /src/control/lqr.rs: -------------------------------------------------------------------------------- 1 | #![allow(non_snake_case)] 2 | #![allow(non_camel_case_types)] 3 | 4 | use nalgebra::{allocator::Allocator, DefaultAllocator, Dim, OMatrix, RealField}; 5 | 6 | pub struct LinearTimeInvariantModel 7 | where 8 | DefaultAllocator: 9 | Allocator + Allocator + Allocator + Allocator, 10 | { 11 | pub A: OMatrix, 12 | pub B: OMatrix, 13 | pub R: OMatrix, 14 | pub Q: OMatrix, 15 | } 16 | 17 | pub fn lqr( 18 | linear_model: &LinearTimeInvariantModel, 19 | max_iter: usize, 20 | epsilon: T, 21 | ) -> Option> 22 | where 23 | DefaultAllocator: Allocator 24 | + Allocator 25 | + Allocator 26 | + Allocator 27 | + Allocator 28 | + Allocator, 29 | { 30 | let A = &linear_model.A; 31 | let A_T = &A.transpose(); 32 | let B = &linear_model.B; 33 | let B_T = &B.transpose(); 34 | let Q = &linear_model.Q; 35 | let R = &linear_model.R; 36 | 37 | // Discrete time Algebraic Riccati Equation (DARE) 38 | let mut P = linear_model.Q.clone_owned(); 39 | for _ in 0..max_iter { 40 | let Pn = A_T * &P * A - A_T * &P * B * (R + B_T * &P * B).try_inverse()? * B_T * &P * A + Q; 41 | if (&Pn - &P).abs().max() < epsilon { 42 | // println!("done {i}"); 43 | break; 44 | } 45 | P = Pn; 46 | } 47 | // LQR gain 48 | let k = (R + B_T * &P * B).try_inverse()? * B_T * &P * A; 49 | // 50 | // // LQR control 51 | Some(k) 52 | } 53 | -------------------------------------------------------------------------------- /src/control/mod.rs: -------------------------------------------------------------------------------- 1 | pub mod lqr; 2 | -------------------------------------------------------------------------------- /src/data/mod.rs: -------------------------------------------------------------------------------- 1 | pub mod slam_course; 2 | pub mod utias; 3 | -------------------------------------------------------------------------------- /src/data/slam_course.rs: -------------------------------------------------------------------------------- 1 | #![allow(dead_code)] 2 | use plotters::prelude::*; 3 | use rustc_hash::FxHashMap; 4 | use serde::{Deserialize, Serialize}; 5 | use std::error::Error; 6 | use std::fs::read_to_string; 7 | 8 | #[derive(Debug, Serialize, Deserialize, Clone)] 9 | pub struct Odometry { 10 | pub rotation1: f64, 11 | pub translation: f64, 12 | pub rotation2: f64, 13 | } 14 | 15 | #[derive(Debug, Serialize, Deserialize, Clone)] 16 | pub struct RangeBearing { 17 | pub id: u32, 18 | pub range: f64, 19 | pub bearing: f64, 20 | } 21 | 22 | #[derive(Debug, Serialize, Deserialize)] 23 | pub struct Landmark { 24 | pub id: u32, 25 | pub x: f64, 26 | pub y: f64, 27 | } 28 | pub struct SlamCourseDataset { 29 | odometry: Vec, 30 | sensors: Vec>, 31 | landmarks: FxHashMap, 32 | } 33 | 34 | // TODO: use nom library instead 35 | impl SlamCourseDataset { 36 | fn new() -> Result> { 37 | let base = "dataset/slam_course"; 38 | 39 | let mut odometry = Vec::new(); 40 | let mut sensors = Vec::new(); 41 | let mut sensors_same_timestep = Vec::new(); 42 | for (i, line) in read_to_string(format!("{base}/sensor_data.dat"))? 43 | .lines() 44 | .enumerate() 45 | { 46 | let line: Vec<&str> = line.split(' ').collect(); 47 | match line[0] { 48 | "ODOMETRY" => { 49 | if i != 0 { 50 | sensors.push(sensors_same_timestep.clone()); 51 | } 52 | sensors_same_timestep.clear(); 53 | let odom = Odometry { 54 | rotation1: line[1].parse::()?, 55 | translation: line[2].parse::()?, 56 | rotation2: line[3].parse::()?, 57 | }; 58 | odometry.push(odom); 59 | } 60 | "SENSOR" => { 61 | let sensor = RangeBearing { 62 | id: line[1].parse::()?, 63 | range: line[2].parse::()?, 64 | bearing: line[3].parse::()?, 65 | }; 66 | sensors_same_timestep.push(sensor) 67 | } 68 | _ => panic!("no"), 69 | } 70 | } 71 | sensors.push(sensors_same_timestep.clone()); 72 | 73 | println!("{}, {}", odometry.len(), sensors.len()); 74 | 75 | let mut landmarks = FxHashMap::default(); 76 | for line in std::fs::read_to_string(format!("{base}/world.dat"))?.lines() { 77 | let line: Vec<&str> = line.split(' ').collect(); 78 | let id = line[0].parse::()?; 79 | landmarks.insert( 80 | id, 81 | Landmark { 82 | id, 83 | x: line[1].parse::()?, 84 | y: line[2].parse::()?, 85 | }, 86 | ); 87 | } 88 | 89 | println!("{:?}", landmarks); 90 | 91 | Ok(SlamCourseDataset { 92 | odometry, 93 | sensors, 94 | landmarks, 95 | }) 96 | } 97 | } 98 | 99 | // Non-Consuming iterator 100 | pub struct SlamCourseDatasetIteratorRef<'a> { 101 | dataset: &'a SlamCourseDataset, 102 | index: usize, 103 | } 104 | 105 | impl<'a> IntoIterator for &'a SlamCourseDataset { 106 | type IntoIter = SlamCourseDatasetIteratorRef<'a>; 107 | type Item = (Option<&'a Vec>, Option<&'a Odometry>); 108 | 109 | fn into_iter(self) -> Self::IntoIter { 110 | SlamCourseDatasetIteratorRef { 111 | dataset: self, 112 | index: 0, 113 | } 114 | } 115 | } 116 | 117 | impl<'a> Iterator for SlamCourseDatasetIteratorRef<'a> { 118 | type Item = (Option<&'a Vec>, Option<&'a Odometry>); 119 | fn next(&mut self) -> Option { 120 | let odometry = &self.dataset.odometry[self.index]; 121 | let measurements = &self.dataset.sensors[self.index]; 122 | self.index += 1; 123 | Some((Some(measurements), Some(odometry))) 124 | } 125 | } 126 | 127 | /// Visualizes the robot in the map. 128 | /// 129 | /// The resulting plot displays the following information: 130 | /// - the landmarks in the map (black +'s) 131 | /// - current robot pose (red) 132 | /// - observations made at this time step (line between robot and landmark) 133 | fn plot(dataset: &SlamCourseDataset) -> Result<(), Box> { 134 | std::fs::create_dir_all("./img")?; 135 | let filename = "./img/slam_course.png"; 136 | let name = "SLAM course"; 137 | let root = BitMapBackend::new(filename, (1024, 768)).into_drawing_area(); 138 | root.fill(&WHITE)?; 139 | let mut chart = ChartBuilder::on(&root) 140 | .margin(10) 141 | .caption(name, ("sans-serif", 40)) 142 | .build_cartesian_2d(-2.0..12.0, -2.0..12.0)?; 143 | 144 | chart.configure_mesh().draw()?; 145 | 146 | // Landmarks 147 | chart 148 | .draw_series( 149 | dataset 150 | .landmarks 151 | .values() 152 | .map(|lm| Circle::new((lm.x, lm.y), 5, RED.filled())), 153 | )? 154 | .label("Landmarks") 155 | .legend(|(x, y)| Circle::new((x, y), 5, RED.filled())); 156 | 157 | root.present().expect( 158 | "Unable to write result to file, please make sure 'img' dir exists under current dir", 159 | ); 160 | Ok(()) 161 | } 162 | 163 | #[cfg(test)] 164 | mod tests { 165 | use super::*; 166 | 167 | #[test] 168 | fn read_slam_course_dataset() -> Result<(), Box> { 169 | let dataset = SlamCourseDataset::new()?; 170 | plot(&dataset)?; 171 | Ok(()) 172 | } 173 | } 174 | -------------------------------------------------------------------------------- /src/data/utias.rs: -------------------------------------------------------------------------------- 1 | use csv; 2 | use rustc_hash::FxHashMap; 3 | use serde::{Deserialize, Serialize}; 4 | use std::error::Error; 5 | 6 | #[derive(Debug, Serialize, Deserialize, Clone)] 7 | pub struct RangeBearing { 8 | pub time: f64, 9 | pub subject_nb: u32, 10 | pub range: f64, 11 | pub bearing: f64, 12 | } 13 | 14 | #[derive(Debug, Serialize, Deserialize)] 15 | pub struct Position { 16 | pub time: f64, 17 | pub x: f64, 18 | pub y: f64, 19 | pub orientation: f64, 20 | } 21 | 22 | #[derive(Debug, Serialize, Deserialize, Clone)] 23 | pub struct Odometry { 24 | pub time: f64, 25 | pub forward_velocity: f64, 26 | pub angular_velocity: f64, 27 | } 28 | 29 | #[derive(Debug, Serialize, Deserialize)] 30 | pub struct Landmark { 31 | pub subject_nb: u32, 32 | pub x: f64, 33 | pub y: f64, 34 | pub x_std_dev: f64, 35 | pub y_std_dev: f64, 36 | } 37 | 38 | #[derive(Debug, Serialize, Deserialize)] 39 | pub struct Barcode { 40 | subject_nb: u32, 41 | barcode_nb: u32, 42 | } 43 | 44 | pub struct UtiasDataset { 45 | pub groundtruth: Vec, 46 | pub landmarks: FxHashMap, 47 | pub measurements: Vec, 48 | pub odometry: Vec, 49 | } 50 | 51 | // iterator 52 | // Consuming iterator 53 | // TODO : remove the clonessubject_nb 54 | pub struct UtiasDatasetIterator { 55 | dataset: UtiasDataset, 56 | index_measurements: usize, 57 | index_odometry: usize, 58 | } 59 | 60 | impl IntoIterator for UtiasDataset { 61 | type IntoIter = UtiasDatasetIterator; 62 | type Item = (Option>, Option); 63 | 64 | fn into_iter(self) -> Self::IntoIter { 65 | UtiasDatasetIterator { 66 | dataset: self, 67 | index_measurements: 0, 68 | index_odometry: 0, 69 | } 70 | } 71 | } 72 | 73 | impl Iterator for UtiasDatasetIterator { 74 | type Item = (Option>, Option); 75 | 76 | fn next(&mut self) -> Option { 77 | let mut me = self 78 | .dataset 79 | .measurements 80 | .iter() 81 | .skip(self.index_measurements); 82 | let mut od = self.dataset.odometry.iter().skip(self.index_odometry); 83 | 84 | let me_next = me.next()?; 85 | let od_next = od.next()?; 86 | 87 | if od_next.time < me_next.time { 88 | self.index_odometry += 1; 89 | return Some((None, Some(od_next.clone()))); 90 | } 91 | let mut measurements = vec![me_next.clone()]; 92 | self.index_measurements += 1; 93 | loop { 94 | let me_next = me.next().unwrap(); 95 | if me_next.time == measurements.last()?.time { 96 | measurements.push(me_next.clone()); 97 | self.index_measurements += 1; 98 | } else { 99 | break; 100 | } 101 | } 102 | if od_next.time == me_next.time { 103 | return Some((Some(measurements), Some(od_next.clone()))); 104 | } 105 | Some((Some(measurements), None)) 106 | } 107 | } 108 | 109 | // Non-Consuming iterator 110 | pub struct UtiasDatasetIteratorRef<'a> { 111 | dataset: &'a UtiasDataset, 112 | index_measurements: usize, 113 | index_odometry: usize, 114 | } 115 | 116 | impl<'a> IntoIterator for &'a UtiasDataset { 117 | type IntoIter = UtiasDatasetIteratorRef<'a>; 118 | type Item = (Option>, Option<&'a Odometry>); 119 | 120 | fn into_iter(self) -> Self::IntoIter { 121 | UtiasDatasetIteratorRef { 122 | dataset: self, 123 | index_measurements: 0, 124 | index_odometry: 0, 125 | } 126 | } 127 | } 128 | 129 | impl<'a> Iterator for UtiasDatasetIteratorRef<'a> { 130 | type Item = (Option>, Option<&'a Odometry>); 131 | 132 | fn next(&mut self) -> Option { 133 | let mut me = self 134 | .dataset 135 | .measurements 136 | .iter() 137 | .skip(self.index_measurements); 138 | let mut od = self.dataset.odometry.iter().skip(self.index_odometry); 139 | 140 | let me_next = me.next()?; 141 | let od_next = od.next()?; 142 | 143 | if od_next.time < me_next.time { 144 | self.index_odometry += 1; 145 | return Some((None, Some(od_next))); 146 | } 147 | let mut measurements = vec![me_next]; 148 | self.index_measurements += 1; 149 | loop { 150 | let me_next = me.next()?; 151 | if me_next.time == measurements.last()?.time { 152 | measurements.push(me_next); 153 | self.index_measurements += 1; 154 | } else { 155 | break; 156 | } 157 | } 158 | if od_next.time == me_next.time { 159 | return Some((Some(measurements), Some(od_next))); 160 | } 161 | Some((Some(measurements), None)) 162 | } 163 | } 164 | 165 | impl UtiasDataset { 166 | pub fn new(dataset: i32) -> Result> { 167 | let base = match dataset { 168 | 0 => "dataset/utias0", 169 | 1 => "dataset/utias1", 170 | _ => panic!("dataset {dataset} not supported"), 171 | }; 172 | 173 | let barcodes: Vec = csv::Reader::from_path(format!("{base}/Barcodes.csv"))? 174 | .deserialize() 175 | .map(|x| x.unwrap()) 176 | .collect(); 177 | 178 | let landmarks_vec: Vec = 179 | csv::Reader::from_path(format!("{base}/Landmark_Groundtruth.csv"))? 180 | .deserialize() 181 | .map(|x| x.unwrap()) 182 | .collect(); 183 | 184 | let mut landmarks = FxHashMap::default(); 185 | for lm in landmarks_vec { 186 | let k = barcodes 187 | .iter() 188 | .find(|bc| bc.subject_nb == lm.subject_nb) 189 | .unwrap() 190 | .barcode_nb; 191 | landmarks.insert(k, lm); 192 | } 193 | 194 | let mut groundtruth: Vec = 195 | csv::Reader::from_path(format!("{base}/Groundtruth.csv"))? 196 | .deserialize() 197 | .map(|x| x.unwrap()) 198 | .collect(); 199 | groundtruth.sort_by(|a, b| a.time.partial_cmp(&b.time).unwrap()); 200 | let min_time = groundtruth.first().unwrap().time; 201 | 202 | let mut measurements: Vec = 203 | csv::Reader::from_path(format!("{base}/Measurement.csv"))? 204 | .deserialize() 205 | .map(|x| x.unwrap()) 206 | // .filter(|rb: &RangeBearing| (rb.range != 0.0) | (rb.bearing != 0.0)) 207 | .filter(|rb: &RangeBearing| rb.time >= min_time) 208 | .collect(); 209 | measurements.sort_by(|a, b| a.time.partial_cmp(&b.time).unwrap()); 210 | 211 | let mut odometry: Vec = csv::Reader::from_path(format!("{base}/Odometry.csv"))? 212 | .deserialize() 213 | .map(|x| x.unwrap()) 214 | .filter(|od: &Odometry| od.time >= min_time) 215 | .collect(); 216 | odometry.sort_by(|a, b| a.time.partial_cmp(&b.time).unwrap()); 217 | 218 | Ok(UtiasDataset { 219 | groundtruth, 220 | landmarks, 221 | measurements, 222 | odometry, 223 | }) 224 | } 225 | } 226 | 227 | #[cfg(test)] 228 | mod tests { 229 | use super::*; 230 | 231 | #[test] 232 | fn read_dataset0() -> Result<(), Box> { 233 | let dataset = UtiasDataset::new(0)?; 234 | for (i, data) in (&dataset).into_iter().take(5).enumerate() { 235 | println!("{i} => {:?}", data); 236 | } 237 | Ok(()) 238 | } 239 | 240 | // #[test] 241 | // fn read_dataset1() -> Result<(), Box> { 242 | // UtiasDataset::new(1)?; 243 | // Ok(()) 244 | // } 245 | 246 | // #[test] 247 | // #[should_panic] 248 | // fn read_dataset_bad() { 249 | // UtiasDataset::new(-1).unwrap(); 250 | // } 251 | } 252 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | pub mod control; 2 | pub mod data; 3 | pub mod localization; 4 | pub mod mapping; 5 | pub mod models; 6 | pub mod utils; 7 | -------------------------------------------------------------------------------- /src/localization/bayesian_filter.rs: -------------------------------------------------------------------------------- 1 | use nalgebra::{allocator::Allocator, DefaultAllocator, Dim, OVector, RealField}; 2 | 3 | use crate::utils::state::GaussianState; 4 | 5 | pub trait BayesianFilter 6 | where 7 | DefaultAllocator: Allocator + Allocator + Allocator + Allocator, 8 | { 9 | fn update_estimate(&mut self, u: &OVector, z: &OVector, dt: T); 10 | 11 | fn gaussian_estimate(&self) -> GaussianState; 12 | } 13 | 14 | pub trait BayesianFilterKnownCorrespondences 15 | where 16 | DefaultAllocator: Allocator + Allocator + Allocator + Allocator, 17 | { 18 | fn update_estimate( 19 | &mut self, 20 | control: Option>, 21 | measurements: Option)>>, 22 | dt: T, 23 | ); 24 | 25 | fn gaussian_estimate(&self) -> GaussianState; 26 | } 27 | -------------------------------------------------------------------------------- /src/localization/extended_kalman_filter.rs: -------------------------------------------------------------------------------- 1 | use nalgebra::{allocator::Allocator, DefaultAllocator, Dim, OMatrix, OVector, RealField}; 2 | use rustc_hash::FxHashMap; 3 | 4 | use crate::localization::{BayesianFilter, BayesianFilterKnownCorrespondences}; 5 | use crate::models::measurement::MeasurementModel; 6 | use crate::models::motion::MotionModel; 7 | use crate::utils::state::GaussianState; 8 | 9 | /// S : State Size, Z: Observation Size, U: Input Size 10 | pub struct ExtendedKalmanFilter 11 | where 12 | DefaultAllocator: Allocator + Allocator + Allocator, 13 | { 14 | r: OMatrix, 15 | q: OMatrix, 16 | measurement_model: Box + Send>, 17 | motion_model: Box + Send>, 18 | state: GaussianState, 19 | } 20 | 21 | impl ExtendedKalmanFilter 22 | where 23 | DefaultAllocator: Allocator + Allocator + Allocator, 24 | { 25 | pub fn new( 26 | r: OMatrix, 27 | q: OMatrix, 28 | measurement_model: Box + Send>, 29 | motion_model: Box + Send>, 30 | initial_state: GaussianState, 31 | ) -> ExtendedKalmanFilter { 32 | ExtendedKalmanFilter { 33 | r, 34 | q, 35 | measurement_model, 36 | motion_model, 37 | state: initial_state, 38 | } 39 | } 40 | } 41 | 42 | impl BayesianFilter 43 | for ExtendedKalmanFilter 44 | where 45 | DefaultAllocator: Allocator 46 | + Allocator 47 | + Allocator 48 | + Allocator 49 | + Allocator 50 | + Allocator 51 | + Allocator 52 | + Allocator 53 | + Allocator, 54 | { 55 | fn update_estimate(&mut self, u: &OVector, z: &OVector, dt: T) { 56 | // predict 57 | let g = self 58 | .motion_model 59 | .jacobian_wrt_state(&self.state.x, u, dt.clone()); 60 | self.state.x = self.motion_model.prediction(&self.state.x, u, dt); 61 | self.state.cov = &g * &self.state.cov * g.transpose() + &self.r; 62 | 63 | // update 64 | let h = self.measurement_model.jacobian(&self.state.x, None); 65 | let z_pred = self.measurement_model.prediction(&self.state.x, None); 66 | 67 | let s = &h * &self.state.cov * h.transpose() + &self.q; 68 | let kalman_gain = &self.state.cov * h.transpose() * s.try_inverse().unwrap(); 69 | self.state.x = &self.state.x + &kalman_gain * (z - z_pred); 70 | let shape = self.state.cov.shape_generic(); 71 | self.state.cov = 72 | (OMatrix::identity_generic(shape.0, shape.1) - kalman_gain * h) * &self.state.cov; 73 | } 74 | 75 | fn gaussian_estimate(&self) -> GaussianState { 76 | self.state.clone() 77 | } 78 | } 79 | 80 | /// S : State Size, Z: Observation Size, U: Input Size 81 | pub struct ExtendedKalmanFilterKnownCorrespondences 82 | where 83 | DefaultAllocator: Allocator + Allocator + Allocator, 84 | { 85 | q: OMatrix, 86 | landmarks: FxHashMap>, 87 | measurement_model: Box + Send>, 88 | motion_model: Box + Send>, 89 | state: GaussianState, 90 | } 91 | 92 | impl ExtendedKalmanFilterKnownCorrespondences 93 | where 94 | DefaultAllocator: Allocator + Allocator + Allocator + Allocator, 95 | { 96 | pub fn new( 97 | q: OMatrix, 98 | landmarks: FxHashMap>, 99 | measurement_model: Box + Send>, 100 | motion_model: Box + Send>, 101 | initial_state: GaussianState, 102 | ) -> ExtendedKalmanFilterKnownCorrespondences { 103 | ExtendedKalmanFilterKnownCorrespondences { 104 | q, 105 | landmarks, 106 | measurement_model, 107 | motion_model, 108 | state: initial_state, 109 | } 110 | } 111 | } 112 | 113 | impl BayesianFilterKnownCorrespondences 114 | for ExtendedKalmanFilterKnownCorrespondences 115 | where 116 | DefaultAllocator: Allocator 117 | + Allocator 118 | + Allocator 119 | + Allocator 120 | + Allocator 121 | + Allocator 122 | + Allocator 123 | + Allocator 124 | + Allocator 125 | + Allocator, 126 | { 127 | fn update_estimate( 128 | &mut self, 129 | control: Option>, 130 | measurements: Option)>>, 131 | dt: T, 132 | ) { 133 | // predict 134 | if let Some(u) = control { 135 | let g = self.motion_model.jacobian_wrt_state(&self.state.x, &u, dt); 136 | let v = self.motion_model.jacobian_wrt_input(&self.state.x, &u, dt); 137 | let m = self.motion_model.cov_noise_control_space(&u); 138 | 139 | self.state.x = self.motion_model.prediction(&self.state.x, &u, dt); 140 | self.state.cov = &g * &self.state.cov * g.transpose() + &v * m * v.transpose(); 141 | } 142 | 143 | // update / correction step 144 | if let Some(measurements) = measurements { 145 | let shape = self.state.cov.shape_generic(); 146 | for (id, z) in measurements 147 | .iter() 148 | .filter(|(id, _)| self.landmarks.contains_key(id)) 149 | { 150 | let landmark = self.landmarks.get(id); 151 | let z_pred = self.measurement_model.prediction(&self.state.x, landmark); 152 | let h = self.measurement_model.jacobian(&self.state.x, landmark); 153 | let s = &h * &self.state.cov * h.transpose() + &self.q; 154 | let kalman_gain = &self.state.cov * h.transpose() * s.try_inverse().unwrap(); 155 | self.state.x += &kalman_gain * (z - z_pred); 156 | self.state.cov = (OMatrix::identity_generic(shape.0, shape.1) - kalman_gain * h) 157 | * &self.state.cov; 158 | } 159 | } 160 | } 161 | 162 | fn gaussian_estimate(&self) -> GaussianState { 163 | self.state.clone() 164 | } 165 | } 166 | -------------------------------------------------------------------------------- /src/localization/mod.rs: -------------------------------------------------------------------------------- 1 | mod bayesian_filter; 2 | mod extended_kalman_filter; 3 | mod particle_filter; 4 | mod unscented_kalman_filter; 5 | 6 | pub use bayesian_filter::{BayesianFilter, BayesianFilterKnownCorrespondences}; 7 | pub use extended_kalman_filter::{ExtendedKalmanFilter, ExtendedKalmanFilterKnownCorrespondences}; 8 | pub use particle_filter::{ParticleFilter, ParticleFilterKnownCorrespondences, ResamplingScheme}; 9 | pub use unscented_kalman_filter::UnscentedKalmanFilter; 10 | -------------------------------------------------------------------------------- /src/localization/particle_filter.rs: -------------------------------------------------------------------------------- 1 | #![allow(dead_code)] // TODO: remove this 2 | use nalgebra::{allocator::Allocator, Const, DefaultAllocator, Dim, OMatrix, OVector, RealField}; 3 | use rand::distributions::Distribution; 4 | use rand::Rng; 5 | use rand_distr::{Standard, StandardNormal}; 6 | use rustc_hash::FxHashMap; 7 | 8 | use crate::localization::bayesian_filter::{BayesianFilter, BayesianFilterKnownCorrespondences}; 9 | use crate::models::measurement::MeasurementModel; 10 | use crate::models::motion::MotionModel; 11 | use crate::utils::mvn::MultiVariateNormal; 12 | use crate::utils::state::GaussianState; 13 | 14 | pub enum ResamplingScheme { 15 | IID, 16 | Stratified, 17 | Systematic, 18 | } 19 | 20 | /// S : State Size, Z: Observation Size, U: Input Size 21 | pub struct ParticleFilter 22 | where 23 | DefaultAllocator: Allocator + Allocator + Allocator, 24 | { 25 | r: OMatrix, 26 | q: OMatrix, 27 | measurement_model: Box + Send>, 28 | motion_model: Box + Send>, 29 | pub particules: Vec>, 30 | resampling_scheme: ResamplingScheme, 31 | } 32 | 33 | impl ParticleFilter 34 | where 35 | StandardNormal: Distribution, 36 | Standard: Distribution, 37 | DefaultAllocator: Allocator 38 | + Allocator 39 | + Allocator 40 | + Allocator, S> 41 | + Allocator, Z>, 42 | { 43 | pub fn new( 44 | r: OMatrix, 45 | q: OMatrix, 46 | measurement_model: Box + Send>, 47 | motion_model: Box + Send>, 48 | initial_state: GaussianState, 49 | num_particules: usize, 50 | resampling_scheme: ResamplingScheme, 51 | ) -> ParticleFilter { 52 | let mvn = MultiVariateNormal::new(&initial_state.x, &r).unwrap(); 53 | let mut particules = Vec::with_capacity(num_particules); 54 | for _ in 0..num_particules { 55 | particules.push(mvn.sample()); 56 | } 57 | 58 | ParticleFilter { 59 | r, 60 | q, 61 | measurement_model, 62 | motion_model, 63 | particules, 64 | resampling_scheme, 65 | } 66 | } 67 | } 68 | 69 | impl BayesianFilter 70 | for ParticleFilter 71 | where 72 | DefaultAllocator: Allocator 73 | + Allocator 74 | + Allocator 75 | + Allocator 76 | + Allocator 77 | + Allocator 78 | + Allocator 79 | + Allocator 80 | + Allocator, S> 81 | + Allocator, Z>, 82 | Standard: Distribution, 83 | StandardNormal: Distribution, 84 | { 85 | fn update_estimate(&mut self, u: &OVector, z: &OVector, dt: T) { 86 | let shape = self.particules[0].shape_generic(); 87 | let mvn = 88 | MultiVariateNormal::new(&OMatrix::zeros_generic(shape.0, shape.1), &self.r).unwrap(); 89 | 90 | self.particules = self 91 | .particules 92 | .iter() 93 | .map(|p| self.motion_model.prediction(p, u, dt) + mvn.sample()) 94 | .collect(); 95 | 96 | let mut weights = vec![T::one(); self.particules.len()]; 97 | let shape = z.shape_generic(); 98 | let mvn = 99 | MultiVariateNormal::new(&OMatrix::zeros_generic(shape.0, shape.1), &self.q).unwrap(); 100 | 101 | for (i, particule) in self.particules.iter().enumerate() { 102 | let z_pred = self.measurement_model.prediction(particule, None); 103 | let error = z - z_pred; 104 | let pdf = mvn.pdf(&error); 105 | weights[i] *= pdf; 106 | } 107 | 108 | self.particules = match self.resampling_scheme { 109 | ResamplingScheme::IID => resampling_sort(&self.particules, &weights), 110 | ResamplingScheme::Stratified => resampling_stratified(&self.particules, &weights), 111 | ResamplingScheme::Systematic => resampling_systematic(&self.particules, &weights), 112 | }; 113 | } 114 | 115 | fn gaussian_estimate(&self) -> GaussianState { 116 | gaussian_estimate(&self.particules) 117 | } 118 | } 119 | 120 | /// S : State Size, Z: Observation Size, U: Input Size 121 | pub struct ParticleFilterKnownCorrespondences 122 | where 123 | DefaultAllocator: Allocator + Allocator + Allocator, 124 | { 125 | q: OMatrix, 126 | landmarks: FxHashMap>, 127 | measurement_model: Box + Send>, 128 | motion_model: Box + Send>, 129 | pub particules: Vec>, 130 | } 131 | 132 | impl ParticleFilterKnownCorrespondences 133 | where 134 | StandardNormal: Distribution, 135 | Standard: Distribution, 136 | DefaultAllocator: 137 | Allocator + Allocator + Allocator + Allocator, S>, 138 | { 139 | pub fn new( 140 | initial_noise: OMatrix, 141 | q: OMatrix, 142 | landmarks: FxHashMap>, 143 | measurement_model: Box + Send>, 144 | motion_model: Box + Send>, 145 | initial_state: GaussianState, 146 | num_particules: usize, 147 | ) -> ParticleFilterKnownCorrespondences { 148 | let mvn = MultiVariateNormal::new(&initial_state.x, &initial_noise).unwrap(); 149 | let mut particules = Vec::with_capacity(num_particules); 150 | for _ in 0..num_particules { 151 | particules.push(mvn.sample()); 152 | } 153 | 154 | ParticleFilterKnownCorrespondences { 155 | q, 156 | landmarks, 157 | measurement_model, 158 | motion_model, 159 | particules, 160 | } 161 | } 162 | } 163 | 164 | impl BayesianFilterKnownCorrespondences 165 | for ParticleFilterKnownCorrespondences 166 | where 167 | StandardNormal: Distribution, 168 | Standard: Distribution, 169 | DefaultAllocator: Allocator 170 | + Allocator 171 | + Allocator 172 | + Allocator 173 | + Allocator 174 | + Allocator 175 | + Allocator 176 | + Allocator 177 | + Allocator, S> 178 | + Allocator, Z>, 179 | { 180 | fn update_estimate( 181 | &mut self, 182 | control: Option>, 183 | measurements: Option)>>, 184 | dt: T, 185 | ) { 186 | if let Some(u) = control { 187 | self.particules = self 188 | .particules 189 | .iter() 190 | .map(|p| self.motion_model.sample(p, &u, dt)) 191 | .collect(); 192 | } 193 | 194 | if let Some(measurements) = measurements { 195 | let mut weights = vec![T::one(); self.particules.len()]; 196 | let shape = measurements[0].1.shape_generic(); 197 | let mvn = MultiVariateNormal::new(&OMatrix::zeros_generic(shape.0, shape.1), &self.q) 198 | .unwrap(); 199 | 200 | for (id, z) in measurements 201 | .iter() 202 | .filter(|(id, _)| self.landmarks.contains_key(id)) 203 | { 204 | let landmark = self.landmarks.get(id); 205 | for (i, particule) in self.particules.iter().enumerate() { 206 | let z_pred = self.measurement_model.prediction(particule, landmark); 207 | let error = z - z_pred; 208 | let pdf = mvn.pdf(&error); 209 | weights[i] *= pdf; 210 | } 211 | } 212 | self.particules = resampling(&self.particules, &weights); 213 | // self.particules = resampling_sort(&self.particules, weights); 214 | } 215 | } 216 | 217 | fn gaussian_estimate(&self) -> GaussianState { 218 | gaussian_estimate(&self.particules) 219 | } 220 | } 221 | 222 | fn gaussian_estimate( 223 | particules: &[OVector], 224 | ) -> GaussianState 225 | where 226 | DefaultAllocator: Allocator + Allocator + Allocator, S>, 227 | { 228 | let shape = particules[0].shape_generic(); 229 | let x = particules 230 | .iter() 231 | .fold(OMatrix::zeros_generic(shape.0, shape.1), |a, b| a + b) 232 | / T::from_usize(particules.len()).unwrap(); 233 | let cov = particules 234 | .iter() 235 | .map(|p| p - &x) 236 | .map(|dx| &dx * dx.transpose()) 237 | .fold(OMatrix::zeros_generic(shape.0, shape.0), |a, b| a + b) 238 | / T::from_usize(particules.len()).unwrap(); 239 | GaussianState { x, cov } 240 | } 241 | 242 | fn resampling( 243 | particules: &Vec>, 244 | weights: &[T], 245 | ) -> Vec> 246 | where 247 | DefaultAllocator: Allocator, 248 | Standard: Distribution, 249 | { 250 | let cum_weight: Vec = weights 251 | .iter() 252 | .scan(T::zero(), |state, &x| { 253 | *state += x; 254 | Some(*state) 255 | }) 256 | .collect(); 257 | let weight_tot = *cum_weight.last().unwrap(); 258 | 259 | // sampling 260 | let mut rng = rand::thread_rng(); 261 | (0..particules.len()) 262 | .map(|_| { 263 | let rng_nb = rng.gen::() * weight_tot; 264 | for (i, p) in particules.iter().enumerate() { 265 | if (&cum_weight)[i] > rng_nb { 266 | return p.clone(); 267 | } 268 | } 269 | unreachable!() 270 | }) 271 | .collect() 272 | } 273 | 274 | fn resampling_sort( 275 | particules: &Vec>, 276 | weights: &[T], 277 | ) -> Vec> 278 | where 279 | DefaultAllocator: Allocator, 280 | Standard: Distribution, 281 | { 282 | let total_weight: T = weights.iter().fold(T::zero(), |a, b| a + *b); 283 | let mut rng = rand::thread_rng(); 284 | let mut draws: Vec = (0..particules.len()) 285 | .map(|_| rng.gen::() * total_weight) 286 | .collect(); 287 | resample(&mut draws, total_weight, particules, weights) 288 | } 289 | 290 | fn resampling_stratified( 291 | particules: &Vec>, 292 | weights: &[T], 293 | ) -> Vec> 294 | where 295 | DefaultAllocator: Allocator, 296 | Standard: Distribution, 297 | { 298 | let total_weight: T = weights.iter().fold(T::zero(), |a, b| a + *b); 299 | let mut rng = rand::thread_rng(); 300 | let mut draws: Vec = (0..particules.len()) 301 | .map(|i| { 302 | (T::from_usize(i).unwrap() + rng.gen::()) / T::from_usize(particules.len()).unwrap() 303 | * total_weight 304 | }) 305 | .collect(); 306 | resample(&mut draws, total_weight, particules, weights) 307 | } 308 | 309 | fn resampling_systematic( 310 | particules: &Vec>, 311 | weights: &[T], 312 | ) -> Vec> 313 | where 314 | DefaultAllocator: Allocator, 315 | Standard: Distribution, 316 | { 317 | let total_weight: T = weights.iter().fold(T::zero(), |a, b| a + *b); 318 | let mut rng = rand::thread_rng(); 319 | let draw = rng.gen::(); 320 | let mut draws: Vec = (0..particules.len()) 321 | .map(|i| { 322 | (T::from_usize(i).unwrap() + draw) / T::from_usize(particules.len()).unwrap() 323 | * total_weight 324 | }) 325 | .collect(); 326 | resample(&mut draws, total_weight, particules, weights) 327 | } 328 | 329 | fn resample( 330 | draws: &mut [T], 331 | total_weight: T, 332 | particules: &Vec>, 333 | weights: &[T], 334 | ) -> Vec> 335 | where 336 | DefaultAllocator: Allocator, 337 | Standard: Distribution, 338 | { 339 | draws.sort_unstable_by(|a, b| a.partial_cmp(b).unwrap()); 340 | let mut index = 0; 341 | let mut cum_weight = draws[0]; 342 | (0..particules.len()) 343 | .map(|i| { 344 | while cum_weight < draws[i] { 345 | if index == particules.len() - 1 { 346 | // weird precision edge case 347 | cum_weight = total_weight; 348 | break; 349 | } else { 350 | cum_weight += weights[index]; 351 | index += 1; 352 | } 353 | } 354 | particules[index].clone() 355 | }) 356 | .collect() 357 | } 358 | -------------------------------------------------------------------------------- /src/localization/unscented_kalman_filter.rs: -------------------------------------------------------------------------------- 1 | use nalgebra::{ 2 | allocator::Allocator, Const, DefaultAllocator, Dim, OMatrix, OVector, RealField, U1, 3 | }; 4 | 5 | use crate::localization::bayesian_filter::BayesianFilter; 6 | use crate::models::measurement::MeasurementModel; 7 | use crate::models::motion::MotionModel; 8 | use crate::utils::state::GaussianState; 9 | 10 | /// S : State Size, Z: Observation Size, U: Input Size 11 | pub struct UnscentedKalmanFilter 12 | where 13 | DefaultAllocator: Allocator + Allocator + Allocator, 14 | { 15 | q: OMatrix, 16 | r: OMatrix, 17 | gamma: T, 18 | observation_model: Box + Send>, 19 | motion_model: Box + Send>, 20 | mw: Vec, 21 | cw: Vec, 22 | state: GaussianState, 23 | } 24 | 25 | impl UnscentedKalmanFilter 26 | where 27 | DefaultAllocator: Allocator + Allocator + Allocator, 28 | { 29 | #[allow(clippy::too_many_arguments)] 30 | pub fn new( 31 | q: OMatrix, 32 | r: OMatrix, 33 | observation_model: Box + Send>, 34 | motion_model: Box + Send>, 35 | alpha: T, 36 | beta: T, 37 | kappa: T, 38 | initial_state: GaussianState, 39 | ) -> UnscentedKalmanFilter { 40 | let dim = q.shape_generic().0.value(); 41 | let (mw, cw, gamma) = 42 | UnscentedKalmanFilter::::sigma_weights(dim, alpha, beta, kappa); 43 | UnscentedKalmanFilter { 44 | q, 45 | r, 46 | observation_model, 47 | motion_model, 48 | gamma, 49 | mw, 50 | cw, 51 | state: initial_state, 52 | } 53 | } 54 | 55 | fn sigma_weights(dim: usize, alpha: T, beta: T, kappa: T) -> (Vec, Vec, T) { 56 | let n = T::from_usize(dim).unwrap(); 57 | let lambda = alpha.powi(2) * (n + kappa) - n; 58 | 59 | let v = T::one() / ((T::one() + T::one()) * (n + lambda)); 60 | let mut mw = vec![v; 2 * dim + 1]; 61 | let mut cw = vec![v; 2 * dim + 1]; 62 | 63 | // special cases 64 | let v = lambda / (n + lambda); 65 | mw[0] = v; 66 | cw[0] = v + T::one() - alpha.powi(2) + beta; 67 | 68 | let gamma = (n + lambda).sqrt(); 69 | (mw, cw, gamma) 70 | } 71 | 72 | pub fn generate_sigma_points(&self, state: &GaussianState) -> Vec> { 73 | let dim = self.q.shape_generic().0.value(); 74 | // use cholesky to compute the matrix square root // cholesky(A) = L * L^T 75 | let sigma = state.cov.clone().cholesky().expect("unable to sqrt").l() * self.gamma; 76 | // let mut sigma_points = vec![state.x; 2 * S::USIZE + 1]; 77 | // for i in 0..S::USIZE { 78 | // let sigma_column = sigma.column(i); 79 | // sigma_points[i + 1] += sigma_column; 80 | // sigma_points[i + 1 + S::USIZE] -= sigma_column; 81 | // } 82 | let mut sigma_points = Vec::with_capacity(2 * dim + 1); 83 | sigma_points.push(state.x.clone()); 84 | for i in 0..dim { 85 | let sigma_column = sigma.column(i); 86 | sigma_points.push(&state.x + sigma_column); 87 | sigma_points.push(&state.x - sigma_column); 88 | } 89 | sigma_points 90 | } 91 | } 92 | 93 | impl BayesianFilter 94 | for UnscentedKalmanFilter 95 | where 96 | DefaultAllocator: Allocator 97 | + Allocator 98 | + Allocator 99 | + Allocator 100 | + Allocator 101 | + Allocator 102 | + Allocator 103 | + Allocator 104 | + Allocator 105 | + Allocator, S> 106 | + Allocator, Z>, 107 | { 108 | fn update_estimate( 109 | &mut self, 110 | // state: &GaussianState, 111 | u: &OVector, 112 | z: &OVector, 113 | dt: T, 114 | ) { 115 | let dim_s = self.q.shape_generic().0; 116 | let dim_z = self.r.shape_generic().0; 117 | // predict 118 | let sigma_points = self.generate_sigma_points(&self.state); 119 | let sp_xpred: Vec> = sigma_points 120 | .iter() 121 | .map(|x| self.motion_model.prediction(x, u, dt)) 122 | .collect(); 123 | 124 | let mean_xpred: OVector = sp_xpred 125 | .iter() 126 | .zip(self.mw.iter()) 127 | .map(|(x, w)| x * *w) 128 | .fold(OMatrix::zeros_generic(dim_s, U1), |a, b| a + b); 129 | 130 | let cov_xpred = sp_xpred 131 | .iter() 132 | .map(|x| x - &mean_xpred) 133 | .zip(self.cw.iter()) 134 | .map(|(dx, cw)| &dx * dx.transpose() * *cw) 135 | .fold(OMatrix::zeros_generic(dim_s, dim_s), |a, b| a + b) 136 | + &self.q; 137 | 138 | let prediction = GaussianState { 139 | x: mean_xpred.clone(), 140 | cov: cov_xpred.clone(), 141 | }; 142 | 143 | // update 144 | let sp_xpred = self.generate_sigma_points(&prediction); 145 | let sp_z: Vec> = sp_xpred 146 | .iter() 147 | .map(|x| self.observation_model.prediction(x, None)) 148 | .collect(); 149 | 150 | let mean_z: OVector = sp_z 151 | .iter() 152 | .zip(self.mw.iter()) 153 | .map(|(x, w)| x * *w) 154 | .fold(OMatrix::zeros_generic(dim_z, U1), |a, b| a + b); 155 | 156 | let cov_z = sp_z 157 | .iter() 158 | .map(|x| x - &mean_z) 159 | .zip(self.cw.iter()) 160 | .map(|(dx, cw)| &dx * dx.transpose() * *cw) 161 | .fold(OMatrix::zeros_generic(dim_z, dim_z), |a, b| a + b) 162 | + &self.r; 163 | 164 | let s = sp_xpred 165 | .iter() 166 | .zip(sp_z.iter().zip(self.cw.iter())) 167 | .map(|(x_pred, (z_point, cw))| { 168 | (x_pred - &mean_xpred) * (z_point - &mean_z).transpose() * *cw 169 | }) 170 | .fold(OMatrix::zeros_generic(dim_s, dim_z), |a, b| a + b); 171 | 172 | let y = z - mean_z; 173 | let kalman_gain = s * cov_z.clone().try_inverse().unwrap(); 174 | 175 | let x_est = mean_xpred + &kalman_gain * y; 176 | let cov_est = cov_xpred - &kalman_gain * cov_z * kalman_gain.transpose(); 177 | self.state = GaussianState { 178 | x: x_est, 179 | cov: cov_est, 180 | } 181 | } 182 | 183 | fn gaussian_estimate(&self) -> GaussianState { 184 | self.state.clone() 185 | } 186 | } 187 | -------------------------------------------------------------------------------- /src/main.rs: -------------------------------------------------------------------------------- 1 | use std::error::Error; 2 | 3 | fn main() -> Result<(), Box> { 4 | Ok(()) 5 | } 6 | -------------------------------------------------------------------------------- /src/mapping/ekf_slam_known.rs: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /src/mapping/g2o.rs: -------------------------------------------------------------------------------- 1 | #![allow(non_camel_case_types)] 2 | 3 | use core::str::FromStr; 4 | use nalgebra::{ 5 | Isometry2, Isometry3, Matrix2, Matrix3, Matrix6, Quaternion, Translation2, Translation3, 6 | UnitComplex, UnitQuaternion, Vector2, 7 | }; 8 | use rustc_hash::FxHashMap; 9 | use std::error::Error; 10 | use std::fmt::Debug; 11 | 12 | use crate::mapping::pose_graph_optimization::{Edge, EdgeSE2, EdgeSE2_XY, EdgeSE3, Node}; 13 | 14 | fn iso2(x: f64, y: f64, angle: f64) -> Isometry2 { 15 | Isometry2::from_parts(Translation2::new(x, y), UnitComplex::from_angle(angle)) 16 | } 17 | 18 | fn iso3(x: f64, y: f64, z: f64, qx: f64, qy: f64, qz: f64, qw: f64) -> Isometry3 { 19 | let translation = Translation3::new(x, y, z); 20 | let rotation = UnitQuaternion::from_quaternion(Quaternion::new(qx, qy, qz, qw)); 21 | Isometry3::from_parts(translation, rotation) 22 | } 23 | 24 | fn vec(line: Vec<&str>, skip: usize) -> Vec 25 | where 26 | ::Err: Debug, 27 | { 28 | line.iter() 29 | .skip(skip) 30 | .map(|x| x.parse::().unwrap()) 31 | .collect() 32 | } 33 | 34 | #[allow(clippy::type_complexity)] 35 | pub fn parse_g2o( 36 | filename: &str, 37 | ) -> Result< 38 | ( 39 | usize, 40 | Vec>, 41 | FxHashMap, 42 | FxHashMap, 43 | ), 44 | Box, 45 | > { 46 | let mut edges = Vec::new(); 47 | let mut lut = FxHashMap::default(); 48 | let mut nodes = FxHashMap::default(); 49 | let mut offset = 0; 50 | 51 | for line in std::fs::read_to_string(filename)?.lines() { 52 | let line: Vec<&str> = line.split(' ').filter(|x| !x.is_empty()).collect(); 53 | match line[0] { 54 | "VERTEX_SE2" => { 55 | let id = line[1].parse()?; 56 | let [x, y, angle] = vec(line, 2)[..] else { 57 | todo!() 58 | }; 59 | nodes.insert(id, Node::SE2(iso2(x, y, angle))); 60 | lut.insert(id, offset); 61 | offset += 3; 62 | } 63 | "VERTEX_XY" => { 64 | let id = line[1].parse()?; 65 | let [x, y] = vec(line, 2)[..] else { todo!() }; 66 | nodes.insert(id, Node::XY(Vector2::new(x, y))); 67 | lut.insert(id, offset); 68 | offset += 2; 69 | } 70 | "VERTEX_SE3:QUAT" => { 71 | let id = line[1].parse()?; 72 | let [x, y, z, qx, qy, qz, qw] = vec(line, 2)[..] else { 73 | todo!() 74 | }; 75 | nodes.insert(id, Node::SE3(iso3(x, y, z, qx, qy, qz, qw))); 76 | lut.insert(id, offset); 77 | offset += 6; 78 | } 79 | "EDGE_SE2" => { 80 | let from = line[1].parse()?; 81 | let to = line[2].parse()?; 82 | let [x, y, angle, i_11, i_12, i_13, i_22, i_23, i_33] = vec(line, 3)[..] else { 83 | todo!() 84 | }; 85 | 86 | let measurement = iso2(x, y, angle); 87 | 88 | #[rustfmt::skip] 89 | let information = Matrix3::new( 90 | i_11, i_12, i_13, 91 | i_12, i_22, i_23, 92 | i_13, i_23, i_33 93 | ); 94 | let edge = Edge::SE2_SE2(EdgeSE2::new(from, to, measurement, information)); 95 | edges.push(edge); 96 | } 97 | "EDGE_SE2_XY" => { 98 | let from = line[1].parse()?; 99 | let to = line[2].parse()?; 100 | let [x, y, i_11, i_12, i_22] = vec(line, 3)[..] else { 101 | todo!() 102 | }; 103 | 104 | let measurement = Vector2::new(x, y); 105 | 106 | #[rustfmt::skip] 107 | let information = Matrix2::new( 108 | i_11, i_12, 109 | i_12, i_22 110 | ); 111 | let edge = Edge::SE2_XY(EdgeSE2_XY::new(from, to, measurement, information)); 112 | edges.push(edge); 113 | } 114 | "EDGE_SE3:QUAT" => { 115 | let from = line[1].parse()?; 116 | let to = line[2].parse()?; 117 | let [x, y, z, qx, qy, qz, qw, i_11, i_12, i_13, i_14, i_15, i_16, i_22, i_23, i_24, i_25, i_26, i_33, i_34, i_35, i_36, i_44, i_45, i_46, i_55, i_56, i_66] = 118 | vec(line, 3)[..] 119 | else { 120 | todo!() 121 | }; 122 | 123 | let measurement = iso3(x, y, z, qx, qy, qz, qw); 124 | 125 | #[rustfmt::skip] 126 | let information = Matrix6::new( 127 | i_11, i_12, i_13, i_14, i_15, i_16, 128 | i_12, i_22, i_23, i_24, i_25, i_26, 129 | i_13, i_23, i_33, i_34, i_35, i_36, 130 | i_14, i_24, i_34, i_44, i_45, i_46, 131 | i_15, i_25, i_35, i_45, i_55, i_56, 132 | i_16, i_26, i_36, i_46, i_56, i_66 133 | ); 134 | 135 | let edge = Edge::SE3_SE3(EdgeSE3::new(from, to, measurement, information)); 136 | edges.push(edge); 137 | } 138 | _ => unimplemented!("{}", line[0]), 139 | } 140 | } 141 | 142 | Ok((offset, edges, lut, nodes)) 143 | } 144 | 145 | #[cfg(test)] 146 | mod tests { 147 | use super::*; 148 | 149 | #[test] 150 | fn from_g2o() -> Result<(), Box> { 151 | let filename = "dataset/g2o/simulation-pose-pose.g2o"; 152 | let (len, edges, _lut, nodes) = parse_g2o(filename)?; 153 | assert_eq!(400, nodes.len()); 154 | assert_eq!(1773, edges.len()); 155 | assert_eq!(1200, len); 156 | 157 | let filename = "dataset/g2o/simulation-pose-landmark.g2o"; 158 | let (len, edges, _lut, nodes) = parse_g2o(filename)?; 159 | assert_eq!(77, nodes.len()); 160 | assert_eq!(297, edges.len()); 161 | assert_eq!(195, len); 162 | 163 | let filename = "dataset/g2o/intel.g2o"; 164 | let (len, edges, _lut, nodes) = parse_g2o(filename)?; 165 | assert_eq!(1728, nodes.len()); 166 | assert_eq!(4830, edges.len()); 167 | assert_eq!(5184, len); 168 | 169 | let filename = "dataset/g2o/dlr.g2o"; 170 | let (len, edges, _lut, nodes) = parse_g2o(filename)?; 171 | assert_eq!(3873, nodes.len()); 172 | assert_eq!(17605, edges.len()); 173 | assert_eq!(11043, len); 174 | Ok(()) 175 | } 176 | } 177 | -------------------------------------------------------------------------------- /src/mapping/mod.rs: -------------------------------------------------------------------------------- 1 | mod ekf_slam_known; 2 | mod g2o; 3 | mod pose_graph_optimization; 4 | mod se2_se3; 5 | 6 | pub use pose_graph_optimization::{PoseGraph, PoseGraphSolver}; 7 | -------------------------------------------------------------------------------- /src/mapping/pose_graph_optimization.rs: -------------------------------------------------------------------------------- 1 | #![allow(non_snake_case)] 2 | #![allow(non_camel_case_types)] 3 | #![allow(dead_code)] // TODO: remove this 4 | 5 | use nalgebra::{ 6 | AbstractRotation, DVector, Isometry, Isometry2, Isometry3, Matrix2, Matrix2x3, Matrix3, 7 | Matrix6, SMatrix, SVector, UnitComplex, Vector2, Vector3, 8 | }; 9 | use plotpy::{Curve, Plot}; 10 | use rayon::prelude::*; 11 | use russell_lab::Vector; 12 | use russell_sparse::prelude::*; 13 | use rustc_hash::FxHashMap; 14 | use std::error::Error; 15 | use std::path::Path; 16 | 17 | use crate::mapping::g2o::parse_g2o; 18 | use crate::mapping::se2_se3::{jacobian_so3, skew, skew_m_and_mult_parts}; 19 | 20 | #[derive(Debug)] 21 | pub enum Edge { 22 | SE2_SE2(EdgeSE2), 23 | SE2_XY(EdgeSE2_XY), 24 | SE3_SE3(EdgeSE3), 25 | SE3_XYZ, 26 | } 27 | 28 | #[derive(PartialEq, Debug)] 29 | pub enum PoseGraphSolver { 30 | GaussNewton, 31 | LevenbergMarquardt, 32 | } 33 | 34 | #[derive(Debug)] 35 | pub struct EdgeSE2 { 36 | from: u32, 37 | to: u32, 38 | measurement: Isometry2, 39 | information: Matrix3, 40 | } 41 | 42 | impl EdgeSE2 { 43 | pub fn new( 44 | from: u32, 45 | to: u32, 46 | measurement: Isometry2, 47 | information: Matrix3, 48 | ) -> EdgeSE2 { 49 | EdgeSE2 { 50 | from, 51 | to, 52 | measurement, 53 | information, 54 | } 55 | } 56 | } 57 | 58 | #[derive(Debug)] 59 | pub struct EdgeSE2_XY { 60 | from: u32, 61 | to: u32, 62 | measurement: Vector2, 63 | information: Matrix2, 64 | } 65 | 66 | impl EdgeSE2_XY { 67 | pub fn new( 68 | from: u32, 69 | to: u32, 70 | measurement: Vector2, 71 | information: Matrix2, 72 | ) -> EdgeSE2_XY { 73 | EdgeSE2_XY { 74 | from, 75 | to, 76 | measurement, 77 | information, 78 | } 79 | } 80 | } 81 | 82 | #[derive(Debug)] 83 | pub struct EdgeSE3 { 84 | from: u32, 85 | to: u32, 86 | measurement: Isometry3, 87 | information: Matrix6, 88 | } 89 | 90 | impl EdgeSE3 { 91 | pub fn new( 92 | from: u32, 93 | to: u32, 94 | measurement: Isometry3, 95 | information: Matrix6, 96 | ) -> EdgeSE3 { 97 | EdgeSE3 { 98 | from, 99 | to, 100 | measurement, 101 | information, 102 | } 103 | } 104 | } 105 | 106 | pub struct SparseLinearSystem { 107 | pub H: SparseMatrix, 108 | pub b: Vector, 109 | } 110 | 111 | impl SparseLinearSystem { 112 | pub fn new(len: usize) -> SparseLinearSystem { 113 | let H = SparseMatrix::new_coo( 114 | len, 115 | len, 116 | len * len, 117 | Some(Symmetry::PositiveDefinite(Storage::Full)), 118 | false, 119 | ) 120 | .unwrap(); 121 | let b = Vector::new(len); 122 | SparseLinearSystem { H, b } 123 | } 124 | pub fn solve(&mut self) -> Result, Box> { 125 | // Russell Sparse (SuiteSparse wrapper) is much faster then nalgebra_sparse 126 | let n = self.b.dim(); 127 | let mut solution = Vector::new(n); 128 | 129 | // allocate solver 130 | let mut umfpack = SolverUMFPACK::new()?; 131 | 132 | // parameters 133 | let mut params = LinSolParams::new(); 134 | params.verbose = false; 135 | params.compute_determinant = true; 136 | 137 | // call factorize 138 | umfpack.factorize(&mut self.H, Some(params))?; 139 | 140 | // calculate the solution 141 | umfpack.solve(&mut solution, &self.H, &self.b, false)?; 142 | 143 | Ok(DVector::from_vec(solution.as_data().clone())) 144 | } 145 | } 146 | 147 | #[allow(clippy::upper_case_acronyms)] 148 | #[derive(PartialEq)] 149 | pub enum Node { 150 | SE2(Isometry2), 151 | SE3(Isometry3), 152 | XY(Vector2), 153 | XYZ(Vector3), 154 | } 155 | pub struct PoseGraph<'a> { 156 | len: usize, 157 | nodes: FxHashMap, 158 | edges: Vec>, 159 | lut: FxHashMap, 160 | iteration: usize, 161 | name: &'a str, 162 | solver: PoseGraphSolver, 163 | } 164 | 165 | fn update_linear_system( 166 | sls: &mut SparseLinearSystem, 167 | e: &SVector, 168 | A: &SMatrix, 169 | B: &SMatrix, 170 | omega: &SMatrix, 171 | from: usize, 172 | to: usize, 173 | ) -> Result<(), Box> { 174 | // Compute jacobians 175 | let H_ii = A.transpose() * omega * A; 176 | let H_ij = A.transpose() * omega * B; 177 | let H_ji = H_ij.transpose(); 178 | let H_jj = B.transpose() * omega * B; 179 | 180 | let b_i = A.transpose() * omega * e; 181 | let b_j = B.transpose() * omega * e; 182 | 183 | // Update the linear system 184 | set_matrix(&mut sls.H, from, from, &H_ii)?; 185 | set_matrix(&mut sls.H, from, to, &H_ij)?; 186 | set_matrix(&mut sls.H, to, from, &H_ji)?; 187 | set_matrix(&mut sls.H, to, to, &H_jj)?; 188 | 189 | set_vector(&mut sls.b, from, &b_i); 190 | set_vector(&mut sls.b, to, &b_j); 191 | Ok(()) 192 | } 193 | 194 | fn set_matrix( 195 | A: &mut SparseMatrix, 196 | i: usize, 197 | j: usize, 198 | m: &SMatrix, 199 | ) -> Result<(), Box> { 200 | for ii in 0..R { 201 | for jj in 0..C { 202 | A.put(i + ii, j + jj, m.fixed_view::<1, 1>(ii, jj).x)?; 203 | } 204 | } 205 | Ok(()) 206 | } 207 | 208 | fn set_vector(v: &mut Vector, i: usize, source: &SVector) { 209 | for ii in 0..D { 210 | v.set(i + ii, source.fixed_view::<1, 1>(ii, 0).x + v.get(i + ii)) 211 | } 212 | } 213 | 214 | impl<'a> PoseGraph<'a> { 215 | pub fn new(file_path: &str, solver: PoseGraphSolver) -> Result> { 216 | let (len, edges, lut, nodes) = parse_g2o(file_path)?; 217 | let name = Path::new(file_path).file_stem().unwrap().to_str().unwrap(); 218 | Ok(PoseGraph { 219 | len, 220 | nodes, 221 | edges, 222 | lut, 223 | iteration: 0, 224 | name, 225 | solver, 226 | }) 227 | } 228 | 229 | fn update_nodes(&mut self, dx: &DVector) { 230 | self.nodes.par_iter_mut().for_each(|(id, node)| { 231 | let offset = *self.lut.get(id).unwrap(); 232 | match node { 233 | Node::SE2(node) => { 234 | let diff = dx.fixed_rows::<3>(offset); 235 | node.translation.vector += diff.xy(); 236 | node.rotation *= UnitComplex::from_angle(diff.z); 237 | } 238 | Node::XY(node) => { 239 | *node += dx.fixed_rows::<2>(offset); 240 | } 241 | Node::SE3(_) => todo!(), 242 | Node::XYZ(_) => todo!(), 243 | } 244 | }); 245 | } 246 | 247 | pub fn optimize( 248 | &mut self, 249 | num_iterations: usize, 250 | log: bool, 251 | plot: bool, 252 | ) -> Result, Box> { 253 | let tolerance = 1e-4; 254 | let mut lambda = 0.01; 255 | let mut norms = Vec::new(); 256 | let mut last_error = global_error(self); 257 | let mut errors = vec![last_error]; 258 | if log { 259 | println!( 260 | "Loaded graph with {} nodes and {} edges", 261 | self.nodes.len(), 262 | self.edges.len() 263 | ); 264 | println!("initial error :{:.5}", errors.last().unwrap()); 265 | } 266 | if plot { 267 | self.plot()?; 268 | } 269 | for i in 0..num_iterations { 270 | self.iteration += 1; 271 | let dx = self.build_linear_system(lambda)?.solve()?; 272 | self.update_nodes(&dx); 273 | let norm_dx = dx.norm(); 274 | let error = global_error(self); 275 | if self.solver == PoseGraphSolver::LevenbergMarquardt { 276 | if last_error < error { 277 | self.update_nodes(&(-dx)); // get back old state 278 | lambda *= 2.0; 279 | } else { 280 | lambda /= 2.0; 281 | } 282 | } 283 | 284 | last_error = error; 285 | norms.push(norm_dx); 286 | errors.push(error); 287 | 288 | if log { 289 | println!( 290 | "step {i:3} : |dx| = {norm_dx:3.5}, error = {:3.5}", 291 | errors.last().unwrap() 292 | ); 293 | } 294 | if plot { 295 | self.plot()?; 296 | } 297 | 298 | if norm_dx < tolerance { 299 | break; 300 | } 301 | } 302 | Ok(errors) 303 | } 304 | 305 | fn build_linear_system(&self, lambda: f64) -> Result> { 306 | let mut sls = SparseLinearSystem::new(self.len); 307 | let mut need_to_add_prior = true; 308 | 309 | for edge in &self.edges { 310 | match edge { 311 | Edge::SE2_SE2(edge) => { 312 | let from_idx = *self.lut.get(&edge.from).unwrap(); 313 | let to_idx = *self.lut.get(&edge.to).unwrap(); 314 | 315 | let Some(Node::SE2(x1)) = self.nodes.get(&edge.from) else { 316 | unreachable!() 317 | }; 318 | let Some(Node::SE2(x2)) = self.nodes.get(&edge.to) else { 319 | unreachable!() 320 | }; 321 | 322 | let z = &edge.measurement; 323 | let omega = &edge.information; 324 | 325 | let e = v3(&pose2D_pose2D_constraint(x1, x2, z)); 326 | let (A, B) = linearize_pose2D_pose2D_constraint(x1, x2, z); 327 | 328 | update_linear_system(&mut sls, &e, &A, &B, omega, from_idx, to_idx)?; 329 | 330 | if need_to_add_prior { 331 | const V: f64 = 10000000.0; 332 | sls.H.put(from_idx, from_idx, V)?; 333 | sls.H.put(from_idx + 1, from_idx + 1, V)?; 334 | sls.H.put(from_idx + 2, from_idx + 2, V)?; 335 | need_to_add_prior = false; 336 | } 337 | } 338 | Edge::SE2_XY(edge) => { 339 | let from_idx = *self.lut.get(&edge.from).unwrap(); 340 | let to_idx = *self.lut.get(&edge.to).unwrap(); 341 | 342 | let Some(Node::SE2(x)) = self.nodes.get(&edge.from) else { 343 | unreachable!() 344 | }; 345 | let Some(Node::XY(landmark)) = self.nodes.get(&edge.to) else { 346 | unreachable!() 347 | }; 348 | 349 | let z = &edge.measurement; 350 | let omega = &edge.information; 351 | 352 | let e = pose2D_landmark2D_constraint(x, landmark, z); 353 | let (A, B) = linearize_pose_landmark_constraint(x, landmark); 354 | 355 | update_linear_system(&mut sls, &e, &A, &B, omega, from_idx, to_idx)?; 356 | } 357 | Edge::SE3_SE3(_) => todo!(), 358 | Edge::SE3_XYZ => todo!(), 359 | } 360 | } 361 | sls.b.map(|x| -x); 362 | if self.solver == PoseGraphSolver::LevenbergMarquardt { 363 | for i in 0..self.len { 364 | sls.H.put(i, i, lambda)?; 365 | } 366 | } 367 | 368 | Ok(sls) 369 | } 370 | 371 | fn linearize_and_solve(&self) -> Result, Box> { 372 | self.build_linear_system(0.0)?.solve() 373 | } 374 | 375 | pub fn plot(&self) -> Result<(), Box> { 376 | // poses + landmarks 377 | let mut landmarks_present = false; 378 | let mut poses_seq = Vec::new(); 379 | let mut poses = Curve::new(); 380 | poses 381 | .set_line_style("None") 382 | .set_marker_color("b") 383 | .set_marker_style("o"); 384 | 385 | let mut landmarks = Curve::new(); 386 | landmarks 387 | .set_line_style("None") 388 | .set_marker_color("r") 389 | .set_marker_style("*"); 390 | poses.points_begin(); 391 | landmarks.points_begin(); 392 | for (id, node) in self.nodes.iter() { 393 | match *node { 394 | Node::SE2(n) => { 395 | poses.points_add(n.translation.x, n.translation.y); 396 | poses_seq.push((id, n.translation.vector)); 397 | } 398 | Node::XY(n) => { 399 | landmarks.points_add(n.x, n.y); 400 | landmarks_present = true; 401 | } 402 | Node::SE3(_) => todo!(), 403 | Node::XYZ(_) => todo!(), 404 | } 405 | } 406 | poses.points_end(); 407 | landmarks.points_end(); 408 | 409 | poses_seq.sort_by(|a, b| (a.0).partial_cmp(b.0).unwrap()); 410 | let mut poses_seq_curve = Curve::new(); 411 | poses_seq_curve.set_line_color("r"); 412 | 413 | poses_seq_curve.points_begin(); 414 | for (_, p) in poses_seq { 415 | poses_seq_curve.points_add(p.x, p.y); 416 | } 417 | poses_seq_curve.points_end(); 418 | 419 | // add features to plot 420 | let mut plot = Plot::new(); 421 | 422 | plot.add(&poses).add(&poses_seq_curve); 423 | if landmarks_present { 424 | plot.add(&landmarks); 425 | } 426 | // save figure 427 | plot.set_equal_axes(true) 428 | // .set_figure_size_points(600.0, 600.0) 429 | .save(format!("img/{}-{}-{:?}.svg", self.name, self.iteration, self.solver).as_str())?; 430 | Ok(()) 431 | } 432 | } 433 | 434 | fn v3(iso2: &Isometry2) -> Vector3 { 435 | Vector3::new( 436 | iso2.translation.x, 437 | iso2.translation.y, 438 | iso2.rotation.angle(), 439 | ) 440 | } 441 | fn pose2D_pose2D_constraint, const D: usize>( 442 | x1: &Isometry, 443 | x2: &Isometry, 444 | z: &Isometry, 445 | ) -> Isometry { 446 | z.inverse() * x1.inverse() * x2 447 | } 448 | 449 | fn pose2D_landmark2D_constraint( 450 | x: &Isometry2, 451 | landmark: &Vector2, 452 | z: &Vector2, 453 | ) -> Vector2 { 454 | x.rotation.to_rotation_matrix().transpose() * (landmark - x.translation.vector) - z 455 | } 456 | 457 | fn linearize_pose2D_pose2D_constraint( 458 | x1: &Isometry2, 459 | x2: &Isometry2, 460 | z: &Isometry2, 461 | ) -> (Matrix3, Matrix3) { 462 | let deriv = Matrix2::::new(0.0, -1.0, 1.0, 0.0); 463 | 464 | let z_rot = z.rotation.to_rotation_matrix(); 465 | let x1_rot = x1.rotation.to_rotation_matrix(); 466 | let a_11 = -(z_rot.inverse() * x1_rot.inverse()).matrix(); 467 | let xr1d = deriv * x1.rotation.to_rotation_matrix().matrix(); 468 | let a_12 = 469 | z_rot.transpose() * xr1d.transpose() * (x2.translation.vector - x1.translation.vector); 470 | 471 | #[rustfmt::skip] 472 | let A = Matrix3::new( 473 | a_11.m11, a_11.m12, a_12.x, 474 | a_11.m21, a_11.m22, a_12.y, 475 | 0.0, 0.0, -1.0, 476 | ); 477 | 478 | let b_11 = (z_rot.inverse() * x1_rot.inverse()).matrix().to_owned(); 479 | #[rustfmt::skip] 480 | let B = Matrix3::new( 481 | b_11.m11, b_11.m12, 0.0, 482 | b_11.m21, b_11.m22, 0.0, 483 | 0.0, 0.0, 1.0, 484 | ); 485 | (A, B) 486 | } 487 | 488 | fn linearize_pose3D_pose3D_constraint( 489 | x1: &Isometry3, 490 | x2: &Isometry3, 491 | z: &Isometry3, 492 | ) -> (Matrix6, Matrix6) { 493 | let a = z.inverse(); 494 | let b = x1.inverse() * x2; 495 | let error = a * b; 496 | let a_rot = a.rotation.to_rotation_matrix(); 497 | let b_rot = b.rotation.to_rotation_matrix(); 498 | let error_rot = error.rotation.to_rotation_matrix(); 499 | let dq_dR = jacobian_so3(error_rot.matrix()); // variable name taken over from g2o 500 | 501 | let mut A = Matrix6::zeros(); 502 | A.index_mut((..3, ..3)).copy_from(&(-1.0 * a_rot.matrix())); 503 | A.index_mut((..3, 3..)) 504 | .copy_from(&(a_rot.matrix() * skew(&b.translation.vector))); 505 | A.index_mut((3.., 3..)) 506 | .copy_from(&(dq_dR * skew_m_and_mult_parts(b_rot.matrix(), a_rot.matrix()))); 507 | 508 | let mut B = Matrix6::zeros(); 509 | B.index_mut((..3, ..3)).copy_from(error_rot.matrix()); 510 | 511 | B.index_mut((3.., 3..)) 512 | .copy_from(&(dq_dR * skew_m_and_mult_parts(&Matrix3::identity(), error_rot.matrix()))); 513 | (A, B) 514 | } 515 | 516 | fn linearize_pose_landmark_constraint( 517 | x: &Isometry2, 518 | landmark: &Vector2, 519 | ) -> (Matrix2x3, Matrix2) { 520 | let deriv = Matrix2::::new(0.0, -1.0, 1.0, 0.0); 521 | 522 | let a_1 = -x.rotation.to_rotation_matrix().transpose().matrix(); 523 | let xrd = deriv * *x.rotation.to_rotation_matrix().matrix(); 524 | let a_2 = xrd.transpose() * (landmark - x.translation.vector); 525 | 526 | #[rustfmt::skip] 527 | let A = Matrix2x3::new( 528 | a_1.m11, a_1.m12, a_2.x, 529 | a_1.m21, a_1.m22, a_2.y, 530 | ); 531 | 532 | let B = *x.rotation.to_rotation_matrix().transpose().matrix(); 533 | 534 | (A, B) 535 | } 536 | 537 | fn global_error(graph: &PoseGraph) -> f64 { 538 | graph 539 | .edges 540 | .iter() 541 | .map(|edge| match edge { 542 | Edge::SE2_SE2(edge) => { 543 | let Some(Node::SE2(x1)) = graph.nodes.get(&edge.from) else { 544 | unreachable!() 545 | }; 546 | let Some(Node::SE2(x2)) = graph.nodes.get(&edge.to) else { 547 | unreachable!() 548 | }; 549 | 550 | let z = &edge.measurement; 551 | let omega = &edge.information; 552 | 553 | let e = v3(&pose2D_pose2D_constraint(x1, x2, z)); 554 | 555 | (e.transpose() * omega * e).x 556 | } 557 | Edge::SE2_XY(edge) => { 558 | let Some(Node::SE2(x)) = graph.nodes.get(&edge.from) else { 559 | unreachable!() 560 | }; 561 | let Some(Node::XY(l)) = graph.nodes.get(&edge.to) else { 562 | unreachable!() 563 | }; 564 | 565 | let z = &edge.measurement; 566 | let omega = &edge.information; 567 | let e = pose2D_landmark2D_constraint(x, l, z); 568 | (e.transpose() * omega * e).x 569 | } 570 | Edge::SE3_SE3(_) => todo!(), 571 | Edge::SE3_XYZ => todo!(), 572 | }) 573 | .sum() 574 | } 575 | 576 | #[cfg(test)] 577 | mod tests { 578 | use super::*; 579 | 580 | #[test] 581 | fn initial_global_error() -> Result<(), Box> { 582 | let filename = "dataset/g2o/simulation-pose-pose.g2o"; 583 | let graph = PoseGraph::new(filename, PoseGraphSolver::GaussNewton)?; 584 | approx::assert_abs_diff_eq!(138862234.0, global_error(&graph), epsilon = 10.0); 585 | 586 | let filename = "dataset/g2o/simulation-pose-landmark.g2o"; 587 | let graph = PoseGraph::new(filename, PoseGraphSolver::GaussNewton)?; 588 | approx::assert_abs_diff_eq!(3030.0, global_error(&graph), epsilon = 1.0); 589 | 590 | let filename = "dataset/g2o/intel.g2o"; 591 | let graph = PoseGraph::new(filename, PoseGraphSolver::GaussNewton)?; 592 | approx::assert_abs_diff_eq!(1795139.0, global_error(&graph), epsilon = 1e-2); 593 | 594 | let filename = "dataset/g2o/dlr.g2o"; 595 | let graph = PoseGraph::new(filename, PoseGraphSolver::GaussNewton)?; 596 | approx::assert_abs_diff_eq!(369655336.0, global_error(&graph), epsilon = 10.0); 597 | Ok(()) 598 | } 599 | 600 | #[test] 601 | fn final_global_error() -> Result<(), Box> { 602 | let filename = "dataset/g2o/simulation-pose-pose.g2o"; 603 | let error = *PoseGraph::new(filename, PoseGraphSolver::GaussNewton)? 604 | .optimize(100, false, false)? 605 | .last() 606 | .unwrap(); 607 | approx::assert_abs_diff_eq!(8269.0, error, epsilon = 1.0); 608 | 609 | let filename = "dataset/g2o/simulation-pose-landmark.g2o"; 610 | let error = *PoseGraph::new(filename, PoseGraphSolver::GaussNewton)? 611 | .optimize(100, false, false)? 612 | .last() 613 | .unwrap(); 614 | approx::assert_abs_diff_eq!(474.0, error, epsilon = 1.0); 615 | 616 | let filename = "dataset/g2o/intel.g2o"; 617 | let error = *PoseGraph::new(filename, PoseGraphSolver::GaussNewton)? 618 | .optimize(100, false, false)? 619 | .last() 620 | .unwrap(); 621 | approx::assert_abs_diff_eq!(360.0, error, epsilon = 1.0); 622 | 623 | let filename = "dataset/g2o/dlr.g2o"; 624 | let error = *PoseGraph::new(filename, PoseGraphSolver::GaussNewton)? 625 | .optimize(100, false, false)? 626 | .last() 627 | .unwrap(); 628 | approx::assert_abs_diff_eq!(56860.0, error, epsilon = 1.0); 629 | 630 | Ok(()) 631 | } 632 | 633 | #[test] 634 | fn linearize_pose_pose_constraint_correct() -> Result<(), Box> { 635 | let filename = "dataset/g2o/simulation-pose-landmark.g2o"; 636 | let graph = PoseGraph::new(filename, PoseGraphSolver::GaussNewton)?; 637 | 638 | match &graph.edges[0] { 639 | Edge::SE2_SE2(e) => { 640 | let Some(Node::SE2(x1)) = graph.nodes.get(&e.from) else { 641 | todo!() 642 | }; 643 | let Some(Node::SE2(x2)) = graph.nodes.get(&e.to) else { 644 | todo!() 645 | }; 646 | 647 | let z = e.measurement; 648 | 649 | let e = v3(&pose2D_pose2D_constraint(&x1, &x2, &z)); 650 | let (A, B) = linearize_pose2D_pose2D_constraint(&x1, &x2, &z); 651 | 652 | let A_expected = Matrix3::new(0.0, 1.0, 0.113, -1., 0., 0.024, 0., 0., -1.); 653 | 654 | let B_expected = Matrix3::new(0.0, -1.0, 0.0, 1., 0., 0.0, 0., 0., 1.); 655 | 656 | approx::assert_abs_diff_eq!(A_expected, A, epsilon = 1e-3); 657 | approx::assert_abs_diff_eq!(B_expected, B, epsilon = 1e-3); 658 | approx::assert_abs_diff_eq!(Vector3::zeros(), e, epsilon = 1e-3); 659 | } 660 | _ => panic!(), 661 | } 662 | 663 | match &graph.edges[10] { 664 | Edge::SE2_SE2(e) => { 665 | let Some(Node::SE2(x1)) = graph.nodes.get(&e.from) else { 666 | todo!() 667 | }; 668 | let Some(Node::SE2(x2)) = graph.nodes.get(&e.to) else { 669 | todo!() 670 | }; 671 | 672 | let z = e.measurement; 673 | 674 | let e = v3(&pose2D_pose2D_constraint(&x1, &x2, &z)); 675 | let (A, B) = linearize_pose2D_pose2D_constraint(&x1, &x2, &z); 676 | 677 | let A_expected = 678 | Matrix3::new(0.037, 0.999, 0.138, -0.999, 0.037, -0.982, 0., 0., -1.); 679 | 680 | let B_expected = Matrix3::new(-0.037, -0.999, 0.0, 0.999, -0.037, 0.0, 0., 0., 1.); 681 | 682 | approx::assert_abs_diff_eq!(A_expected, A, epsilon = 1e-3); 683 | approx::assert_abs_diff_eq!(B_expected, B, epsilon = 1e-3); 684 | approx::assert_abs_diff_eq!(Vector3::zeros(), e, epsilon = 1e-3); 685 | } 686 | _ => panic!(), 687 | } 688 | 689 | Ok(()) 690 | } 691 | 692 | #[test] 693 | fn linearize_pose_landmark_constraint_correct() -> Result<(), Box> { 694 | let filename = "dataset/g2o/simulation-pose-landmark.g2o"; 695 | let graph = PoseGraph::new(filename, PoseGraphSolver::GaussNewton)?; 696 | 697 | match &graph.edges[1] { 698 | Edge::SE2_XY(edge) => { 699 | let Some(Node::SE2(x)) = graph.nodes.get(&edge.from) else { 700 | todo!() 701 | }; 702 | let Some(Node::XY(landmark)) = graph.nodes.get(&edge.to) else { 703 | todo!() 704 | }; 705 | 706 | let z = edge.measurement; 707 | 708 | let e = pose2D_landmark2D_constraint(&x, &landmark, &z); 709 | let (A, B) = linearize_pose_landmark_constraint(&x, &landmark); 710 | 711 | let A_expected = Matrix2x3::new(0.0, 1.0, 0.358, -1., 0., -0.051); 712 | 713 | let B_expected = Matrix2::new(0.0, -1.0, 1., 0.); 714 | 715 | approx::assert_abs_diff_eq!(A_expected, A, epsilon = 1e-3); 716 | approx::assert_abs_diff_eq!(B_expected, B, epsilon = 1e-3); 717 | approx::assert_abs_diff_eq!(Vector2::zeros(), e, epsilon = 1e-3); 718 | } 719 | _ => panic!(), 720 | } 721 | Ok(()) 722 | } 723 | 724 | #[test] 725 | fn linearize_and_solve_correct() -> Result<(), Box> { 726 | let filename = "dataset/g2o/simulation-pose-landmark.g2o"; 727 | let graph = PoseGraph::new(filename, PoseGraphSolver::GaussNewton)?; 728 | let dx = graph.linearize_and_solve()?; 729 | let expected_first_5 = DVector::::from_vec(vec![ 730 | 1.68518905e-01, 731 | 5.74311089e-01, 732 | -5.08805168e-02, 733 | -3.67482151e-02, 734 | 8.89458085e-01, 735 | ]); 736 | let first_5 = dx.rows(0, 5).clone_owned(); 737 | approx::assert_abs_diff_eq!(expected_first_5, first_5, epsilon = 1e-3); 738 | Ok(()) 739 | } 740 | } 741 | -------------------------------------------------------------------------------- /src/mapping/se2_se3.rs: -------------------------------------------------------------------------------- 1 | // rust traduction of code in https://github.com/RainerKuemmerle/g2o/blob/master/g2o/types/slam3d/isometry3d_gradients.h 2 | 3 | #![allow(non_snake_case)] 4 | #![allow(non_camel_case_types)] 5 | 6 | use nalgebra::{Matrix3, SMatrix, Vector3}; 7 | 8 | // pub trait Manifold{ 9 | // // fn box_plus(other: &self) -> self; 10 | // fn jacobian_boxplus(&self); 11 | // } 12 | 13 | // impl Manifold for Isometry3 { 14 | // fn jacobian_boxplus(&self) { 15 | // let [x, y, z] = self.translation.vector.as_slice() else {unreachable!()}; 16 | // let [qx, qy, qz, qw] =self.rotation.coords.as_slice() else {unreachable!()}; 17 | 18 | // #[rustfmt::skip] 19 | // let j = [ 20 | // 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 21 | // 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22 | // 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 23 | // 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 24 | // 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 25 | // 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 26 | // 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 27 | // ]; 28 | // } 29 | // } 30 | 31 | /// Source : A tutorial on $\mathbf{SE}(3)$ transformation parameterizations and on-manifold optimization. 32 | /// 33 | /// Equation : Jacobian of the SO(3) logarithm. 34 | /// 35 | /// No the most numerically stable, might be better to use g2o code 36 | pub fn jacobian_so3(m: &Matrix3) -> SMatrix { 37 | let trace = m.trace(); 38 | let cos = (trace - 1.0).sqrt() * 0.5; 39 | let mut a1 = 0.0; 40 | let mut a2 = 0.0; 41 | let mut a3 = 0.0; 42 | let mut b = 0.5; 43 | 44 | if cos < 0.9999999 { 45 | let sin = (1.0 - cos * cos).sqrt(); 46 | let theta = f64::atan2(sin, cos); 47 | let factor = (theta * cos - sin) / (4.0 * sin * sin * sin); 48 | a1 = (m.m32 - m.m23) * factor; 49 | a2 = (m.m13 - m.m31) * factor; 50 | a3 = (m.m21 - m.m12) * factor; 51 | b = 0.5 * theta / sin; 52 | } 53 | 54 | #[rustfmt::skip] 55 | let res = SMatrix::::from_column_slice(&[ 56 | // transpose of actual matrix 57 | a1, a2, a3, 58 | 0.0, 0.0, b, 59 | 0.0, -b, 0.0, 60 | 0.0, 0.0, -b, 61 | a1, a2, a3, 62 | b, 0.0, 0.0, 63 | 0.0, b, 0.0, 64 | -b, 0.0, 0.0, 65 | a1, a2, a3 66 | ]); 67 | res 68 | } 69 | 70 | pub fn skew(t: &Vector3) -> Matrix3 { 71 | #[rustfmt::skip] 72 | let res = Matrix3::new( 73 | 0.0, -t.z, t.y, 74 | t.z, 0.0, -t.x, 75 | -t.y, t.x, 0.0 76 | ); 77 | res 78 | } 79 | 80 | pub fn skew_m_and_mult_parts(m: &Matrix3, mult: &Matrix3) -> SMatrix { 81 | let top = mult * skew(&m.column(0).clone_owned()); 82 | let mid = mult * skew(&m.column(1).clone_owned()); 83 | let bot = mult * skew(&m.column(2).clone_owned()); 84 | let mut ret = SMatrix::::zeros(); 85 | ret.index_mut((0..3, ..)).copy_from(&top); 86 | ret.index_mut((3..6, ..)).copy_from(&mid); 87 | ret.index_mut((6..9, ..)).copy_from(&bot); 88 | ret 89 | } 90 | 91 | // enum Quat{ 92 | // w, x, y, z 93 | // } 94 | 95 | // fn q2m(m: &Matrix3){ 96 | // let trace = m.trace(); 97 | 98 | // } 99 | 100 | #[cfg(test)] 101 | mod tests { 102 | use super::*; 103 | 104 | // #[test] 105 | // fn compute_dq_dR_correct() { 106 | // #[rustfmt::skip] 107 | // let err_rot_matrix = Matrix3::new( 108 | // 1.0, -7.7835e-07, 5.74141e-08, // transposed matrix is displayed 109 | // 7.99504e-07, 1.0, 1.14998e-07, 110 | // -7.15592e-08, -1.46825e-07, 1.0 111 | // ); 112 | // let actual = jacobian_so3(&err_rot_matrix); 113 | // let a1 = -8.18195e-09; 114 | // let a2 = 4.03041e-09; 115 | // let a3 = 4.93079e-08; 116 | // let b = 0.25; 117 | 118 | // #[rustfmt::skip] 119 | // let expected = SMatrix::::from_vec(vec![ 120 | // a1, a2, a3, // transposed matrix is displayed 121 | // 0.0, 0.0, b, 122 | // 0.0, -b, 0.0, 123 | // 0.0, 0.0, -b, 124 | // a1, a2, a3, 125 | // b, 0.0, 0.0, 126 | // 0.0, b, 0.0, 127 | // -b, 0.0, 0.0, 128 | // a1, a2, a3 129 | // ]); 130 | 131 | // println!("{}", 0.5 * actual); 132 | // println!("{}", expected); 133 | 134 | // approx::assert_abs_diff_eq!(0.5 * actual, expected, epsilon = 1e-5); 135 | // } 136 | 137 | #[test] 138 | fn skew_correct() { 139 | #[rustfmt::skip] 140 | let t = Vector3::new(-0.0199389, 2.43871, -0.14102); 141 | 142 | #[rustfmt::skip] 143 | let expected = Matrix3::new( 144 | 0.0, -0.282041, -4.87743, 145 | 0.282041, 0.0, -0.0398779, 146 | 4.87743, 0.0398779, 0.0 147 | ); 148 | 149 | approx::assert_abs_diff_eq!(skew(&(2.0 * t)).transpose(), expected, epsilon = 1e-2); 150 | } 151 | } 152 | -------------------------------------------------------------------------------- /src/models/measurement.rs: -------------------------------------------------------------------------------- 1 | use nalgebra::{ 2 | allocator::Allocator, Const, DefaultAllocator, Dim, Matrix2x3, Matrix2x4, OMatrix, OVector, 3 | RealField, Vector2, Vector3, Vector4, 4 | }; 5 | 6 | pub trait MeasurementModel 7 | where 8 | DefaultAllocator: Allocator + Allocator + Allocator + Allocator, 9 | { 10 | fn prediction(&self, x: &OVector, landmark: Option<&OVector>) -> OVector; 11 | fn jacobian(&self, x: &OVector, landmark: Option<&OVector>) -> OMatrix; 12 | } 13 | 14 | /// Measurement = [range, bearing, signature] 15 | /// Probabilistic Robotics p. 177 16 | pub struct RangeBearingMeasurementModel; 17 | 18 | impl RangeBearingMeasurementModel { 19 | pub fn new() -> Box { 20 | Box::new(RangeBearingMeasurementModel {}) 21 | } 22 | } 23 | 24 | impl MeasurementModel, Const<2>> for RangeBearingMeasurementModel { 25 | fn prediction(&self, x: &Vector3, landmark: Option<&Vector3>) -> Vector2 { 26 | //state 27 | let x_x = x[0]; 28 | let x_y = x[1]; 29 | let x_theta = x[2]; 30 | // landmark 31 | let Some(lm) = landmark else { 32 | panic!("WRONG!!!") 33 | }; 34 | let l_x = lm[0]; 35 | let l_y = lm[1]; 36 | 37 | let q = (l_x - x_x).powi(2) + (l_y - x_y).powi(2); 38 | let q_sqrt = q.sqrt(); 39 | 40 | let range = q_sqrt; 41 | let bearing = f64::atan2(l_y - x_y, l_x - x_x) - x_theta; 42 | Vector2::new(range, bearing) 43 | } 44 | 45 | fn jacobian(&self, x: &Vector3, landmark: Option<&Vector3>) -> Matrix2x3 { 46 | //state 47 | let x_x = x[0]; 48 | let x_y = x[1]; 49 | // landmark 50 | let Some(lm) = landmark else { 51 | panic!("WRONG x2 !!!") 52 | }; 53 | let l_x = lm[0]; 54 | let l_y = lm[1]; 55 | 56 | let q = (l_x - x_x).powi(2) + (l_y - x_y).powi(2); 57 | let q_sqrt = q.sqrt(); 58 | 59 | #[rustfmt::skip] 60 | let jac = Matrix2x3::::new( 61 | -(l_x - x_x) / q_sqrt, -(l_y - x_y) / q_sqrt, 0., 62 | (l_y - x_y) / q, (l_x - x_x) / q, -1., 63 | ); 64 | jac 65 | } 66 | } 67 | 68 | pub struct SimpleProblemMeasurementModel; 69 | 70 | impl SimpleProblemMeasurementModel { 71 | pub fn new() -> Box { 72 | Box::new(SimpleProblemMeasurementModel {}) 73 | } 74 | } 75 | 76 | impl MeasurementModel, Const<2>> for SimpleProblemMeasurementModel { 77 | fn prediction(&self, x: &Vector4, _landmark: Option<&Vector4>) -> Vector2 { 78 | x.xy() 79 | } 80 | 81 | fn jacobian(&self, _x: &Vector4, _landmark: Option<&Vector4>) -> Matrix2x4 { 82 | #[rustfmt::skip] 83 | let jac = Matrix2x4::::new( 84 | 1., 0., 0., 0., 85 | 0., 1., 0., 0. 86 | ); 87 | jac 88 | } 89 | } 90 | -------------------------------------------------------------------------------- /src/models/mod.rs: -------------------------------------------------------------------------------- 1 | pub mod measurement; 2 | pub mod motion; 3 | -------------------------------------------------------------------------------- /src/models/motion.rs: -------------------------------------------------------------------------------- 1 | // use enum_dispatch::enum_dispatch; 2 | use nalgebra::{ 3 | allocator::Allocator, Const, DefaultAllocator, Dim, Matrix2, Matrix3, Matrix3x2, Matrix4, 4 | Matrix4x2, OMatrix, OVector, RealField, Vector2, Vector3, Vector4, 5 | }; 6 | 7 | use rand_distr::{Distribution, Normal}; 8 | 9 | // #[enum_dispatch(MM)] 10 | pub trait MotionModel 11 | where 12 | DefaultAllocator: Allocator 13 | + Allocator 14 | + Allocator 15 | + Allocator 16 | + Allocator 17 | + Allocator, 18 | { 19 | fn prediction(&self, x: &OVector, u: &OVector, dt: T) -> OVector; 20 | fn jacobian_wrt_state(&self, x: &OVector, u: &OVector, dt: T) -> OMatrix; 21 | fn jacobian_wrt_input(&self, x: &OVector, u: &OVector, dt: T) -> OMatrix; 22 | fn cov_noise_control_space(&self, u: &OVector) -> OMatrix; 23 | fn sample(&self, x: &OVector, u: &OVector, dt: T) -> OVector; 24 | } 25 | 26 | pub struct Velocity { 27 | a: [f64; 6], 28 | } 29 | 30 | impl Velocity { 31 | pub fn new(a: [f64; 6]) -> Box { 32 | Box::new(Velocity { a }) 33 | } 34 | } 35 | 36 | impl MotionModel, Const<2>, Const<2>> for Velocity { 37 | fn prediction(&self, x: &Vector3, u: &Vector2, dt: f64) -> Vector3 { 38 | //state 39 | let theta = x[2]; 40 | //control 41 | let v = u[0]; 42 | let w = u[1]; 43 | let delta = if w != 0.0 { 44 | Vector3::new( 45 | v / w * (-theta.sin() + (theta + w * dt).sin()), 46 | v / w * (theta.cos() - (theta + w * dt).cos()), 47 | w * dt, 48 | ) 49 | } else { 50 | // no rotation 51 | Vector3::new(v * theta.cos() * dt, v * theta.sin() * dt, w * dt) 52 | }; 53 | 54 | let mut out = x + delta; 55 | 56 | // Limit theta within [-pi, pi] 57 | let mut theta_out = out[2]; 58 | if theta_out > std::f64::consts::PI { 59 | theta_out -= 2.0 * std::f64::consts::PI; 60 | } else if theta_out < -std::f64::consts::PI { 61 | theta_out += 2.0 * std::f64::consts::PI; 62 | } 63 | out[2] = theta_out; 64 | 65 | out 66 | } 67 | 68 | fn jacobian_wrt_state(&self, x: &Vector3, u: &Vector2, dt: f64) -> Matrix3 { 69 | //state 70 | let theta = x[2]; 71 | //control 72 | let v = u[0]; 73 | let w = u[1]; 74 | if w != 0.0 { 75 | #[rustfmt::skip] 76 | let jac = Matrix3::::new( 77 | 1., 0., v / w * (-theta.cos() + (theta + w * dt).cos()), 78 | 0., 1., v / w * (-theta.sin() + (theta + w * dt).sin()), 79 | 0., 0., 1. 80 | ); 81 | jac 82 | } else { 83 | #[rustfmt::skip] 84 | let jac = Matrix3::::new( 85 | 1., 0., -v * theta.sin() * dt, 86 | 0., 1., -v * theta.cos() * dt, 87 | 0., 0., 1. 88 | ); 89 | jac 90 | } 91 | } 92 | 93 | fn jacobian_wrt_input(&self, x: &Vector3, u: &Vector2, dt: f64) -> Matrix3x2 { 94 | //state 95 | let theta = x[2]; 96 | //control 97 | let v = u[0]; 98 | let w = x[1]; 99 | 100 | if w != 0.0 { 101 | let sint = theta.sin(); 102 | let cost = theta.cos(); 103 | let sintdt = (theta + w * dt).sin(); 104 | let costdt = (theta + w * dt).cos(); 105 | let w2 = w * w; 106 | #[rustfmt::skip] 107 | let jac = Matrix3x2::::new( 108 | (-sint + sintdt) / w, v * ((sint - sintdt) / w2 + costdt * dt / w), 109 | (cost - costdt) / w, v * (-(cost - costdt) / w2 + sintdt * dt / w), 110 | 0., dt 111 | ); 112 | jac 113 | } else { 114 | #[rustfmt::skip] 115 | let jac = Matrix3x2::::new( 116 | theta.cos() * dt, 0., 117 | theta.sin() * dt, 0., 118 | 0., dt 119 | ); 120 | jac 121 | } 122 | } 123 | 124 | fn cov_noise_control_space(&self, u: &Vector2) -> Matrix2 { 125 | let v2 = u[0].powi(2); 126 | let w2 = u[1].powi(2); 127 | let eps = 0.00001; 128 | #[rustfmt::skip] 129 | let cov = Matrix2::::new( 130 | self.a[0] * v2 + self.a[1] * w2 + eps, 0.0, 131 | 0.0, self.a[2] * v2 + self.a[3] * w2 + eps, 132 | ); 133 | cov 134 | } 135 | 136 | fn sample(&self, x: &Vector3, u: &Vector2, dt: f64) -> Vector3 { 137 | //state 138 | let theta = x[2]; 139 | //control 140 | let v = u[0]; 141 | let w = u[1]; 142 | 143 | let v2 = v.powi(2); 144 | let w2 = w.powi(2); 145 | let eps = 0.00001; 146 | let mut rng = rand::thread_rng(); 147 | let v_noisy = Normal::new(v, (self.a[0] * v2 + self.a[1] * w2 + eps).sqrt()) 148 | .unwrap() 149 | .sample(&mut rng); 150 | let w_noisy = Normal::new(w, (self.a[2] * v2 + self.a[3] * w2 + eps).sqrt()) 151 | .unwrap() 152 | .sample(&mut rng); 153 | let gamma_noisy = Normal::new(0.0, (self.a[4] * v2 + self.a[5] * w2).sqrt()) 154 | .unwrap() 155 | .sample(&mut rng); 156 | 157 | let delta = Vector3::new( 158 | v_noisy / w_noisy * (-theta.sin() + (theta + w_noisy * dt).sin()), 159 | v_noisy / w_noisy * (theta.cos() - (theta + w_noisy * dt).cos()), 160 | w_noisy * dt + gamma_noisy * dt, 161 | ); 162 | 163 | let mut out = x + delta; 164 | 165 | // Limit theta within [-pi, pi] 166 | let mut theta_out = out[2]; 167 | if theta_out > std::f64::consts::PI { 168 | theta_out -= 2.0 * std::f64::consts::PI; 169 | } else if theta_out < -std::f64::consts::PI { 170 | theta_out += 2.0 * std::f64::consts::PI; 171 | } 172 | out[2] = theta_out; 173 | 174 | out 175 | } 176 | } 177 | 178 | /// motion model 179 | /// 180 | /// x_{t+1} = x_t + v * dt * cos(yaw) 181 | /// 182 | /// y_{t+1} = y_t + v * dt * sin(yaw) 183 | /// 184 | /// yaw_{t+1} = yaw_t + omega * dt 185 | /// 186 | /// v_{t+1} = v{t} 187 | /// 188 | /// so 189 | /// 190 | /// dx/dyaw = -v * dt * sin(yaw) 191 | /// 192 | /// dx/dv = dt * cos(yaw) 193 | /// 194 | /// dy/dyaw = v * dt * cos(yaw) 195 | /// 196 | /// dy/dv = dt * sin(yaw) 197 | pub struct SimpleProblemMotionModel {} 198 | 199 | impl SimpleProblemMotionModel { 200 | pub fn new() -> Box { 201 | Box::new(SimpleProblemMotionModel {}) 202 | } 203 | } 204 | 205 | impl MotionModel, Const<2>, Const<2>> for SimpleProblemMotionModel { 206 | fn prediction(&self, x: &Vector4, u: &Vector2, dt: f64) -> Vector4 { 207 | let yaw = x[2]; 208 | let v = x[3]; 209 | Vector4::new( 210 | x.x + yaw.cos() * v * dt, 211 | x.y + yaw.sin() * v * dt, 212 | yaw + u.y * dt, 213 | u.x, 214 | ) 215 | } 216 | 217 | fn jacobian_wrt_state(&self, x: &Vector4, u: &Vector2, dt: f64) -> Matrix4 { 218 | let yaw = x[2]; 219 | let v = u[0]; 220 | #[rustfmt::skip] 221 | let jac = Matrix4::::new( 222 | 1., 0., -dt * v * (yaw).sin(), dt * (yaw).cos(), 223 | 0., 1., dt * v * (yaw).cos(), dt * (yaw).sin(), 224 | 0., 0., 1., 0., 225 | 0., 0., 0., 0., 226 | ); 227 | jac 228 | } 229 | fn jacobian_wrt_input(&self, _x: &Vector4, _u: &Vector2, _dt: f64) -> Matrix4x2 { 230 | unimplemented!() 231 | } 232 | fn cov_noise_control_space(&self, _u: &Vector2) -> Matrix2 { 233 | unimplemented!() 234 | } 235 | fn sample(&self, _x: &Vector4, _u: &Vector2, _dt: f64) -> Vector4 { 236 | unimplemented!() 237 | } 238 | } 239 | -------------------------------------------------------------------------------- /src/utils/mod.rs: -------------------------------------------------------------------------------- 1 | pub mod mvn; 2 | pub mod plot; 3 | pub mod state; 4 | 5 | pub fn deg2rad(x: f64) -> f64 { 6 | const DEG2RAD_FACTOR: f64 = std::f64::consts::PI / 180.0; 7 | x * DEG2RAD_FACTOR 8 | } 9 | 10 | pub fn rad2deg(x: f64) -> f64 { 11 | const RAD2DEG_FACTOR: f64 = 180.0 / std::f64::consts::PI; 12 | x * RAD2DEG_FACTOR 13 | } 14 | -------------------------------------------------------------------------------- /src/utils/mvn.rs: -------------------------------------------------------------------------------- 1 | use nalgebra::{ 2 | allocator::Allocator, Const, DefaultAllocator, Dim, OMatrix, OVector, RealField, U1, 3 | }; 4 | use rand::distributions::Distribution; 5 | 6 | #[derive(Debug, Clone)] 7 | pub enum ErrorType { 8 | CovarianceNotSemiDefinitePositive, 9 | } 10 | 11 | #[derive(Debug)] 12 | pub struct Error { 13 | error_type: ErrorType, 14 | } 15 | 16 | impl Error { 17 | pub fn kind(&self) -> &ErrorType { 18 | &self.error_type 19 | } 20 | } 21 | 22 | impl std::error::Error for Error {} 23 | 24 | impl std::fmt::Display for Error { 25 | fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result { 26 | write!(f, "{:?}", self.error_type) 27 | } 28 | } 29 | 30 | pub struct MultiVariateNormal 31 | where 32 | DefaultAllocator: Allocator + Allocator, 33 | { 34 | mean: OVector, 35 | precision: OMatrix, 36 | lower: OMatrix, 37 | factor: T, 38 | } 39 | 40 | impl MultiVariateNormal 41 | where 42 | rand_distr::StandardNormal: Distribution, 43 | DefaultAllocator: Allocator + Allocator + Allocator, D>, 44 | { 45 | pub fn new(mean: &OVector, covariance: &OMatrix) -> Result { 46 | let Some(covariance_cholesky) = covariance.clone().cholesky() else { 47 | return Err(Error { 48 | error_type: ErrorType::CovarianceNotSemiDefinitePositive, 49 | }); 50 | }; 51 | let det = covariance_cholesky.determinant(); 52 | let precision = covariance_cholesky.inverse(); 53 | let factor = 54 | T::one() / (T::two_pi().powi(mean.shape_generic().0.value() as i32) * det).sqrt(); 55 | let mvn = MultiVariateNormal { 56 | mean: mean.clone(), 57 | precision, 58 | lower: covariance_cholesky.l(), 59 | factor, 60 | }; 61 | Ok(mvn) 62 | } 63 | 64 | /// Probability density function 65 | pub fn pdf(&self, x: &OVector) -> T { 66 | let dx = &self.mean - x; 67 | let neg_half = T::from_f32(-0.5).unwrap(); 68 | let interior = (&dx.transpose() * &self.precision * dx).x.clone(); 69 | T::exp(neg_half * interior) * self.factor.clone() 70 | } 71 | 72 | pub fn sample(&self) -> OVector { 73 | // https://juanitorduz.github.io/multivariate_normal/ 74 | let mut rng = rand::thread_rng(); 75 | let dim = self.mean.shape_generic().0; 76 | let u = OVector::::from_distribution_generic( 77 | dim, 78 | U1, 79 | &rand_distr::StandardNormal, 80 | &mut rng, 81 | ); 82 | &self.mean + &self.lower * u 83 | } 84 | } 85 | 86 | #[cfg(test)] 87 | mod tests { 88 | use super::*; 89 | use approx::assert_relative_eq; 90 | use nalgebra as na; 91 | 92 | #[test] 93 | fn test_density() { 94 | // parameters for a standard normal (mean=0, sigma=1) 95 | let mu = na::Vector2::::new(0.0, 0.0); 96 | let precision = na::Matrix2::::new(1.0, 0.0, 0.0, 1.0); 97 | 98 | let mvn = MultiVariateNormal::new(&mu, &precision).unwrap(); 99 | 100 | let x0 = na::Vector2::::new(0.0, 0.0); 101 | let x1 = na::Vector2::::new(1.0, 0.0); 102 | let x2 = na::Vector2::::new(0.0, 1.0); 103 | 104 | let epsilon = 1e-5; 105 | // some spot checks with standard normal 106 | assert_relative_eq!(mvn.pdf(&x0), 0.15915494, epsilon = epsilon); 107 | assert_relative_eq!(mvn.pdf(&x1), 0.09653235, epsilon = epsilon); 108 | assert_relative_eq!(mvn.pdf(&x2), 0.09653235, epsilon = epsilon); 109 | } 110 | } 111 | -------------------------------------------------------------------------------- /src/utils/plot.rs: -------------------------------------------------------------------------------- 1 | use nalgebra::{Const, Matrix2, Matrix3, Vector2}; 2 | use plotters::coord::Shift; 3 | use plotters::prelude::*; 4 | use std::error::Error; 5 | 6 | use crate::data::utias::UtiasDataset; 7 | use crate::utils::state::GaussianState; 8 | 9 | pub fn ellipse_series(xy: Vector2, cov_xy: Matrix2) -> Option> { 10 | let eigen = cov_xy.symmetric_eigen(); 11 | let eigenvectors = eigen.eigenvectors; 12 | let eigenvalues = eigen.eigenvalues; 13 | 14 | let (a, b, angle) = if eigenvalues.x >= eigenvalues.y { 15 | ( 16 | eigenvalues.x.sqrt(), 17 | eigenvalues.y.sqrt(), 18 | f64::atan2(eigenvectors.m21, eigenvectors.m22), 19 | ) 20 | } else { 21 | ( 22 | eigenvalues.y.sqrt(), 23 | eigenvalues.x.sqrt(), 24 | f64::atan2(eigenvectors.m22, eigenvectors.m21), 25 | ) 26 | }; 27 | 28 | let rot_mat = Matrix3::new_rotation(angle) 29 | .fixed_view::<2, 2>(0, 0) 30 | .clone_owned(); 31 | 32 | let ellipse_points = (0..100) 33 | .map(|x| x as f64 / 100.0 * std::f64::consts::TAU) // map [0..100] -> [0..2pi] 34 | .map(|t| rot_mat * Vector2::new(a * t.cos(), b * t.sin()) + xy) //map [0..2pi] -> elipse points 35 | .map(|xy| (xy.x, xy.y)) 36 | .collect(); 37 | Some(ellipse_points) 38 | } 39 | 40 | #[derive(Debug, Default)] 41 | pub struct History { 42 | pub z: Vec<(f64, f64)>, 43 | pub x_true: Vec<(f64, f64)>, 44 | pub x_dr: Vec<(f64, f64)>, 45 | pub x_est: Vec<(f64, f64)>, 46 | // pub gaussian_state: Vec>>, 47 | pub gaussian_state: Vec>>, 48 | } 49 | 50 | pub fn chart( 51 | root: &DrawingArea, 52 | history: &History, 53 | i: usize, 54 | name: &str, 55 | ) -> Result<(), Box> { 56 | let min_x = history 57 | .z 58 | .iter() 59 | .take(i) 60 | .map(|(x, _y)| *x) 61 | .reduce(f64::min) 62 | .unwrap_or(-1.0) 63 | - 1.0; 64 | let min_y = history 65 | .z 66 | .iter() 67 | .take(i) 68 | .map(|(_x, y)| *y) 69 | .reduce(f64::min) 70 | .unwrap_or(-1.0) 71 | - 1.0; 72 | let max_x = history 73 | .z 74 | .iter() 75 | .take(i) 76 | .map(|(x, _y)| *x) 77 | .reduce(f64::max) 78 | .unwrap_or(1.0) 79 | + 1.0; 80 | let max_y = history 81 | .z 82 | .iter() 83 | .take(i) 84 | .map(|(_x, y)| *y) 85 | .reduce(f64::max) 86 | .unwrap_or(1.0) 87 | + 1.0; 88 | // find chart dimensions 89 | let mut chart = ChartBuilder::on(root) 90 | .margin(10) 91 | .caption(name, ("sans-serif", 40)) 92 | .build_cartesian_2d(min_x..max_x, min_y..max_y)?; 93 | 94 | chart.configure_mesh().draw()?; 95 | 96 | chart 97 | .draw_series( 98 | history 99 | .z 100 | .iter() 101 | .take(i) 102 | .map(|(x, y)| Circle::new((*x, *y), 3, RED.filled())), 103 | )? 104 | .label("observation") 105 | .legend(|(x, y)| Circle::new((x, y), 3, RED.filled())); 106 | chart 107 | .draw_series( 108 | history 109 | .x_true 110 | .iter() 111 | .take(i) 112 | .map(|(x, y)| Circle::new((*x, *y), 3, BLUE.filled())), 113 | )? 114 | .label("true position") 115 | .legend(|(x, y)| Circle::new((x, y), 3, BLUE.filled())); 116 | chart 117 | .draw_series( 118 | history 119 | .x_dr 120 | .iter() 121 | .take(i) 122 | .map(|(x, y)| Circle::new((*x, *y), 3, YELLOW.filled())), 123 | )? 124 | .label("Dead reckoning") 125 | .legend(|(x, y)| Circle::new((x, y), 3, YELLOW.filled())); 126 | chart 127 | .draw_series( 128 | history 129 | .x_est 130 | .iter() 131 | .take(i) 132 | .map(|(x, y)| Circle::new((*x, *y), 3, GREEN.filled())), 133 | )? 134 | .label("kalman estimate") 135 | .legend(|(x, y)| Circle::new((x, y), 3, GREEN.filled())); 136 | 137 | // Draw ellipse 138 | let state = history.gaussian_state.get(i).unwrap(); 139 | let cov_xy = state.cov.fixed_view::<2, 2>(0, 0).clone_owned(); 140 | let xy = state.x.fixed_view::<2, 1>(0, 0).clone_owned(); 141 | 142 | chart.draw_series(std::iter::once(Polygon::new( 143 | ellipse_series(xy, cov_xy).unwrap(), 144 | GREEN.mix(0.4), 145 | )))?; 146 | chart.draw_series(std::iter::once(PathElement::new( 147 | ellipse_series(xy, cov_xy).unwrap(), 148 | GREEN, 149 | )))?; 150 | 151 | chart 152 | .configure_series_labels() 153 | .position(SeriesLabelPosition::LowerRight) 154 | .border_style(BLACK) 155 | .draw()?; 156 | Ok(()) 157 | } 158 | 159 | pub fn plot_landmarks( 160 | dataset: &UtiasDataset, 161 | states: &[GaussianState>], 162 | states_measurement: &[GaussianState>], 163 | max_time: f64, 164 | filename: &str, 165 | name: &str, 166 | ) -> Result<(), Box> { 167 | // Create output directory if it didnt exist 168 | std::fs::create_dir_all("./img")?; 169 | 170 | let root = BitMapBackend::new(filename, (1024, 768)).into_drawing_area(); 171 | root.fill(&WHITE)?; 172 | // let name = "Particle Filter (Monte Carlo) landmarks"; 173 | let min_x = 0.0; 174 | let max_x = 5.0; 175 | let min_y = -6.0; 176 | let max_y = 5.0; 177 | 178 | let mut chart = ChartBuilder::on(&root) 179 | .margin(10) 180 | .caption(name, ("sans-serif", 40)) 181 | .build_cartesian_2d(min_x..max_x, min_y..max_y)?; 182 | 183 | chart.configure_mesh().draw()?; 184 | 185 | // Ground Truth 186 | chart 187 | .draw_series( 188 | dataset 189 | .groundtruth 190 | .iter() 191 | .filter(|p| p.time <= max_time) 192 | .map(|p| Circle::new((p.x, p.y), 1, BLUE.filled())), 193 | )? 194 | .label("Ground truth") 195 | .legend(|(x, y)| Circle::new((x, y), 3, BLUE.filled())); 196 | 197 | // Landmarks 198 | chart 199 | .draw_series( 200 | dataset 201 | .landmarks 202 | .values() 203 | .map(|lm| Circle::new((lm.x, lm.y), 5, RED.filled())), 204 | )? 205 | .label("Landmarks") 206 | .legend(|(x, y)| Circle::new((x, y), 5, RED.filled())); 207 | 208 | chart.draw_series(dataset.landmarks.values().map(|lm| { 209 | Text::new( 210 | format!("{:?}", lm.subject_nb), 211 | (lm.x + 0.05, lm.y), 212 | ("sans-serif", 15), 213 | ) 214 | }))?; 215 | 216 | // States 217 | chart 218 | .draw_series( 219 | states 220 | .iter() 221 | .map(|gs| Circle::new((gs.x[0], gs.x[1]), 1, GREEN.filled())), 222 | )? 223 | .label("Estimates") 224 | .legend(|(x, y)| Circle::new((x, y), 3, GREEN.filled())); 225 | 226 | // States Measurements 227 | chart 228 | .draw_series( 229 | states_measurement 230 | .iter() 231 | .map(|gs| Circle::new((gs.x[0], gs.x[1]), 1, RED.filled())), 232 | )? 233 | .label("Estimates Measurements") 234 | .legend(|(x, y)| Cross::new((x, y), 3, RED.filled())); 235 | 236 | // Legend 237 | chart 238 | .configure_series_labels() 239 | .position(SeriesLabelPosition::LowerRight) 240 | .border_style(BLACK) 241 | .draw()?; 242 | 243 | root.present()?; 244 | 245 | Ok(()) 246 | } 247 | -------------------------------------------------------------------------------- /src/utils/state.rs: -------------------------------------------------------------------------------- 1 | use nalgebra::{allocator::Allocator, DefaultAllocator, Dim, OMatrix, OVector, RealField}; 2 | 3 | #[derive(Debug, Clone)] 4 | pub struct GaussianState 5 | where 6 | DefaultAllocator: Allocator + Allocator, 7 | { 8 | /// State Vector 9 | pub x: OVector, 10 | /// Covariance Matrix 11 | pub cov: OMatrix, 12 | } 13 | --------------------------------------------------------------------------------