├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── DEPENDENCIES.md ├── LICENSE ├── NOTICE ├── README.md ├── experiments ├── CMakeLists.txt ├── README.md ├── datasets │ ├── README.md │ ├── city10000 │ │ ├── README.md │ │ └── city10000.g2o │ ├── csail │ │ ├── README.md │ │ ├── csail.g2o │ │ └── csail.toro │ ├── cubicle │ │ ├── README.md │ │ └── cubicle.g2o │ ├── garage │ │ ├── README.md │ │ └── parking-garage.g2o │ ├── intel │ │ ├── README.md │ │ └── intel.g2o │ ├── manhattan │ │ ├── README.md │ │ └── manhattanOlson3500.g2o │ ├── manhattan_noisy │ │ ├── README.md │ │ └── manhattan_noisy.irl │ ├── rim │ │ ├── README.md │ │ └── rim.g2o │ ├── sphere2500 │ │ ├── README.md │ │ └── sphere2500.g2o │ └── sphere_a │ │ ├── README.md │ │ └── sphere_a.g2o ├── exp_runner │ ├── CMakeLists.txt │ ├── README.md │ └── include │ │ └── exp_runner │ │ ├── DCRunner-inl.h │ │ ├── DCRunner.h │ │ ├── Factory.h │ │ ├── GNCBatchRunner-inl.h │ │ ├── GNCBatchRunner.h │ │ ├── MEstRunner-inl.h │ │ ├── MEstRunner.h │ │ ├── MaxMixRunner-inl.h │ │ ├── MaxMixRunner.h │ │ ├── ModeDetermination.h │ │ ├── PCMRunner-inl.h │ │ ├── PCMRunner.h │ │ ├── PseudoGtRunner-inl.h │ │ ├── PseudoGtRunner.h │ │ ├── RiSAM2Runner-inl.h │ │ ├── RiSAM2Runner.h │ │ ├── Runner-inl.h │ │ └── Runner.h ├── irl │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── irl │ │ │ ├── irl.h │ │ │ ├── irl_types-inl.h │ │ │ └── irl_types.h │ └── python │ │ ├── irl_parsing.py │ │ └── irl_types.py ├── run-experiment.cpp ├── scripts │ ├── README.md │ ├── animate-traj │ ├── comparisons.py │ ├── g2o-2-irl │ ├── gtg2o-2-values │ ├── make-gridworld-dataset │ ├── multi-plot │ ├── plot-timing │ ├── plot-traj │ ├── plot.py │ ├── precision-recall │ ├── run-batch-g2o │ ├── run-dataset-parallel │ └── toro-2-g2o └── thirdparty │ └── CMakeLists.txt ├── media └── result.png └── risam ├── .clang-format ├── CMakeLists.txt ├── README.md ├── include └── risam │ ├── DoglegLineSearch.h │ ├── GraduatedFactor.h │ ├── GraduatedKernel.h │ └── RISAM2.h └── src ├── GraduatedFactor.cpp ├── GraduatedKernel.cpp └── RISAM2.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | .vscode/ 3 | *.so 4 | __pycache__/ -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "experiments/thirdparty/Kimera-RPGO"] 2 | path = experiments/thirdparty/Kimera-RPGO 3 | url = https://github.com/MIT-SPARK/Kimera-RPGO 4 | [submodule "experiments/thirdparty/dcsam"] 5 | path = experiments/thirdparty/dcsam 6 | url = https://github.com/MarineRoboticsGroup/dcsam 7 | [submodule "dcsam"] 8 | path = dcsam 9 | url = https://github.com/MarineRoboticsGroup/dcsam 10 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(robust-inference CXX C) 2 | cmake_minimum_required(VERSION 3.10) 3 | 4 | message(STATUS "================ ROBUST-INFERENCE ======================") 5 | 6 | find_package(GTSAM REQUIRED) 7 | include_directories(${GTSAM_INCLUDE_DIR}) 8 | 9 | # build riSAM 10 | add_subdirectory(risam) 11 | 12 | # Build Experiments directory 13 | add_subdirectory(experiments) 14 | -------------------------------------------------------------------------------- /DEPENDENCIES.md: -------------------------------------------------------------------------------- 1 | # Dependencies 2 | 3 | This file outlines the direct dependencies of this code specifies the license that each dependency uses and specifies how this code interacts with that dependency. 4 | 5 | # EGNC-PGO 6 | * riSAM [MIT license] (Checkout NOTICE file) 7 | 8 | # riSAM 9 | * C++ and its standard libraries 10 | * GTSAM [BSD license] (unmodified linked dependency), (Modified source code to create riSAM algorithm see: RISAM2.h, RISAM2.cpp) 11 | 12 | # Experiments 13 | * Runners: 14 | * C++ and its builtin libraries 15 | * Kimera-RPGO [BSD 2-Clause License] (referenced as submodule) 16 | * dcsam [MIT License] (referenced as submodule) 17 | * GTSAM [BSD License] (unmodified linked dependency) 18 | * Boost [Boost Software License] (unmodified linked dependency) 19 | 20 | * Scripts 21 | * Python and its builtin Standard libraries 22 | * GTSAM [BSD license] (unmodified linked dependency) 23 | * NUMPY [Numpy License] (unmodified linked dependency) 24 | * Matplotlib [Matplotlib License] (unmodified linked dependency) 25 | * SciPy [BSD licensed] (unmodified linked dependency) 26 | 27 | * Datasets 28 | * Derived from various published works. Source reported in readme for each dataset. 29 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Daniel McGann 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 | -------------------------------------------------------------------------------- /NOTICE: -------------------------------------------------------------------------------- 1 | NOTICE 2 | 3 | This software is a modified version of the original riSAM, which is distributed under the terms of the MIT License by Daniel McGann. 4 | 5 | Modifications and distribution by: Wonseok Kang, Jaehyun Kim, Jiseong Chung, Seungwon Choi, and Tae-wan Kim 6 | Contact: eoid361@snu.ac.kr, jaedalong@snu.ac.kr, dntksdmfwmf@snu.ac.kr, csw3575@snu.ac.kr, taewan@snu.ac.kr 7 | Date: 10th, Oct., 2023 8 | 9 | Please refer to the original LICENSE file for the terms of the MIT License. 10 | Please refer to the README.md file for original repository information. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Efficient Graduated Non-Convexity for Pose Graph Optimization (EGNC-PGO) 2 | 3 | [![arXiv](https://img.shields.io/badge/cs.RO-2310.06765-b31b1b?logo=arxiv&logoColor=red)](https://arxiv.org/abs/2310.06765) 4 | 5 |

6 | EGNC-PGO showed better result in challenging case than the former approach 9 |

10 | 11 | # Structure 12 | * `experiments`: Contains the implementations for experiments. 13 | * `risam`: Contains the implementation of the algorithm. 14 | 15 | 16 | ## Building Instructions (Validated as of Oct 2023) 17 | * Version Summary (tested and confirmed with the following dependency versions) 18 | * GTSAM: Tag=4.2a8, exact hash=9902ccc0a4f62123e91f057babe3612a95c15c20 19 | * KimeraRPGO: exact hash=8c5e163ba38345ff583d87403ad53bf966c0221b 20 | * dcsam: exact hash=b7f62295eec201fb00ee6d1d828fa551ac1f4bd7 21 | * GCC: 11.4.0 22 | * These should be checked out when the git submodules are initialized, but are included here for completeness 23 | 24 | * GTSAM 25 | * Download [GTSAM version 4.2a8](https://github.com/borglab/gtsam/releases/tag/4.2a8) **! 4.2a9 not working!** 26 | * Setup compile time options [required by KimeraRPGO](https://github.com/MIT-SPARK/Kimera-RPGO) 27 | * Build and optionally install GTSAM (Scripts assume GTSAM python is installed in the active python environment) 28 | * Clone EGNC-PGO and Submodules 29 | * `git clone --recursive https://github.com/SNU-DLLAB/EGNC-PGO.git` 30 | * Build GTSAM 31 | * Configure cmake with following options: `cmake .. -DGTSAM_POSE3_EXPMAP=ON -DGTSAM_ROT3_EXPMAP=ON -DGTSAM_USE_SYSTEM_EIGEN=ON` 32 | * Link GTSAM 33 | * If you `install` GTSAM this should be automatic 34 | * If you are working with a local build of GTSAM set `GTSAM_DIR` and `GTSAM_INCLUDE_DIR` to the appropriate directories. 35 | * Build EGNC-PGO with riSAM 36 | * `cd EGNC-PGO` 37 | * `mkdir build` 38 | * `cd build` 39 | * `cmake ..` 40 | * `make` 41 | 42 | 43 | ## Acknowlodgement 44 | The original code is from "Robot Perception Lab - Carnegie Mellon University". link: https://github.com/rpl-cmu/risam 45 | -------------------------------------------------------------------------------- /experiments/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | message(STATUS "================ EXPERIMENTS ======================") 2 | 3 | # Prior Works for Comparison 4 | add_subdirectory(thirdparty) 5 | 6 | # Subprojects 7 | add_subdirectory(irl) 8 | add_subdirectory(exp_runner) 9 | 10 | # Extra Boost Libraries needed by experiments 11 | FIND_PACKAGE(Boost COMPONENTS program_options REQUIRED) 12 | INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS}) 13 | 14 | # Declare and configure experiment runner executable 15 | add_executable(run-experiment "run-experiment.cpp") 16 | target_link_libraries(run-experiment gtsam ${Boost_LIBRARIES} exp_runner irl) -------------------------------------------------------------------------------- /experiments/README.md: -------------------------------------------------------------------------------- 1 | # Experimental Validation 2 | 3 | This folder contains scripts, data and results used for experimental validation of EGNC-PGO, riSAM and relative prior works. 4 | This file provides general documentation on the process, file formats, and how to use. 5 | 6 | # Directory Structure 7 | * `dataset`: Contains benchmark SLAM datasets in g2o/toro format 8 | * `exp_runner`: Implements wrappers around all methods so that they all provide a standard interface 9 | * `scripts`: Contains a number of useful pythons scripts for manipulating datasets, evaluating results, and visualizing 10 | * `thirdparty`: Contains thirdparty code that implements prior works 11 | * `run-experiment.cpp`: Main entry point for running a method implemented in `exp_runner` on an IRL dataset (For details on IRL datasets see `experiments/irl/README.md`). 12 | 13 | Each subdirectory contains its own README with relevant information. 14 | 15 | # Experiment Runner 16 | The executable `run-experiment` is the main entry point to running EGNC-PGO, riSAM and prior works. Its arguments are as follows: 17 | * `irl_file`, `i`: Path to Incremental Robot Log file. (see `experiments/irl/README.md`) 18 | * `method`, `m`: The name of the method to run (e.g. pseudo-gt, see `exp_runners/include/Factory.h` for more options). 19 | * `save_every_n`, `n`: Runner will save intermediate result files every N iterations. 20 | * `output_dir`, `o`: Directory to which the results will be saved. 21 | * `mode`, `d`: Mode to run (only works with risam method. For the others, it doesn't work but required.). 0: original risam, 3: EGNC-PGO (1: B-spline without adaptive, 2: adaptive, from AGNC-PGO) 22 | 23 | All options are required. This script will produce an output whos contents are specified in the next section. 24 | 25 | ## Experiment Runner Quirks 26 | The Experiment Runner interface was originally written to support generic multi-modal measurements, rather than just instances of robust-SLAM problems where we have at most two modes (some measurement and the Null Hypothesis). However, not all of the method implementations are written this generically. All method implementations are guaranteed to work for IRL files (see `experiments/irl/README.md`) that contain, single modal priors, single modal odometry, and loop-closure measurements with two modes, a measurement and a Null Hypothesis mode. If you use the scripts in `scripts` to generate datasets you should be all set to use this interface. 27 | 28 | # Quick Start Guide 29 | The following is an example work-flow to play with EGNC-PGO and the prior works included in this repo. In this we assume the working directory is `experiments` 30 | 31 | ### 1. Corrupt a Gridworld dataset 32 | * First lets make a directory to store all of our data and results. 33 | * `mkdir ~//EGNC-PGO_test_environment` 34 | * `mkdir ~//EGNC-PGO_test_environment/datasets` 35 | * Next lets corrupt a 2d dataset with outliers and convert to IRL format using the `g2o-2-irl` script 36 | * `./scripts/g2o-2-irl -i datasets/csail/csail.g2o -n CSAIL -o ~//EGNC-PGO_test_environment/datasets/ --add_outliers --outlier_percentiles 10 --outlier_covariance 0.015 0.01 0.001 --outlier_chi2_thresh 0.95` 37 | * `-i` is the input dataset, in this case CSAIL 38 | * `-o` is the output directory for the datasets generated by the script 39 | * `--add_outliers` Flags that we are adding outliers while converting the dataset 40 | * `--outlier_percentiles` Are the percentiles of total loop closures that outliers should make up. If multiple are given the script generates multiple datasets. 41 | * `--outlier_covariance` specifies the noise model for outlier loop-closures and the value above is approximately the average loop-closure noise model from the CSAIL dataset. For other datasets, a model should be used to match the dataset. 42 | * `--outlier_chi2_thresh` specifies the chi-squared value that an outlier must have relative to the un-corrupted solution. 43 | * For more options run the script with `--help` 44 | 45 | * You should now see `~//EGNC-PGO_test_environment/datasets/10` that contains one CSAIL dataset with 10% outliers. 46 | 47 | ### 2. Run EGNC-PGO on the dataset 48 | * First lets make a directory to store the results 49 | * `mkdir ~//EGNC-PGO_test_environment/results` 50 | * Now lets run EGNC-PGO 51 | * `../build/experiments/run-experiment -i ~//EGNC-PGO_test_environment/datasets/10/CSAIL_10_random_0.irl -o ~//EGNC-PGO_test_environment/results/ -m risam -n 1 -d 3` 52 | * We should now see a directory in results like `results/CSAIL_10_random_0_risam_YYYY-MM-DD_HH-MM-SS` 53 | * You can run prior works by changing the `-m` ("method") option 54 | * You can run riSAM by using `-m risam -d 0` option 55 | * You can run AGNC-PGO by using `-m risam -d 1`, or `-m risam -d 2` option 56 | * You can run EGNC-PGO by using `-m risam -d 3` option 57 | 58 | ### 3. Plot our results 59 | * Lets visualize the results 60 | * `./scripts/plot-traj -i ~//EGNC-PGO_test_environment/datasets/10/CSAIL_10_random_0.irl -r ~//EGNC-PGO_test_environment/results/CSAIL_10_random_0_risam_YYYY-MM-DD_HH-MM-SS/ --legend` 61 | * You should see a plot of the final solution and print outs of the trajectory. 62 | * Further, lets animate the trajectory so we can see all the incremental steps 63 | * `./scripts/animate-traj -i ~//EGNC-PGO_test_environment/datasets/10/CSAIL_10_random_0.irl -r ~//EGNC-PGO_test_environment/results/CSAIL_10_random_0_risam_YYYY-MM-DD_HH-MM-SS/iterations/` 64 | 65 | ## Reproduction of the experimental results of the paper 66 | * We provide the dataset used in experimental results of our paper, shown in Fig. 6. 67 | * To reproduce the result, follow the steps: 68 | * First, make a directory to save the results 69 | * `mkdir ~//EGNC-PGO_test_environment/results` 70 | * Then, run EGNC-PGO 71 | * `../build/experiments/run-experiment -i ./datasets/manhattan_noisy/manhattan_noisy.irl -o ~//EGNC-PGO_test_environment/results/ -m risam -n 1 -d 3` 72 | * You can run prior works by changing the `-m` ("method") option 73 | * Lets visualize the results 74 | * `./scripts/plot-traj -i ~//EGNC-PGO/experiments/datasets/manhattan_noisy/manhattan_noisy.irl -r ~//EGNC-PGO_test_environment/results/manhattan_noisy_0_random_0_risam_YYYY-MM-DD_HH-MM-SS/ --legend` 75 | 76 | # Prior Works 77 | 78 | Each entry the name is followed by common abbreviation then "Method Name" as defined in `exp_runner/include/exp_runner/Factor.h` 79 | 80 | ### riSAM 81 | ''' 82 | McGann, Daniel, John G. Rogers, and Michael Kaess. "Robust Incremental 83 | Smoothing and Mapping (riSAM)." In 2023 IEEE International Conference 84 | on Robotics and Automation (ICRA), pp. 4157-4163. IEEE, 2023. 85 | ''' 86 | Implementation provided with mode 0 87 | 88 | ### Graduated Non-Convexity (GNC, gnc-batch) 89 | ``` 90 | H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated non- 91 | convexity for robust spatial perception: From non-minimal solvers to 92 | global outlier rejection,” IEEE Robotics and Automation Letters (RA- 93 | L), vol. 5, no. 2, pp. 1127–1134, 2020. 94 | ``` 95 | Implementation provided in GTSAM 96 | 97 | ### Pairwise Consistency Maximization (PCM, pcm) 98 | Note: different method names correspond to different hyper-parameters. 99 | ``` 100 | J. Mangelson, D. Dominic, R. Eustice, and R. Vasudevan, “Pairwise 101 | consistent measurement set maximization for robust multi-robot map 102 | merging,” in Proc. IEEE Intl. Conf. on Robotics and Automation 103 | (ICRA), Brisbane, AU, 2018, pp. 2916–2923. 104 | ``` 105 | Implementation provided in KimeraRPGO (`thirdparty/Kimera-RPGO`) 106 | 107 | ### MaxMixture (MaxMix, maxmix) 108 | ``` 109 | E. Olson and P. Agarwal, “Inference on networks of mixtures for robust 110 | robot mapping,” Intl. J. of Robotics Research (IJRR), vol. 32, no. 7, 111 | pp. 826–840, 2013. 112 | ``` 113 | Implementation internal. 114 | 115 | ### MEstimator (MEst, method name dependent on the M-Estimator used) 116 | ``` 117 | Z. Zhang, “Parameter estimation techniques: a tutorial with application 118 | to conic fitting,” Image and Vision Computing, vol. 15, no. 1, pp. 59– 119 | 76, 1997. 120 | ``` 121 | Implementation baked into GTSAM. 122 | 123 | ### Discrete Continuous Smoothing and Mapping (DC-SAM, dcsam) 124 | ``` 125 | K. Doherty, Z. Lu, K. Singh, and J. Leonard, “Discrete-continuous 126 | smoothing and mapping,” arXiv preprint arXiv:2204.11936v2 [cs], 127 | 2022 128 | ``` 129 | Implementation provided by DC-SAM (`thirdparty/dcsam`) 130 | 131 | 132 | ### Adaptive Graduated Non-Convexity for Pose Graph Optimization (AGNC-PGO) 133 | ``` 134 | Choi, Seungwon, Wonseok Kang, Jiseong Chung, Jaehyun Kim, and Tae-wan Kim. "Adaptive Graduated Non-Convexity for Pose Graph Optimization." arXiv preprint arXiv:2308.11444 (2023). 135 | ``` 136 | Our prior work. Provided with modes 1, 2 137 | -------------------------------------------------------------------------------- /experiments/datasets/README.md: -------------------------------------------------------------------------------- 1 | This directory holds a number of benchmark pose-graph datasets. Some of which were used in the original riSAM publication. These benchmark datasets are all stored as g2o files and were originally meant for batch optimization. See `scripts` for info on how to convert these to IRL datasets for use with the rest of the `experiment` machinery. 2 | 3 | Datasets: 4 | * City1000 \[2d\] (also referred to as City10k) - Very long synthetic grid world dataset 5 | * CSAIL \[2d\] - Short real-world dataset with very few loop closures 6 | * Cubicle \[3d\] (broken) - longer term 3d, but still largely planar dataset 7 | * Garage \[3d\] - longer real-world 3d dataset with interesting elevation changes, however, the noise models are very inaccurate to reality 8 | * Manhattan \[2d\] - mid length synthetic grid world dataset 9 | * rim \[3d\] (broken) - longer term real-world 3d dataset, like cubicle is largely planar 10 | * sphere_a \[3d\] - Larger synthetic 3d dataset 11 | * sphere2500 \[3d\] - Larger synthetic 3d dataset, different (not sure how) from sphere_a 12 | * manhattan_noisy \[2d\] - added noise on Manhattan dataset 13 | 14 | Each subdirectory contains a readme with source from which from which the datasets were taken. These may not be the original source of the datasets. 15 | 16 | Note: Cubicle and RIM are broken from an incremental perspective. Both pose graphs are missing at least one odometry link this means that when running incrementally there are unconstrained variables for at least one step. They can still be optimized in batch because loop-closures ensure that all variables are constrained, but are useless when converting to an irl file and running incrementally. -------------------------------------------------------------------------------- /experiments/datasets/city10000/README.md: -------------------------------------------------------------------------------- 1 | LOOPS: 10688 2 | 3 | This dataset is courtesy of Michael Kaess and was published along with iSAM2: 4 | 5 | M. Kaess, H. Johannsson, R. Roberts, V. Ila, J.J. Leonard, and F. Dellaert 6 | "iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree", 7 | International Journal of Robotics Research, 8 | vol. 31, Feb. 2012, pp. 217-236. 9 | -------------------------------------------------------------------------------- /experiments/datasets/csail/README.md: -------------------------------------------------------------------------------- 1 | LOOPS: 128 2 | 3 | Source: https://lucacarlone.mit.edu/datasets/ 4 | 5 | 6 | Average Loop Closure Covariance 7 | [[0.01640127 0.00099003 0. ] 8 | [0.00099003 0.01011058 0. ] 9 | [0. 0. 0.0008421 ]] 10 | 11 | ~ DIAG[0.015, 0.01, 0.001] -------------------------------------------------------------------------------- /experiments/datasets/cubicle/README.md: -------------------------------------------------------------------------------- 1 | LOOPS: 8021 2 | 3 | Source: https://lucacarlone.mit.edu/datasets/ 4 | 5 | This dataset appears to be marginally broken. There is at least one odometry measurement that is missing (x14-x15) -------------------------------------------------------------------------------- /experiments/datasets/garage/README.md: -------------------------------------------------------------------------------- 1 | LOOPS: 4615 2 | 3 | origin: https://lucacarlone.mit.edu/datasets/ -------------------------------------------------------------------------------- /experiments/datasets/intel/README.md: -------------------------------------------------------------------------------- 1 | TOTAL LOOPS: 1340 2 | -------------------------------------------------------------------------------- /experiments/datasets/manhattan/README.md: -------------------------------------------------------------------------------- 1 | LOOPS: 2099 2 | 3 | -------------------------------------------------------------------------------- /experiments/datasets/manhattan_noisy/README.md: -------------------------------------------------------------------------------- 1 | # EGNC-PGO original dataset 2 | 3 | To reproduce the result on paper, run this code: 4 | `cd experiments` 5 | `../build/experiments/run-experiment -i datasets/manhattan_noisy/manhattan_noisy.irl -o ~//EGNC-PGO_test_environment/results/ -m risam -n 1 -d ` 6 | 7 | To plot, run this code: `./scripts/plot-traj -i datasets/manhattan_noisy/manhattan_noisy.irl -r ~//EGNC-PGO_test_environment/results/manhattan_noisy_risam_2023-10-10_13-50-46/ --legend` 8 | 9 | Plotting code is from original riSAM code. -------------------------------------------------------------------------------- /experiments/datasets/rim/README.md: -------------------------------------------------------------------------------- 1 | LOOPS: 14156 2 | 3 | Source: https://lucacarlone.mit.edu/datasets/ 4 | 5 | NOTE! Dataset appears to be marginally broken. There is at lease one odometry measurement missing (x33-x34) 6 | -------------------------------------------------------------------------------- /experiments/datasets/sphere2500/README.md: -------------------------------------------------------------------------------- 1 | LOOPS: 2 | 3 | -------------------------------------------------------------------------------- /experiments/datasets/sphere_a/README.md: -------------------------------------------------------------------------------- 1 | LOOPS: 6448 2 | 3 | Source: https://lucacarlone.mit.edu/datasets/ -------------------------------------------------------------------------------- /experiments/exp_runner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(exp_runner INTERFACE) 2 | target_link_libraries(exp_runner INTERFACE gtsam risam irl KimeraRPGO dcsam) 3 | target_include_directories(exp_runner INTERFACE "${CMAKE_CURRENT_SOURCE_DIR}/include") 4 | -------------------------------------------------------------------------------- /experiments/exp_runner/README.md: -------------------------------------------------------------------------------- 1 | # Experiment Runner 2 | 3 | This directory contains two things. First is the definition of a common "runner" interface to regularize how each method interacts with the main entry point `run-experiment`. This interface is contained in `Runner.h` + `Runner-inl.h`. Second, are the wrappers for each method to be compatible with the standard runner interface. These implementations can be found in `Runner.h` and `Runner-inl.h`. 4 | 5 | All runners are templated by the type of the robot pose (Pose3, Pose2, Point2, Point3). Therefore they have to exist in headers. To maintain separation of definitions and implementations we treat the `-inl` ("inline") files are the source code files. -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/DCRunner-inl.h: -------------------------------------------------------------------------------- 1 | /** @brief Implementation for DC solver 2 | * 3 | * 4 | * @author Dan McGann 5 | * @date Feb 2022 6 | */ 7 | #pragma once 8 | #include "dcsam/DCMixtureFactor.h" 9 | #include "dcsam/DiscretePriorFactor.h" 10 | #include "exp_runner/DCRunner.h" 11 | 12 | using gtsam::symbol_shorthand::D; 13 | 14 | namespace exp_runner { 15 | /***************************************************************************/ 16 | template 17 | void DCRunner::init() { 18 | gtsam::ISAM2Params params; 19 | params.setOptimizationParams(gtsam::ISAM2DoglegParams()); 20 | solver_ = boost::make_shared(params); 21 | } 22 | 23 | /***************************************************************************/ 24 | template 25 | void DCRunner::handlePrior(irl::Prior& prior) { 26 | // ASSUME INLIER PRIORS!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 27 | dcsam::HybridFactorGraph new_factors; 28 | gtsam::Values new_values; 29 | dcsam::DiscreteValues new_discrete_values; 30 | // Initialize the factor based on the selected mode 31 | new_factors.push_nonlinear(prior.asFactor(0)); 32 | new_values = prior.getGenerator(0)(current_estimate_.continuous); 33 | 34 | // Update the underlying solver 35 | solver_->update(new_factors, new_values); 36 | current_estimate_ = solver_->calculateEstimate(); 37 | current_mode_sequence_.push_back(0); 38 | } 39 | 40 | /***************************************************************************/ 41 | template 42 | void DCRunner::handleOdometry(irl::Odometry& odom) { 43 | // ASSUME INLIER ODOMETRY!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 44 | dcsam::HybridFactorGraph new_factors; 45 | gtsam::Values new_values; 46 | new_factors.push_nonlinear(odom.asFactor(0)); 47 | solver_->update(new_factors, odom.getGenerator(0)(current_estimate_.continuous)); 48 | current_mode_sequence_.push_back(0); 49 | // Update current estimate info 50 | current_estimate_ = solver_->calculateEstimate(); 51 | } 52 | 53 | /***************************************************************************/ 54 | template 55 | void DCRunner::handleLoop(irl::Loop& loop) { 56 | // Handle Single Hypothesis case 57 | if (loop.num_modes == 1) { 58 | dcsam::HybridFactorGraph new_factors; 59 | new_factors.push_nonlinear(loop.asFactor(0)); 60 | solver_->update(new_factors, loop.getGenerator(0)(current_estimate_.continuous)); 61 | current_mode_sequence_.push_back(0); 62 | } 63 | // Handle MultiHypothesis Case 64 | else { 65 | dcsam::HybridFactorGraph new_factors; 66 | gtsam::Values new_values; 67 | DiscreteValues new_discrete_values; 68 | gtsam::DiscreteKey discrete_key(D(discrete_var_counter_++), 2); 69 | gtsam::FastVector> generators; 70 | 71 | for (size_t i = 0; i < loop.num_modes; i++) { 72 | auto loop_measure = boost::dynamic_pointer_cast>(loop.measurements[i]); 73 | if (loop_measure) { 74 | // Add discrete variable for this mode 75 | new_discrete_values[discrete_key.first] = 0; // Assume inlier at first 76 | // Add the Loop factor for this mode 77 | 78 | std::vector> components; 79 | components.push_back( // Inlier Measurement 80 | gtsam::BetweenFactor(loop.pose_a_key, loop_measure->pose_b_key, loop_measure->rel_pose, 81 | gtsam::noiseModel::Gaussian::Covariance(loop_measure->covariance))); 82 | components.push_back( // Outlier Measurement 83 | gtsam::BetweenFactor(loop.pose_a_key, loop_measure->pose_b_key, loop_measure->rel_pose, 84 | gtsam::noiseModel::Isotropic::Sigma(loop_measure->covariance.rows(), 1.6e7))); 85 | 86 | new_factors.push_dc(dcsam::DCMixtureFactor>(components[0].keys(), discrete_key, 87 | components, false)); 88 | new_factors.push_discrete(dcsam::DiscretePriorFactor(discrete_key, {1 - 10e-7, 10e-7})); 89 | } 90 | generators.push_back(loop.getGenerator(i)); 91 | } 92 | // Determine linearization point 93 | new_values.insert(generators[0](current_estimate_.continuous)); 94 | 95 | // Preform update 96 | solver_->update(new_factors, new_values, new_discrete_values); 97 | 98 | mh_measures_infos_.push_back(std::make_pair(current_mode_sequence_.size(), discrete_key)); 99 | current_mode_sequence_.push_back(0); 100 | } 101 | 102 | // Update current estimate info 103 | current_estimate_ = solver_->calculateEstimate(); 104 | } 105 | 106 | /***************************************************************************/ 107 | template 108 | MethodEstimateResults DCRunner::getEstimate() { 109 | MethodEstimateResults result; 110 | // current_estimate_.discrete.print(); 111 | result.push_back(current_estimate_.continuous); 112 | return result; 113 | } 114 | 115 | /***************************************************************************/ 116 | template 117 | MethodModeSeqResults DCRunner::getModeSequence() { 118 | MethodModeSeqResults result; 119 | ModeSequence seq; 120 | for (auto& pair : mh_measures_infos_) { 121 | current_mode_sequence_[pair.first] = current_estimate_.discrete.at(pair.second.first); 122 | } 123 | seq.assign(current_mode_sequence_.begin(), current_mode_sequence_.end()); 124 | result.push_back(seq); 125 | return result; 126 | } 127 | 128 | /***************************************************************************/ 129 | template 130 | std::string DCRunner::getName() { 131 | return "dcsam"; 132 | } 133 | } // namespace exp_runner -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/DCRunner.h: -------------------------------------------------------------------------------- 1 | /** @brief Experiment Interface for Discrete Continuous SAM 2 | * 3 | * @author Dan McGann 4 | * @date Feb 2022 5 | */ 6 | #pragma once 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "dcsam/DCFactor.h" 15 | #include "dcsam/DCSAM.h" 16 | #include "exp_runner/Runner.h" 17 | 18 | /** 19 | * DCBetweenFactor 20 | * 21 | * Based heavily on 22 | * dcsam:SemanticBearingRangeFactor 23 | */ 24 | template 25 | class DCBetweenFactor : public dcsam::DCFactor { 26 | private: 27 | typedef DCBetweenFactor This; 28 | 29 | gtsam::BetweenFactor factor_; 30 | std::vector probs_; 31 | 32 | public: 33 | using Base = DCFactor; 34 | 35 | DCBetweenFactor() = default; 36 | 37 | DCBetweenFactor(const gtsam::Key& key1, const gtsam::Key& key2, const gtsam::DiscreteKey& discreteKey, 38 | const std::vector measuredProbs, const VALUE_TYPE& measured, 39 | const gtsam::SharedNoiseModel& model) 40 | : probs_(measuredProbs), factor_(key1, key2, measured, model) { 41 | gtsam::KeyVector keys{key1, key2}; 42 | gtsam::DiscreteKeys dks(discreteKey); 43 | keys_ = keys; 44 | discreteKeys_ = dks; 45 | } 46 | 47 | virtual ~DCBetweenFactor() = default; 48 | 49 | DCBetweenFactor& operator=(const DCBetweenFactor& rhs) { 50 | Base::operator=(rhs); 51 | this->factor_ = rhs.factor_; 52 | this->probs_ = rhs.probs_; 53 | this->keys_ = rhs.keys_; 54 | this->discreteKeys_ = rhs.discreteKeys_; 55 | return *this; 56 | } 57 | 58 | // Error is the sum of the continuous and discrete negative 59 | // log-likelihoods 60 | double error(const gtsam::Values& continuousVals, const DiscreteValues& discreteVals) const override { 61 | size_t assignment = discreteVals.at(discreteKeys_[0].first); 62 | double discrete_error = log(probs_[assignment]); 63 | 64 | // Subtraction because -log(p(A,B)) = -log p(A)p(B) = -log p(A) - log p(B) 65 | return factor_.error(continuousVals) - discrete_error; 66 | } 67 | 68 | // dim is the dimension of the underlying between factor 69 | size_t dim() const override { return factor_.dim(); } 70 | 71 | boost::shared_ptr linearize(const gtsam::Values& continuousVals, 72 | const DiscreteValues& discreteVals) const override { 73 | return factor_.linearize(continuousVals); 74 | } 75 | 76 | bool equals(const DCFactor& other, double tol = 1e-9) const override { 77 | // We attempt a dynamic cast from DCFactor to DCBetweenFactor. 78 | // If it fails, return false. 79 | if (!dynamic_cast(&other)) return false; 80 | 81 | // If the cast is successful, we'll properly construct a 82 | // DCBetweenFactor object from `other` 83 | const DCBetweenFactor& f(static_cast(other)); 84 | 85 | // compare the between factors 86 | if (!(factor_.equals(f.factor_, tol))) return false; 87 | 88 | // If everything above passes, and the keys_, discreteKeys_ and probs_ 89 | // variables are identical, return true. 90 | return (std::equal(keys_.begin(), keys_.end(), f.keys().begin()) && (discreteKeys_ == f.discreteKeys_) && 91 | (probs_ == f.probs_)); 92 | } 93 | 94 | double logNormalizingConstant(const gtsam::Values& values) const override { 95 | return nonlinearFactorLogNormalizingConstant(this->factor_, values); 96 | } 97 | }; 98 | 99 | namespace exp_runner { 100 | 101 | template 102 | class DCRunner : public Method { 103 | /** FIELDS **/ 104 | private: 105 | /// @brief the actual solver 106 | boost::shared_ptr solver_; 107 | /// @brief internal tracker for current estimate 108 | dcsam::DCValues current_estimate_; 109 | /// @brief the current assignment of modes 110 | ModeSequence current_mode_sequence_; 111 | /// @brief information about mh measurements 112 | std::vector> mh_measures_infos_; 113 | 114 | /** PARAMETERS **/ 115 | private: 116 | int discrete_var_counter_{0}; 117 | 118 | /** HELPERS **/ 119 | protected: 120 | /** INTERFACE IMPL **/ 121 | public: 122 | void set_mode(size_t mode) override{}; // Just to get virtual inheritance to work 123 | 124 | void init() override; 125 | void handlePrior(irl::Prior& prior) override; 126 | void handleOdometry(irl::Odometry& odom) override; 127 | void handleLoop(irl::Loop& loop) override; 128 | MethodEstimateResults getEstimate() override; 129 | MethodModeSeqResults getModeSequence() override; 130 | std::string getName() override; 131 | }; 132 | } // namespace exp_runner 133 | 134 | #include "exp_runner/DCRunner-inl.h" -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/Factory.h: -------------------------------------------------------------------------------- 1 | /** @brief Factor for Runner Methods 2 | * 3 | * @author Dan McGann 4 | * @date Jan 2022 5 | */ 6 | #pragma once 7 | #include 8 | 9 | #include 10 | 11 | #include "exp_runner/DCRunner.h" 12 | #include "exp_runner/GNCBatchRunner.h" 13 | #include "exp_runner/MEstRunner.h" 14 | #include "exp_runner/MaxMixRunner.h" 15 | #include "exp_runner/PCMRunner.h" 16 | #include "exp_runner/PseudoGtRunner.h" 17 | #include "exp_runner/RiSAM2Runner.h" 18 | #include "exp_runner/Runner.h" 19 | 20 | namespace exp_runner { 21 | 22 | /// @brief Factor for Pose2 methods 23 | template 24 | boost::shared_ptr> method_factory(std::string method_name) { 25 | if (method_name == "pseudo-gt") { 26 | return boost::make_shared>(); 27 | } else if (method_name == "gnc-batch") { 28 | return boost::make_shared>(); 29 | } else if (method_name == "pcm") { 30 | return boost::make_shared>(3.0, 3.0, "pcm"); 31 | } else if (method_name == "maxmix") { 32 | return boost::make_shared>(); 33 | } else if (method_name == "dcsam") { 34 | return boost::make_shared>(); 35 | } else if (method_name == "huber") { 36 | return boost::make_shared>(gtsam::noiseModel::mEstimator::Huber::Create(3), 37 | "huber"); 38 | } else if (method_name == "gm") { 39 | return boost::make_shared>(gtsam::noiseModel::mEstimator::GemanMcClure::Create(3), 40 | "gm"); 41 | } else if (method_name == "gauss") { 42 | return boost::make_shared>(gtsam::noiseModel::mEstimator::Null::Create(), 43 | "gauss"); 44 | } else if (method_name == "risam") { // riSAM Paper Experimental Params 45 | risam::DoglegLineSearchParams opt_params; 46 | opt_params.search_type = risam::DoglegLineSearchType::OUTWARD; 47 | opt_params.init_delta = 1.0; 48 | opt_params.min_delta = 1.0; 49 | opt_params.max_delta = 50; 50 | risam::RISAM2::RISAM2Params riparams; 51 | riparams.converge_after_new_gnc = true; 52 | riparams.converge_mu = true; 53 | riparams.converge_values = false; 54 | riparams.increment_outlier_mu = true; 55 | riparams.optimization_params = opt_params; 56 | return boost::make_shared>(riparams, 3, method_name); 57 | } else { 58 | throw std::runtime_error("Unknown Method Name"); 59 | } 60 | } 61 | } // namespace exp_runner 62 | -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/GNCBatchRunner-inl.h: -------------------------------------------------------------------------------- 1 | /** @brief Implementation for GNC solver 2 | * 3 | * @author Dan McGann 4 | * @date Feb 2022 5 | */ 6 | #pragma once 7 | #include "exp_runner/GNCBatchRunner.h" 8 | 9 | namespace exp_runner { 10 | /***************************************************************************/ 11 | template 12 | void GNCBatchRunner::init() {} 13 | 14 | /***************************************************************************/ 15 | template 16 | void GNCBatchRunner::handleUpdate() { 17 | auto params = gtsam::GncParams(optimizer_params_); 18 | params.setKnownInliers(known_inliers_); 19 | gtsam::GncOptimizer> optimizer(graph_, initial_estimates_, params); 20 | current_estimate_ = optimizer.optimize(); 21 | initial_estimates_.update(current_estimate_); 22 | } 23 | 24 | /***************************************************************************/ 25 | template 26 | void GNCBatchRunner::handlePrior(irl::Prior& prior) { 27 | if (prior.num_modes == 1) { 28 | known_inliers_.push_back(graph_.nrFactors()); 29 | graph_.push_back(prior.asFactor(0)); 30 | initial_estimates_.insert(prior.getGenerator(0)(initial_estimates_)); 31 | current_mode_sequence_.push_back(0); 32 | handleUpdate(); 33 | } else { 34 | gtsam::NonlinearFactorGraph prior_factors; 35 | for (size_t i = 0; i < prior.num_modes; i++) { 36 | prior_factors.push_back(prior.asFactor(i)); 37 | initial_estimates_.insert(prior.getGenerator(i)(initial_estimates_)); 38 | } 39 | // Update the loop closure info 40 | mh_infos_.push_back(MHInfo(prior.correct_mode_idx, current_mode_sequence_.size(), prior_factors)); 41 | current_mode_sequence_.push_back(0); // Push back temp index this will get updated in the next call 42 | graph_.push_back(prior_factors); 43 | 44 | handleUpdate(); 45 | } 46 | } 47 | 48 | /***************************************************************************/ 49 | template 50 | void GNCBatchRunner::handleOdometry(irl::Odometry& odom) { 51 | if (odom.num_modes == 1) { 52 | known_inliers_.push_back(graph_.nrFactors()); 53 | graph_.push_back(odom.asFactor(0)); 54 | initial_estimates_.insert(odom.getGenerator(0)(initial_estimates_)); 55 | current_mode_sequence_.push_back(0); 56 | handleUpdate(); 57 | } else { 58 | gtsam::NonlinearFactorGraph odom_factors; 59 | for (size_t i = 0; i < odom.num_modes; i++) { 60 | auto factor = odom.asFactor(i); 61 | if (factor) { 62 | odom_factors.push_back(factor); 63 | initial_estimates_.insert(odom.getGenerator(i)(initial_estimates_)); 64 | } 65 | } 66 | // Update the loop closure info 67 | mh_infos_.push_back(MHInfo(odom.correct_mode_idx, current_mode_sequence_.size(), odom_factors)); 68 | current_mode_sequence_.push_back(0); // Push back temp index this will get updated in the next call 69 | graph_.push_back(odom_factors); 70 | handleUpdate(); 71 | } 72 | } 73 | 74 | /***************************************************************************/ 75 | template 76 | void GNCBatchRunner::handleLoop(irl::Loop& loop) { 77 | gtsam::NonlinearFactorGraph loop_factors; 78 | for (size_t i = 0; i < loop.num_modes; i++) { 79 | auto factor = loop.asFactor(i); 80 | if (factor) { 81 | loop_factors.push_back(factor); 82 | initial_estimates_.insert(loop.getGenerator(i)(initial_estimates_)); 83 | } 84 | } 85 | mh_infos_.push_back(MHInfo(loop.correct_mode_idx, current_mode_sequence_.size(), loop_factors)); 86 | current_mode_sequence_.push_back(0); // Push back temp index this will get updated in the next call 87 | graph_.push_back(loop_factors); 88 | handleUpdate(); 89 | } 90 | 91 | /***************************************************************************/ 92 | template 93 | void GNCBatchRunner::handleFinal() { 94 | auto params = gtsam::GncParams(optimizer_params_); 95 | params.setKnownInliers(known_inliers_); 96 | gtsam::GncOptimizer> optimizer(graph_, initial_estimates_, params); 97 | current_estimate_ = optimizer.optimize(); 98 | } 99 | 100 | /***************************************************************************/ 101 | template 102 | MethodEstimateResults GNCBatchRunner::getEstimate() { 103 | MethodEstimateResults result; 104 | result.push_back(current_estimate_); 105 | return result; 106 | } 107 | 108 | /***************************************************************************/ 109 | template 110 | MethodModeSeqResults GNCBatchRunner::getModeSequence() { 111 | MethodModeSeqResults result; 112 | ModeSequence seq; 113 | computeModeSequence(current_mode_sequence_, graph_, current_estimate_, mh_infos_, &chiSqInlierCheck); 114 | seq.assign(current_mode_sequence_.begin(), current_mode_sequence_.end()); 115 | result.push_back(seq); 116 | return result; 117 | } 118 | 119 | /***************************************************************************/ 120 | template 121 | std::string GNCBatchRunner::getName() { 122 | return "gnc-batch"; 123 | } 124 | } // namespace exp_runner -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/GNCBatchRunner.h: -------------------------------------------------------------------------------- 1 | /** @brief Experiment Interface for Graduated Non-Convexity 2 | * 3 | * @author Dan McGann 4 | * @date Feb 2022 5 | */ 6 | #pragma once 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "exp_runner/ModeDetermination.h" 16 | #include "exp_runner/Runner.h" 17 | 18 | namespace exp_runner { 19 | 20 | template 21 | class GNCBatchRunner : public Method { 22 | /** FIELDS **/ 23 | private: 24 | /// @brief iteration_counter 25 | size_t iter{0}; 26 | /// @brief The graph that we are optimizing 27 | gtsam::NonlinearFactorGraph graph_; 28 | /// @brief Initial Conditions used at each batch solve 29 | gtsam::Values initial_estimates_; 30 | 31 | /// @brief internal tracker for current estimate 32 | gtsam::Values current_estimate_; 33 | /// @brief the current assignment of modes 34 | ModeSequence current_mode_sequence_; 35 | /// @brief list of known inlier factors (i.e. odometry) that we should not convexify 36 | gtsam::GncParams::IndexVector known_inliers_; 37 | /// @brief info about each loop closure 38 | std::vector mh_infos_; 39 | 40 | /** PARAMETERS **/ 41 | private: 42 | /// @brief the parameters to use for Optimization 43 | gtsam::GaussNewtonParams optimizer_params_; 44 | 45 | /** HELPERS **/ 46 | protected: 47 | /** @brief Checks iteration count and runs optimizer if needed 48 | */ 49 | void handleUpdate(); 50 | 51 | /** INTERFACE IMPL **/ 52 | public: 53 | void set_mode(size_t mode) override{}; // Just to get virtual inheritance to work 54 | 55 | void init() override; 56 | void handlePrior(irl::Prior& prior) override; 57 | void handleOdometry(irl::Odometry& odom) override; 58 | void handleLoop(irl::Loop& loop) override; 59 | void handleFinal() override; 60 | MethodEstimateResults getEstimate() override; 61 | MethodModeSeqResults getModeSequence() override; 62 | std::string getName() override; 63 | }; 64 | } // namespace exp_runner 65 | 66 | #include "exp_runner/GNCBatchRunner-inl.h" -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/MEstRunner-inl.h: -------------------------------------------------------------------------------- 1 | /** @brief Implementation for MEstimator Runner 2 | * 3 | * @author Dan McGann 4 | * @date May 2022 5 | */ 6 | #pragma once 7 | 8 | #include "exp_runner/MEstRunner.h" 9 | namespace exp_runner { 10 | 11 | /***************************************************************************/ 12 | template 13 | void MEstRunner::init() { 14 | gtsam::ISAM2Params params; 15 | params.setOptimizationParams(gtsam::ISAM2DoglegParams(0.1, 1e-10, gtsam::DoglegOptimizerImpl::TrustRegionAdaptationMode::ONE_STEP_PER_ITERATION)); 16 | solver_ = boost::make_shared(params); 17 | } 18 | 19 | /***************************************************************************/ 20 | template 21 | void MEstRunner::handleFinal() { 22 | for (size_t i = 0; i < 20; i++) { 23 | solver_->update(); 24 | } 25 | current_estimate_ = solver_->calculateBestEstimate(); 26 | } 27 | 28 | /***************************************************************************/ 29 | template 30 | void MEstRunner::handlePrior(irl::Prior& prior) { 31 | gtsam::NonlinearFactorGraph new_factors; 32 | if (prior.num_modes == 1) { 33 | new_factors.push_back(prior.asFactor(0)); 34 | } else { 35 | for (size_t i = 0; i < prior.num_modes; i++) { 36 | auto factor = prior.asFactor(i); 37 | if (factor) { 38 | auto nm_factor = boost::dynamic_pointer_cast(factor); 39 | new_factors.push_back(nm_factor->cloneWithNewNoiseModel( 40 | gtsam::noiseModel::Robust::Create(robust_kernel_, nm_factor->noiseModel()))); 41 | } 42 | } 43 | // Update the loop closure info 44 | mh_infos_.push_back(MHInfo(prior.correct_mode_idx, current_mode_sequence_.size(), new_factors)); 45 | } 46 | 47 | gtsam::Values new_values = prior.getGenerator(0)(current_estimate_); 48 | solver_->update(new_factors, new_values); 49 | current_estimate_ = solver_->calculateEstimate(); 50 | current_mode_sequence_.push_back(0); 51 | } 52 | 53 | /***************************************************************************/ 54 | template 55 | void MEstRunner::handleOdometry(irl::Odometry& odom) { 56 | gtsam::NonlinearFactorGraph new_factors; 57 | if (odom.num_modes == 1) { 58 | new_factors.push_back(odom.asFactor(0)); 59 | } else { 60 | for (size_t i = 0; i < odom.num_modes; i++) { 61 | auto factor = odom.asFactor(i); 62 | if (factor) { 63 | auto nm_factor = boost::dynamic_pointer_cast(factor); 64 | new_factors.push_back(nm_factor->cloneWithNewNoiseModel( 65 | gtsam::noiseModel::Robust::Create(robust_kernel_, nm_factor->noiseModel()))); 66 | } 67 | } 68 | // Update the loop closure info 69 | mh_infos_.push_back(MHInfo(odom.correct_mode_idx, current_mode_sequence_.size(), new_factors)); 70 | } 71 | 72 | gtsam::Values new_values = odom.getGenerator(0)(current_estimate_); 73 | solver_->update(new_factors, new_values); 74 | current_estimate_ = solver_->calculateEstimate(); 75 | current_mode_sequence_.push_back(0); 76 | } 77 | 78 | /***************************************************************************/ 79 | template 80 | void MEstRunner::handleLoop(irl::Loop& loop) { 81 | gtsam::NonlinearFactorGraph new_factors; 82 | if (loop.num_modes == 1) { 83 | new_factors.push_back(loop.asFactor(0)); 84 | } else { 85 | for (size_t i = 0; i < loop.num_modes; i++) { 86 | auto factor = loop.asFactor(i); 87 | if (factor) { 88 | auto nm_factor = boost::dynamic_pointer_cast(factor); 89 | new_factors.push_back(nm_factor->cloneWithNewNoiseModel( 90 | gtsam::noiseModel::Robust::Create(robust_kernel_, nm_factor->noiseModel()))); 91 | } 92 | } 93 | // Update the loop closure info 94 | mh_infos_.push_back(MHInfo(loop.correct_mode_idx, current_mode_sequence_.size(), new_factors)); 95 | } 96 | 97 | gtsam::Values new_values = loop.getGenerator(0)(current_estimate_); 98 | solver_->update(new_factors, new_values); 99 | current_estimate_ = solver_->calculateEstimate(); 100 | current_mode_sequence_.push_back(0); 101 | } 102 | 103 | /***************************************************************************/ 104 | template 105 | MethodEstimateResults MEstRunner::getEstimate() { 106 | MethodEstimateResults result; 107 | result.push_back(current_estimate_); 108 | return result; 109 | } 110 | 111 | /***************************************************************************/ 112 | template 113 | MethodModeSeqResults MEstRunner::getModeSequence() { 114 | MethodModeSeqResults result; 115 | ModeSequence seq; 116 | computeModeSequence(current_mode_sequence_, solver_->getFactorsUnsafe(), current_estimate_, mh_infos_, 117 | &chiSqInlierCheck); 118 | seq.assign(current_mode_sequence_.begin(), current_mode_sequence_.end()); 119 | result.push_back(seq); 120 | return result; 121 | } 122 | 123 | /***************************************************************************/ 124 | template 125 | std::string MEstRunner::getName() { 126 | return kernel_name_; 127 | } 128 | 129 | } // namespace exp_runner 130 | -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/MEstRunner.h: -------------------------------------------------------------------------------- 1 | /** @brief Runner that adds all measurements with a robust estimator 2 | * 3 | * @author Dan McGann 4 | * @date January 2022 5 | */ 6 | #pragma once 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "exp_runner/Runner.h" 15 | 16 | namespace exp_runner { 17 | 18 | template 19 | class MEstRunner : public Method { 20 | /** FIELDS **/ 21 | private: 22 | /// @brief the solver 23 | boost::shared_ptr solver_; 24 | /// @brief internal tracker for current estimate 25 | gtsam::Values current_estimate_; 26 | /// @brief Current Mode Sequence 27 | ModeSequence current_mode_sequence_; 28 | /// @brief information about mh measurements 29 | std::vector mh_infos_; 30 | /// @brief The robust kernel to use 31 | gtsam::noiseModel::mEstimator::Base::shared_ptr robust_kernel_; 32 | std::string kernel_name_; 33 | 34 | /** INTERFACE IMPL **/ 35 | public: 36 | MEstRunner(gtsam::noiseModel::mEstimator::Base::shared_ptr robust_kernel, std::string kernel_name) 37 | : robust_kernel_(robust_kernel), kernel_name_(kernel_name) {} 38 | 39 | void set_mode(size_t mode) override{}; // Just to get virtual inheritance to work 40 | 41 | void init() override; 42 | void handlePrior(irl::Prior& prior) override; 43 | void handleOdometry(irl::Odometry& odom) override; 44 | void handleLoop(irl::Loop& loop) override; 45 | void handleFinal() override; 46 | MethodEstimateResults getEstimate() override; 47 | MethodModeSeqResults getModeSequence() override; 48 | std::string getName() override; 49 | }; 50 | } // namespace exp_runner 51 | 52 | #include "exp_runner/MEstRunner-inl.h" -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/MaxMixRunner.h: -------------------------------------------------------------------------------- 1 | /** @brief Experiment Interface for MaxMixture SLAM 2 | * 3 | * @author Dan McGann 4 | * @date January 2022 5 | */ 6 | #pragma once 7 | #include 8 | #include 9 | #include 10 | 11 | #include "exp_runner/Runner.h" 12 | 13 | namespace exp_runner { 14 | 15 | class MaxMixtureFactor : public gtsam::NonlinearFactor { 16 | /** Public Types **/ 17 | public: 18 | typedef boost::shared_ptr shared_ptr; 19 | typedef gtsam::FastVector MixtureModel; 20 | 21 | /** Protected Fields **/ 22 | protected: 23 | /// @brief The mixture model of Factors making up this MaxMixtureFactor. 24 | MixtureModel mixture_model_; 25 | /// @brief log(weight) assigned to each of the mixture components 26 | gtsam::FastVector log_weights_; 27 | /// @brief the Negative Log Normalization Constants for each Model @see computeNegLogNormalizationConstant 28 | gtsam::FastVector negative_log_normalizers_; 29 | 30 | public: 31 | /** @brief MaxMixtureFactor Constructor 32 | * @param keys: All keys referenced by this factor. The set-union of keys of all its components. 33 | * @param mixture_components: The factors describing the modes of this max mixture model. Mixture Components must be 34 | * NoiseModelFactors with Gaussian Noise Models. Additionally, all Mixture Components are expected to linearize to the 35 | * same number of rows. 36 | * @param weights: the importance weights assigned to each mode 37 | */ 38 | MaxMixtureFactor(const gtsam::KeyVector& keys, const MixtureModel& mixture_components, 39 | const gtsam::FastVector& weights); 40 | 41 | /* PUBLIC INTERFACE */ 42 | public: 43 | /** @brief Computes the negative log normalization constant of for the linearized (gaussian) component. 44 | * Computes: -\log\left(\frac{1}{(2\pi)^{d/2}|\Sigma_j|^{1/2}} \exp \left\{ -\frac{1}{2}||h(x_i) - 45 | * z_i||^2_{\Sigma_j} \right\} \right) 46 | * @param component: The mode for which to compute its constant. 47 | */ 48 | static double computeNegLogNormalizationConstant(const gtsam::NonlinearFactor::shared_ptr& component); 49 | 50 | /** @brief Computes the Index and normalized error for the component that is most probable given 51 | * the estimate. 52 | * @param current_estimate: The current estimate at which to evaluate component probabilities 53 | */ 54 | std::pair computeMaxIndexAndNormalizedError(const gtsam::Values& current_estimate) const; 55 | 56 | /// @brief Returns the number of modes in this factor 57 | size_t nrModes() const; 58 | 59 | /// @brief Returns the dimensionality of the factor (rows on linearization) 60 | size_t dim() const override; 61 | 62 | /** @brief Computes the normalized error for the factor 0.5 the squared mahalanobis distance 63 | * @param current_estimate: the variable estimate at which to compute the error 64 | */ 65 | double error(const gtsam::Values& current_estimate) const override; 66 | 67 | /** @brief Linearize this factor at a specific point 68 | * @param current_estimate: the variable estimate at which to linearize the Factor 69 | */ 70 | gtsam::GaussianFactor::shared_ptr linearize(const gtsam::Values& current_estimate) const override; 71 | 72 | /// @brief Creates a deep copy of this Factor 73 | gtsam::NonlinearFactor::shared_ptr clone() const override; 74 | 75 | /// @brief Prints to stdout information about this factor for params see gtsam print function interface 76 | void print(const std::string& s = "", const gtsam::KeyFormatter& kf = gtsam::DefaultKeyFormatter) const override; 77 | }; 78 | 79 | template 80 | class MaxMixRunner : public Method { 81 | /** FIELDS **/ 82 | private: 83 | /// @brief the actual solver 84 | boost::shared_ptr solver_; 85 | /// @brief internal tracker for current estimate 86 | gtsam::Values current_estimate_; 87 | /// @brief Current Mode Sequence 88 | ModeSequence current_mode_sequence_; 89 | /// @brief Flag to get the mode from the max mixture factor 90 | gtsam::FastVector get_mode_from_factor_; 91 | 92 | /** PARAMETERS **/ 93 | private: 94 | /// @brief Weight (0,1] for the null hypo mode. 95 | const double null_hypo_weight_{1e-7}; 96 | /// @brief Scale factor applied to measurement covariance for null hypothesis factors 97 | const double null_hypo_cov_scale_{1.6e7}; 98 | 99 | /** HELPERS **/ 100 | protected: 101 | /// @brief Internal Handler used for both Odometry and Loop Closures 102 | void handleBetweenPose(gtsam::Key start_key, gtsam::Key end_key, size_t num_modes, 103 | std::vector measurements, bool use_best_initialization); 104 | 105 | /** INTERFACE IMPL **/ 106 | public: 107 | void set_mode(size_t mode) override{}; // Just to get virtual inheritance to work 108 | 109 | void init() override; 110 | void handlePrior(irl::Prior& prior) override; 111 | void handleOdometry(irl::Odometry& odom) override; 112 | void handleLoop(irl::Loop& loop) override; 113 | MethodEstimateResults getEstimate() override; 114 | MethodModeSeqResults getModeSequence() override; 115 | std::string getName() override; 116 | }; 117 | } // namespace exp_runner 118 | 119 | #include "exp_runner/MaxMixRunner-inl.h" -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/ModeDetermination.h: -------------------------------------------------------------------------------- 1 | /** Helpers to figure out what modes are engaged and dis-engaged for different methods 2 | * 3 | * Author: Dan McGann 4 | * Date: April 2022 5 | */ 6 | #pragma once 7 | #include 8 | #include 9 | 10 | #include "exp_runner/Runner.h" 11 | 12 | namespace exp_runner { 13 | /// @brief Structure for storing information about one ambiguous loop closure event 14 | struct MHInfo { 15 | MHInfo(size_t correct_mode, size_t seq_idx, gtsam::NonlinearFactorGraph& fg) 16 | : correct_mode_idx(correct_mode), seq_idx(seq_idx), factors(fg) {} 17 | size_t correct_mode_idx; /// @brief the index of the correct mode 18 | size_t seq_idx; /// @brief The index in the Mode sequence that this loop closure corresponds to 19 | gtsam::NonlinearFactorGraph factors; /// @brief the factors assciated with this loop closure event 20 | }; 21 | 22 | /// @brief functor type for determining if a factor is in inlier 23 | typedef std::function InlierCheckFunc; 24 | 25 | /** Computes a mode sequence by looking at active factors and factors listed in a history associated with loop closures 26 | * @Mutates mode_seq 27 | */ 28 | void computeModeSequence(ModeSequence& mode_seq, const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, 29 | const std::vector& mh_info, const InlierCheckFunc& inlier_check_func) { 30 | // Compute all inlier factors 31 | gtsam::NonlinearFactorGraph inlier_factors; 32 | for (size_t i = 0; i < graph.size(); i++) { 33 | auto factor = graph[i]; 34 | if (factor && inlier_check_func(factor, values)) { 35 | inlier_factors.push_back(factor); 36 | } 37 | } 38 | 39 | // Update mode for all loop closure events 40 | for (auto& lci : mh_info) { 41 | // Accumulate all the modes from this event that are active 42 | std::vector indices_of_active_factors; 43 | for (int i = 0; i < lci.factors.size(); i++) { 44 | // If the mode factor is found accumulate its index 45 | if (std::find_if(inlier_factors.begin(), inlier_factors.end(), 46 | [&lci, &i](const gtsam::NonlinearFactor::shared_ptr& a) { 47 | return lci.factors[i]->equals(*a); 48 | }) != inlier_factors.end()) { 49 | indices_of_active_factors.push_back(i); 50 | } 51 | } 52 | 53 | // Determine if the result is any of the special cases 54 | int mode_selection; 55 | 56 | // SPECIAL CASE: Correct mode is null hypothesis (nh is always last mode) 57 | if (indices_of_active_factors.size() == 0) { 58 | mode_selection = lci.factors.size(); 59 | } 60 | // SPECIAL CASE: More than one mode active 61 | else if (indices_of_active_factors.size() > 1) { 62 | // CASE 1 multiple active modes and correct mode is one of them (-1) 63 | if (std::find(indices_of_active_factors.begin(), indices_of_active_factors.end(), lci.correct_mode_idx) != 64 | indices_of_active_factors.end()) { 65 | mode_selection = -1; 66 | } 67 | // CASE 2 multiple active modes and correct mode is NOT one of them (-2) 68 | else { 69 | mode_selection = -2; 70 | } 71 | } 72 | // standard: one active mode 73 | else { 74 | mode_selection = indices_of_active_factors.front(); 75 | } 76 | 77 | // Finally update the mode sequence for this event 78 | mode_seq[lci.seq_idx] = mode_selection; 79 | } 80 | } 81 | 82 | /** Common Inlier Check Functions **/ 83 | bool chiSqInlierCheck(const gtsam::NonlinearFactor::shared_ptr& factor, const gtsam::Values& values) { 84 | auto nmfactor = boost::dynamic_pointer_cast(factor); 85 | auto err = sqrt(nmfactor->noiseModel()->squaredMahalanobisDistance(nmfactor->unwhitenedError(values))); // Mahalanobis Distance 86 | //auto err = std::sqrt(2.0 * nmfactor->error(values)); 87 | double thresh = boost::math::quantile(boost::math::chi_squared_distribution(factor->dim()), 0.95); 88 | return err < thresh; 89 | } 90 | 91 | bool trueInlierCheck(const gtsam::NonlinearFactor::shared_ptr& factor, const gtsam::Values& values) { return true; } 92 | 93 | } // namespace exp_runner -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/PCMRunner-inl.h: -------------------------------------------------------------------------------- 1 | /** @brief Implementation for PCM solver 2 | * 3 | * @note ONLY WORKS FOR POSE GRAPH OPTIMIZATION 4 | * 5 | * @author Dan McGann 6 | * @date January 2022 7 | */ 8 | #pragma once 9 | #include "exp_runner/PCMRunner.h" 10 | 11 | namespace exp_runner { 12 | /***************************************************************************/ 13 | template 14 | void PCMRunner::init() { 15 | KimeraRPGO::RobustSolverParams params; 16 | if (std::is_same::value) { 17 | params.setPcm2DParams(odom_threshold_, lc_threshold_, KimeraRPGO::Verbosity::VERBOSE); 18 | // params.setIncremental(); 19 | } else { 20 | params.setPcm3DParams(odom_threshold_, lc_threshold_, KimeraRPGO::Verbosity::VERBOSE); 21 | // params.setIncremental(); 22 | } 23 | params.use_gnc_ = false; 24 | solver_ = boost::make_shared(params); 25 | } 26 | 27 | /***************************************************************************/ 28 | template 29 | void PCMRunner::handleUpdate() { 30 | solver_->update(new_factors_, new_values_); 31 | current_estimate_ = solver_->calculateEstimate(); 32 | new_factors_ = gtsam::NonlinearFactorGraph(); 33 | new_values_ = gtsam::Values(); 34 | } 35 | 36 | /***************************************************************************/ 37 | template 38 | void PCMRunner::handlePrior(irl::Prior& prior) { 39 | // ASSUME INLIER PRIOR !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 40 | new_factors_.push_back(prior.asFactor(0)); 41 | gtsam::Values merged(current_estimate_); 42 | merged.insert(new_values_); 43 | new_values_.insert(prior.getGenerator(0)(merged)); 44 | 45 | // Update the underlying solver 46 | current_mode_sequence_.push_back(0); 47 | handleUpdate(); 48 | } 49 | 50 | /***************************************************************************/ 51 | template 52 | void PCMRunner::handleOdometry(irl::Odometry& odom) { 53 | // ASSUME INLIER ODOM !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 54 | new_factors_.push_back(odom.asFactor(0)); 55 | gtsam::Values merged(current_estimate_); 56 | merged.insert(new_values_); 57 | new_values_.insert(odom.getGenerator(0)(merged)); 58 | 59 | // Update the underlying solver 60 | current_mode_sequence_.push_back(0); 61 | handleUpdate(); 62 | } 63 | 64 | /***************************************************************************/ 65 | template 66 | void PCMRunner::handleLoop(irl::Loop& loop) { 67 | gtsam::NonlinearFactorGraph loop_factors; 68 | // PCM cannot handle "Either-Or" ambiguity, so add all potential loop closures 69 | for (size_t i = 0; i < loop.num_modes; i++) { 70 | auto factor = loop.asFactor(i); 71 | if (factor) { 72 | loop_factors.push_back(factor); 73 | } 74 | } 75 | 76 | // Update the loop closure info 77 | mh_infos_.push_back(MHInfo(loop.correct_mode_idx, current_mode_sequence_.size(), loop_factors)); 78 | current_mode_sequence_.push_back(0); // Push back temp index this will get updated in the next call 79 | new_factors_.push_back(loop_factors); 80 | handleUpdate(); 81 | } 82 | 83 | /***************************************************************************/ 84 | template 85 | void PCMRunner::handleFinal() { 86 | solver_->update(new_factors_, new_values_); 87 | current_estimate_ = solver_->calculateEstimate(); 88 | } 89 | 90 | /***************************************************************************/ 91 | template 92 | MethodEstimateResults PCMRunner::getEstimate() { 93 | MethodEstimateResults result; 94 | result.push_back(current_estimate_); 95 | return result; 96 | } 97 | 98 | /***************************************************************************/ 99 | template 100 | MethodModeSeqResults PCMRunner::getModeSequence() { 101 | MethodModeSeqResults result; 102 | ModeSequence seq; 103 | auto inlier_factors = solver_->getFactorsUnsafe(); 104 | computeModeSequence(current_mode_sequence_, inlier_factors, current_estimate_, mh_infos_, &trueInlierCheck); 105 | seq.assign(current_mode_sequence_.begin(), current_mode_sequence_.end()); 106 | result.push_back(seq); 107 | return result; 108 | } 109 | 110 | /***************************************************************************/ 111 | template 112 | std::string PCMRunner::getName() { 113 | return name_; 114 | } 115 | } // namespace exp_runner -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/PCMRunner.h: -------------------------------------------------------------------------------- 1 | /** @brief Experiment Interface for Incremental Single Robot Pairwise Consistency Measure 2 | * Implementation provided by `thirdpart/Kimera-RPGO` 3 | * 4 | * @note ONLY WORKS FOR POSE GRAPH OPTIMIZATION 5 | * 6 | * @author Dan McGann 7 | * @date January 2022 8 | */ 9 | #pragma once 10 | #define SLOW_BUT_CORRECT_BETWEENFACTOR 11 | #include 12 | #include 13 | #include 14 | 15 | #include "KimeraRPGO/RobustSolver.h" 16 | #include "KimeraRPGO/SolverParams.h" 17 | #include "exp_runner/ModeDetermination.h" 18 | #include "exp_runner/Runner.h" 19 | 20 | namespace exp_runner { 21 | 22 | template 23 | class PCMRunner : public Method { 24 | /** FIELDS **/ 25 | private: 26 | /// @brief iteration_counter 27 | size_t iter{0}; 28 | /// @brief The factors to add to the solver on the next update iteration 29 | gtsam::NonlinearFactorGraph new_factors_; 30 | /// @brief The new values to add to the solver on the next update iteration 31 | gtsam::Values new_values_; 32 | /// @brief the actual solver 33 | boost::shared_ptr solver_; 34 | /// @brief internal tracker for current estimate 35 | gtsam::Values current_estimate_; 36 | /// @brief the current assignment of modes 37 | ModeSequence current_mode_sequence_; 38 | /// @brief tracks information about loop closures 39 | std::vector mh_infos_; 40 | 41 | /** PARAMETERS **/ 42 | private: 43 | std::string name_; 44 | /// @brief max allowable M distance deviation from odometry 45 | double odom_threshold_; 46 | /// @brief max allowable M distance deviation between pairs of measurements 47 | double lc_threshold_; 48 | 49 | /** HELPERS **/ 50 | protected: 51 | /** @brief Checks iteration count and runs optimizer if needed 52 | */ 53 | void handleUpdate(); 54 | 55 | /** INTERFACE IMPL **/ 56 | public: 57 | PCMRunner(double odom_threshold = 3.0, double loop_threshold = 3.0, std::string name = "pcm-o3-l3") 58 | : odom_threshold_(odom_threshold), lc_threshold_(loop_threshold), name_(name) {} 59 | 60 | void set_mode(size_t mode) override{}; // Just to get virtual inheritance to work 61 | 62 | void init() override; 63 | void handlePrior(irl::Prior& prior) override; 64 | void handleOdometry(irl::Odometry& odom) override; 65 | void handleLoop(irl::Loop& loop) override; 66 | void handleFinal() override; 67 | MethodEstimateResults getEstimate() override; 68 | MethodModeSeqResults getModeSequence() override; 69 | std::string getName() override; 70 | }; 71 | } // namespace exp_runner 72 | 73 | #include "exp_runner/PCMRunner-inl.h" -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/PseudoGtRunner-inl.h: -------------------------------------------------------------------------------- 1 | /** @brief Implementation for Baseline Experiment Runner 2 | * This runner always uses the correct mode, and is intendend to be treatede as a reference solution 3 | * 4 | * @author Dan McGann 5 | * @date January 2022 6 | */ 7 | #pragma once 8 | #include 9 | 10 | namespace exp_runner { 11 | 12 | /***************************************************************************/ 13 | template 14 | void PseudoGtRunner::init() { 15 | gtsam::ISAM2Params params; 16 | params.enableDetailedResults = true; 17 | params.relinearizeThreshold = 0.01; 18 | params.setOptimizationParams(gtsam::ISAM2DoglegParams()); 19 | solver_ = boost::make_shared(params); 20 | } 21 | 22 | /***************************************************************************/ 23 | template 24 | void PseudoGtRunner::handlePrior(irl::Prior& prior) { 25 | gtsam::NonlinearFactorGraph new_factors; 26 | gtsam::Values new_values; 27 | new_factors.push_back(prior.asFactor(prior.correct_mode_idx)); 28 | new_values = prior.getGenerator(prior.correct_mode_idx)(current_estimate_); 29 | auto result = solver_->update(new_factors, new_values); 30 | current_estimate_ = solver_->calculateEstimate(); 31 | current_mode_sequence_.push_back(prior.correct_mode_idx); 32 | } 33 | 34 | /***************************************************************************/ 35 | template 36 | void PseudoGtRunner::handleOdometry(irl::Odometry& odom) { 37 | gtsam::NonlinearFactorGraph new_factors; 38 | gtsam::Values new_values; 39 | new_factors.push_back(odom.asFactor(odom.correct_mode_idx)); 40 | new_values = odom.getGenerator(odom.correct_mode_idx)(current_estimate_); 41 | auto result = solver_->update(new_factors, new_values); 42 | current_estimate_ = solver_->calculateEstimate(); 43 | current_mode_sequence_.push_back(odom.correct_mode_idx); 44 | } 45 | 46 | /***************************************************************************/ 47 | template 48 | void PseudoGtRunner::handleLoop(irl::Loop& loop) { 49 | gtsam::NonlinearFactorGraph new_factors; 50 | gtsam::Values new_values; 51 | new_factors.push_back(loop.asFactor(loop.correct_mode_idx)); 52 | new_values = loop.getGenerator(loop.correct_mode_idx)(current_estimate_); 53 | auto result = solver_->update(new_factors, new_values); 54 | current_estimate_ = solver_->calculateEstimate(); 55 | current_mode_sequence_.push_back(loop.correct_mode_idx); 56 | } 57 | 58 | /***************************************************************************/ 59 | template 60 | MethodEstimateResults PseudoGtRunner::getEstimate() { 61 | MethodEstimateResults result; 62 | result.push_back(current_estimate_); 63 | return result; 64 | } 65 | 66 | /***************************************************************************/ 67 | template 68 | MethodModeSeqResults PseudoGtRunner::getModeSequence() { 69 | MethodModeSeqResults result; 70 | ModeSequence seq; 71 | seq.assign(current_mode_sequence_.begin(), current_mode_sequence_.end()); 72 | result.push_back(seq); 73 | return result; 74 | } 75 | 76 | /***************************************************************************/ 77 | template 78 | std::string PseudoGtRunner::getName() { 79 | return "pseudo-gt"; 80 | } 81 | 82 | } // namespace exp_runner 83 | -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/PseudoGtRunner.h: -------------------------------------------------------------------------------- 1 | /** @brief Experiment Interface for pseudo gt runner 2 | * 3 | * @author Dan McGann 4 | * @date January 2022 5 | */ 6 | #pragma once 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "exp_runner/Runner.h" 14 | 15 | namespace exp_runner { 16 | 17 | template 18 | class PseudoGtRunner : public Method { 19 | /** FIELDS **/ 20 | private: 21 | /// @brief the solver 22 | boost::shared_ptr solver_; 23 | /// @brief internal tracker for current estimate 24 | gtsam::Values current_estimate_; 25 | /// @brief Current Mode Sequence 26 | ModeSequence current_mode_sequence_; 27 | size_t count = 0; 28 | 29 | /** INTERFACE IMPL **/ 30 | public: 31 | 32 | void set_mode(size_t mode) override{}; // Just to get virtual inheritance to work 33 | 34 | void init() override; 35 | void handlePrior(irl::Prior& prior) override; 36 | void handleOdometry(irl::Odometry& odom) override; 37 | void handleLoop(irl::Loop& loop) override; 38 | MethodEstimateResults getEstimate() override; 39 | MethodModeSeqResults getModeSequence() override; 40 | std::string getName() override; 41 | }; 42 | } // namespace exp_runner 43 | 44 | #include "exp_runner/PseudoGtRunner-inl.h" -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/RiSAM2Runner-inl.h: -------------------------------------------------------------------------------- 1 | /** @brief Implementation for Baseline Experiment Runner 2 | * This runner always uses the correct mode, and is intendend to be treatede as a reference solution 3 | * 4 | * @author Dan McGann 5 | * @date January 2022 6 | */ 7 | #pragma once 8 | #include 9 | 10 | #include "risam/RISAM2.h" 11 | 12 | namespace exp_runner { 13 | 14 | /***************************************************************************/ 15 | template 16 | void RiSAM2Runner::set_mode(size_t mode) { 17 | mode_ = mode; 18 | solver_->set_mode(mode); 19 | } 20 | 21 | /***************************************************************************/ 22 | template 23 | void RiSAM2Runner::init() { 24 | solver_ = boost::make_shared(riparams_); 25 | } 26 | 27 | /***************************************************************************/ 28 | template 29 | void RiSAM2Runner::handleFinal() { 30 | for (size_t i = 0; i < 20; i++) { 31 | solver_->update(NonlinearFactorGraph(), Values(), false, ISAM2UpdateParams()); 32 | } 33 | current_estimate_ = solver_->calculateEstimate(); 34 | } 35 | 36 | /***************************************************************************/ 37 | template 38 | void RiSAM2Runner::handlePrior(irl::Prior& prior) { 39 | gtsam::NonlinearFactorGraph new_factors; 40 | gtsam::Values new_values; 41 | if (prior.num_modes == 1) { 42 | new_factors.push_back(prior.asFactor(0)); 43 | new_values.insert(prior.getGenerator(0)(current_estimate_)); 44 | current_mode_sequence_.push_back(0); 45 | auto result = solver_->update(new_factors, new_values, true); 46 | current_estimate_ = solver_->calculateEstimate(); 47 | } else { 48 | for (size_t i = 0; i < prior.num_modes; i++) { 49 | new_factors.push_back(prior.asGNCFactor(i, kernel_shape_param_)); 50 | } 51 | new_values.insert(prior.getGenerator(0)(current_estimate_)); // TODO (dan) better method? 52 | mh_infos_.push_back(MHInfo(prior.correct_mode_idx, current_mode_sequence_.size(), new_factors)); 53 | current_mode_sequence_.push_back(0); 54 | auto result = solver_->update(new_factors, new_values, false); 55 | current_estimate_ = solver_->calculateEstimate(); 56 | } 57 | } 58 | 59 | /***************************************************************************/ 60 | template 61 | void RiSAM2Runner::handleOdometry(irl::Odometry& odom) { 62 | gtsam::NonlinearFactorGraph new_factors; 63 | gtsam::Values new_values; 64 | 65 | if (odom.num_modes == 1) { 66 | new_factors.push_back(odom.asFactor(0)); 67 | new_values.insert(odom.getGenerator(0)(current_estimate_)); 68 | current_mode_sequence_.push_back(0); 69 | auto result = solver_->update(new_factors, new_values, true); 70 | current_estimate_ = solver_->calculateEstimate(); 71 | } else { 72 | for (size_t i = 0; i < odom.num_modes; i++) { 73 | auto factor = odom.asGNCFactor(i, kernel_shape_param_); 74 | if (factor) { 75 | new_factors.push_back(factor); 76 | } 77 | } 78 | new_values.insert(odom.getGenerator(0)(current_estimate_)); // TODO (dan) better method? 79 | mh_infos_.push_back(MHInfo(odom.correct_mode_idx, current_mode_sequence_.size(), new_factors)); 80 | current_mode_sequence_.push_back(0); 81 | auto result = solver_->update(new_factors, new_values, false); 82 | current_estimate_ = solver_->calculateEstimate(); 83 | } 84 | } 85 | 86 | /***************************************************************************/ 87 | template 88 | void RiSAM2Runner::handleLoop(irl::Loop& loop) { 89 | gtsam::NonlinearFactorGraph new_factors; 90 | gtsam::Values new_values; 91 | for (size_t i = 0; i < loop.num_modes; i++) { 92 | auto factor = loop.asGNCFactor(i, kernel_shape_param_); 93 | if (factor) { 94 | new_factors.push_back(factor); 95 | new_values.insert(loop.getGenerator(i)(current_estimate_)); 96 | } 97 | } 98 | mh_infos_.push_back(MHInfo(loop.correct_mode_idx, current_mode_sequence_.size(), new_factors)); 99 | current_mode_sequence_.push_back(0); // Push back temp index this will get updated in the next call 100 | auto result = solver_->update(new_factors, new_values, false); 101 | current_estimate_ = solver_->calculateEstimate(); 102 | } 103 | 104 | /***************************************************************************/ 105 | template 106 | MethodEstimateResults RiSAM2Runner::getEstimate() { 107 | MethodEstimateResults result; 108 | result.push_back(current_estimate_); 109 | return result; 110 | } 111 | 112 | /***************************************************************************/ 113 | template 114 | MethodModeSeqResults RiSAM2Runner::getModeSequence() { 115 | MethodModeSeqResults result; 116 | computeModeSequence(current_mode_sequence_, solver_->getFactorsUnsafe(), current_estimate_, mh_infos_, 117 | &chiSqInlierCheck); 118 | result.push_back(current_mode_sequence_); 119 | return result; 120 | } 121 | 122 | /***************************************************************************/ 123 | template 124 | std::string RiSAM2Runner::getName() { 125 | return name_; 126 | } 127 | 128 | } // namespace exp_runner 129 | -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/RiSAM2Runner.h: -------------------------------------------------------------------------------- 1 | /** @brief Experiment Interface for pseudo gt runner 2 | * 3 | * @author Dan McGann 4 | * @date January 2022 5 | */ 6 | #pragma once 7 | #include "exp_runner/ModeDetermination.h" 8 | #include "exp_runner/PseudoGtRunner.h" 9 | #include "exp_runner/Runner.h" 10 | #include "risam/RISAM2.h" 11 | 12 | namespace exp_runner { 13 | 14 | template 15 | class RiSAM2Runner : public Method { 16 | /** FIELDS **/ 17 | private: 18 | /// @brief The Parameters to use for RISAM 19 | risam::RISAM2::RISAM2Params riparams_; 20 | /// @brief The shape parameter used for SIG Kernels 21 | double kernel_shape_param_; 22 | /// @brief The name of this method (may change for factory presets) 23 | std::string name_; 24 | /// @brief the solver 25 | boost::shared_ptr solver_; 26 | /// @brief internal tracker for current estimate 27 | gtsam::Values current_estimate_; 28 | /// @brief Current Mode Sequence 29 | ModeSequence current_mode_sequence_; 30 | /// @brief information about mh measurements 31 | std::vector mh_infos_; 32 | 33 | /** INTERFACE IMPL **/ 34 | public: 35 | RiSAM2Runner(risam::RISAM2::RISAM2Params& params, double kernel_shape_param, std::string name = "risam") 36 | : riparams_(params), kernel_shape_param_(kernel_shape_param), name_(name) {} 37 | 38 | int mode_; // RISAM mode 39 | 40 | void set_mode(size_t mode) override; // set mode function 41 | 42 | void init() override; 43 | void handlePrior(irl::Prior& prior) override; 44 | void handleOdometry(irl::Odometry& odom) override; 45 | void handleLoop(irl::Loop& loop) override; 46 | void handleFinal() override; 47 | MethodEstimateResults getEstimate() override; 48 | MethodModeSeqResults getModeSequence() override; 49 | std::string getName() override; 50 | }; 51 | } // namespace exp_runner 52 | 53 | #include "exp_runner/RiSAM2Runner-inl.h" -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/Runner-inl.h: -------------------------------------------------------------------------------- 1 | /** @brief Implementation for Runner Functions 2 | * 3 | * @author Dan McGann 4 | * @date January 2022 5 | */ 6 | #pragma once 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "exp_runner/Runner.h" 14 | 15 | namespace bfs = boost::filesystem; 16 | 17 | // ######## config 18 | 19 | bool save_file = false; 20 | 21 | namespace exp_runner { 22 | 23 | /***************************************************************************/ 24 | template 25 | ExperimentRunner::ExperimentRunner(boost::shared_ptr> method, size_t save_every_n) 26 | : method_(method), save_every_n_(save_every_n) {} 27 | 28 | /***************************************************************************/ 29 | template 30 | void ExperimentRunner::run(irl::Log& log, std::string output_dir, size_t mode) { 31 | method_->init(); 32 | 33 | // set mode if the method is risam 34 | if (method_->getName() == "risam") { 35 | method_->set_mode(mode); 36 | } 37 | 38 | // Ensure that the output location exists 39 | bfs::create_directory(output_dir); 40 | // Generate the output dir 41 | auto t = std::time(nullptr); 42 | auto tm = *std::localtime(&t); 43 | std::stringstream time_ss; 44 | time_ss << std::put_time(&tm, "%Y-%m-%d_%H-%M-%S"); 45 | std::string result_dir = output_dir + "/" + log.header.name + "_" + method_->getName() + "_" + time_ss.str(); 46 | bfs::create_directory(result_dir); 47 | 48 | // Generate Interation output folder 49 | std::string interation_result_dir = result_dir + "/iterations"; 50 | bfs::create_directory(interation_result_dir); 51 | 52 | // Iterate through all entries and call appropriate method handler 53 | size_t iteration_index = 0; 54 | for (auto& entry : log.entries) { 55 | // Try cast to all entry types 56 | auto prior = boost::dynamic_pointer_cast>(entry); 57 | auto odom = boost::dynamic_pointer_cast>(entry); 58 | auto loop = boost::dynamic_pointer_cast>(entry); 59 | 60 | // Run handler for the proper entry type 61 | std::cout << std::endl << "ITER: " << iteration_index << std::endl; 62 | auto exe_time_start = std::chrono::high_resolution_clock::now(); 63 | if (prior) { 64 | method_->handlePrior(*prior); 65 | } else if (odom) { 66 | method_->handleOdometry(*odom); 67 | } else if (loop) { 68 | method_->handleLoop(*loop); 69 | } else { 70 | throw std::runtime_error("Experiment Runner: Encountered Entry of unknown type in IRL."); 71 | } 72 | auto exe_time_end = std::chrono::high_resolution_clock::now(); 73 | auto iter_ms = std::chrono::duration_cast(exe_time_end - exe_time_start).count() * 1e-6; 74 | 75 | // After we have executed the code, accumulate metric information 76 | if (save_file) { 77 | iteration_values_ = method_->getEstimate(); 78 | iteration_modes_ = method_->getModeSequence(); 79 | } 80 | iteration_times_ms_.push_back(iter_ms); 81 | total_execution_time_s_ += iter_ms / 1000.0; 82 | 83 | if (save_file) { 84 | if ((iteration_index % save_every_n_) == 0) { 85 | // Write iteration info "Values" format 86 | std::string prefix = (boost::format("%05d") % iteration_index).str(); 87 | auto values_file = std::make_shared(interation_result_dir + "/" + prefix + "_values.txt"); 88 | auto modes_file = std::make_shared(interation_result_dir + "/" + prefix + "_modes.txt"); 89 | for (size_t hypo_idx = 0; hypo_idx < iteration_values_.size(); hypo_idx++) { 90 | *values_file << serialize_values(iteration_values_[hypo_idx]) << std::endl; 91 | *modes_file << serialize_modes(iteration_modes_[hypo_idx]) << std::endl; 92 | } 93 | } 94 | } 95 | 96 | std::cout << "Total Execution Time (s): " << total_execution_time_s_ << std::endl; 97 | 98 | iteration_index++; 99 | } 100 | 101 | // Handle any final compute before writing final values 102 | method_->handleFinal(); 103 | iteration_values_ = method_->getEstimate(); 104 | iteration_modes_ = method_->getModeSequence(); 105 | 106 | // Write the iteration times 107 | auto iteration_times_file = std::make_shared(result_dir + "/iteration_times.txt"); 108 | for (auto t_ms : iteration_times_ms_) *iteration_times_file << t_ms << std::endl; 109 | *iteration_times_file << total_execution_time_s_ << std::endl; // add total time to end of file 110 | 111 | // Write out convience final values and modes 112 | auto final_values_file = std::make_shared(result_dir + "/" + "final_values.txt"); 113 | auto final_modes_file = std::make_shared(result_dir + "/" + "final_modes.txt"); 114 | for (size_t hypo_idx = 0; hypo_idx < iteration_values_.size(); hypo_idx++) { 115 | *final_values_file << serialize_values(iteration_values_[hypo_idx]) << std::endl; 116 | *final_modes_file << serialize_modes(iteration_modes_[hypo_idx]) << std::endl; 117 | } 118 | } 119 | 120 | /***************************************************************************/ 121 | template 122 | std::string ExperimentRunner::serialize_values(gtsam::Values& values) { 123 | bool first = false; 124 | std::stringstream ss; 125 | ss << std::setprecision(5) << std::fixed; 126 | 127 | for (auto kvp : values) { 128 | if (first) { 129 | first = false; 130 | } else { 131 | ss << " "; 132 | } 133 | 134 | auto symbol = gtsam::Symbol(kvp.key); 135 | if (symbol.chr() == 'x') { 136 | if (std::is_same::value) { 137 | auto pose = kvp.value.cast(); 138 | ss << "POSE2 " << symbol.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta(); 139 | } else if (std::is_same::value) { 140 | auto pose = kvp.value.cast(); 141 | ss << "POINT2 " << symbol.index() << " " << pose.x() << " " << pose.y(); 142 | } else if (std::is_same::value) { 143 | auto pose = kvp.value.cast(); 144 | ss << "POINT3 " << symbol.index() << " " << pose.x() << " " << pose.y() << " " << pose.z(); 145 | } else if (std::is_same::value) { 146 | auto pose = kvp.value.cast(); 147 | auto rxryrz = pose.rotation().xyz(); 148 | ss << "POSE3 " << symbol.index() << " " << pose.x() << " " << pose.y() << " " << pose.z() << " " << rxryrz[0] 149 | << " " << rxryrz[1] << " " << rxryrz[2]; 150 | } else { 151 | throw std::runtime_error("INVALID VALUE TYPE TO SERIALIZE"); 152 | } 153 | } else { 154 | std::cout << "Warning: Unknown Symbol found in Values, only poses 'X' allowed. Unknown value not serialized"; 155 | } 156 | } 157 | return ss.str(); 158 | } 159 | 160 | /***************************************************************************/ 161 | template 162 | std::string ExperimentRunner::serialize_modes(ModeSequence& modes) { 163 | bool first = false; 164 | std::stringstream ss; 165 | 166 | for (auto& mode : modes) { 167 | if (first) { 168 | first = false; 169 | } else { 170 | ss << " "; 171 | } 172 | 173 | ss << mode; 174 | } 175 | return ss.str(); 176 | } 177 | 178 | } // namespace exp_runner -------------------------------------------------------------------------------- /experiments/exp_runner/include/exp_runner/Runner.h: -------------------------------------------------------------------------------- 1 | /** @brief Provides interface used to run experiments as well as interface required by each methood. 2 | * 3 | * @author Dan McGann 4 | * @date January 2022 5 | */ 6 | #pragma once 7 | #include 8 | 9 | #include 10 | 11 | #include "irl/irl.h" 12 | 13 | namespace exp_runner { 14 | 15 | /** TYPES **/ 16 | typedef std::vector MethodEstimateResults; 17 | /** @brief indicates the mode selection for each entry 18 | * There are two special values 19 | * - (-1): Indicates that more than one mode from a single measurement 20 | * were selected and that the correct mode was one of them 21 | * - (-2): Indicates that more than one mode from a single measurement 22 | * were selected and that the correct mode was NOT one of them 23 | * 24 | * These special cases are required by the "add-all" prior works like PCM, or DCS 25 | */ 26 | typedef std::vector ModeSequence; 27 | typedef std::vector MethodModeSeqResults; 28 | 29 | /// @brief Interface that each method must provide to run the experiments 30 | template 31 | class Method { 32 | /** INTERFACE **/ 33 | public: 34 | /// @brief Sets the mode of the method (RISAM only) 35 | virtual void set_mode(size_t mode) {}; 36 | 37 | /// @brief Run initialization code, potentially using non-default parameters from file 38 | virtual void init() = 0; 39 | 40 | /// @brief Updates the method with a new Prior measurement 41 | virtual void handlePrior(irl::Prior& prior) = 0; 42 | 43 | /// @brief Updates the method with a new Odometry measurement 44 | virtual void handleOdometry(irl::Odometry& odom) = 0; 45 | 46 | /// @brief Updates the method with a new loop closure measurement 47 | virtual void handleLoop(irl::Loop& loop) = 0; 48 | 49 | /// @brief Finished up any compute for the methods 50 | virtual void handleFinal() {} // This is used primarily by batch runners to ensure final estimates are correct 51 | 52 | /// @brief returns the current estimate(s) from the method (if >1 ordered by hypothesis score) 53 | virtual MethodEstimateResults getEstimate() = 0; 54 | 55 | /// @brief returns the current mode sequence from the method (if >1 order by hypothesis score) 56 | virtual MethodModeSeqResults getModeSequence() = 0; 57 | 58 | /// @brief returns the name of the method 59 | virtual std::string getName() = 0; 60 | }; 61 | 62 | /// @brief Class used to run an experiment with a configured method 63 | template 64 | class ExperimentRunner { 65 | /** FIELDS **/ 66 | protected: 67 | /// @brief the method that this runner is executing 68 | boost::shared_ptr> method_; 69 | 70 | /// @brief The estimate provided by the method after each iteration (new measurement) 71 | MethodEstimateResults iteration_values_; 72 | /// @brief The interation time of the method required for each iteration (new measurement) 73 | std::vector iteration_times_ms_; 74 | /// @brief The modes tracked after each iteration 75 | MethodModeSeqResults iteration_modes_; 76 | /// @brief The total execution time for this method 77 | double total_execution_time_s_{0}; 78 | /// @brief Index increment at which to save intermediate results 79 | size_t save_every_n_{10}; 80 | 81 | /** INTERFACE **/ 82 | public: 83 | /// @brief Constructs a runner for the given method 84 | ExperimentRunner(boost::shared_ptr> method, size_t save_every_n); 85 | 86 | /// @brief Executes the method on the given log file accumulating metric data for each iteration 87 | void run(irl::Log& log, std::string output_dir, size_t mode = 0); 88 | 89 | private: 90 | std::string serialize_values(gtsam::Values& values); 91 | std::string serialize_modes(ModeSequence& modes); 92 | }; 93 | } // namespace exp_runner 94 | 95 | #include "exp_runner/Runner-inl.h" 96 | -------------------------------------------------------------------------------- /experiments/irl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(irl INTERFACE) 2 | target_link_libraries(irl INTERFACE gtsam) 3 | target_include_directories(irl INTERFACE "${CMAKE_CURRENT_SOURCE_DIR}/include") -------------------------------------------------------------------------------- /experiments/irl/README.md: -------------------------------------------------------------------------------- 1 | # Incremental Robot Logs (`.IRL`) 2 | Incremental Robot Log files define the data used in an experiment with file format definition provided here. 3 | Existing dataset formats (g2o) represent a batch problem and are not easy to work with in incremental settings. The Incremental Robot Log format makes the temporal sequence of measurements explicit and as such is a better format for incremental SLAM datasets. Additionally, IRL files allow for multi-hypothesis measurements (and in turn outlier measurements) while explicitly defining the correct hypothesis to aid evaluation. However, its multi-hypothesis capacity has not been widely used as riSAM targets primarily the robust SLAM problem. 4 | 5 | ### Header 6 | The file header provides meta data on the data contained, and consists of static number of elements defined as... 7 | - **Name** - the title or name of the dataset 8 | - **Date Generated** - The date the dataset was generated in `YYYY-MM-DD` format 9 | - **Pose Dimension** - The dimensionality for poses of the file. Either 2 or 3. 10 | - **Problem Linearity** - The linearity of the problem either "linear" or "nonlinear". Linear problems use POINT object (e.g. `gtsam::Point2`, `gtsam::Point3`) as robot poses while nonlinear problems uses POSE objects (e.g. `gtsam::Pose2`, `gtsam::Pose3`) for robot poses. 11 | - **User String** - User defined string of other relevant info on the dataset. 12 | 13 | ### Entries 14 | All lines beyond the header should consist of one of the following entries that define the actions/observations of the robot. All entries have the following format: 15 | ``` 16 | ... 17 | ``` 18 | Where: 19 | - `MODES [int]` 20 | - The number of modes for this entry. 21 | - There must be exactly MODES number of measurements listed 22 | - `CORRECT_MODE [int]` 23 | - The index of the mode that is ground truth correct. 24 | - `TAG_SPECIFIC [?]` 25 | - Space for tag specific information 26 | - `MEASUREMENT [?/NULL]` 27 | - The measurement for this entry specified depending on the Tag or `NULL` indicating the null Hypothesis. 28 | - `NULL` cannot be the only measurement for a entry 29 | 30 | The following Tags are defined. 31 | - `PRIOR` 32 | - Interpretation: Prior on the first pose 33 | - Tag Specific: `` 34 | - Measurement: ` ` 35 | - Pose of global dim and covariance which is a Matrix of Corresponding DIM. 36 | - NULL Hypothesis NOT allowed 37 | - `ODOMETRY` 38 | - Interpretation: Odometry Measurement from start pose to end pose. (Expect to be sequential with start pose == current pose) 39 | - Tag Specific: ` ` 40 | - Measurement: ` ` 41 | - Pose of global dim and covariance which is a Matrix of Corresponding DIM. 42 | - NULL Hypothesis NOT allowed 43 | - `LOOP` 44 | - Interpretation: Loop Closure detected between pose A and pose B 45 | - Tag Specific: `` 46 | - Measurement: ` ` 47 | - Pose (POINT or POSE object) of global dim and covariance which is a Matrix of Corresponding DIM. 48 | 49 | ### Null 50 | The null hypothesis indicates the possibility that none of the measurements for an entry are correct. 51 | 52 | ### Serialization of Data Types 53 | Within the entries there are a number of data types that are serialized as follows. Assume all values are floats unless otherwise specified, and all angles are provided in radians 54 | 55 | - Pose2 = `X Y THETA` 56 | - Pose3 = `X Y Z Rx Ry Rz` (`gtsam::Rot3::RzRyRx(Rx,Ry,Rz)`) 57 | - MATRIX = Row Major Order (Dim Inferred from Tag and Global Dim) 58 | - Point2 = `X Y` 59 | - Point3 = `X Y Z` 60 | 61 | ## Result Files 62 | IRL files provide input to the algorithm. Output generated by `run-experiment` is a directory containing... 63 | * `final_values.txt`: Values after the final iteration 64 | * `final_modes.txt`: Modes after the final iteration 65 | * `iteration_times.txt`: Each line is the runtime of the algorithm in milliseconds. 66 | * `iterations/` 67 | * `{:06d}_values.txt`: The values after intermediate iterations 68 | * `{:06d}_modes.txt`: The modes after intermediate iterations 69 | 70 | Value and Modes files have the following format. 71 | 72 | ### Values `.txt` File 73 | Contains pose values for a single step of a solution. Each value is formatted as... 74 | ``` 75 | 76 | ``` 77 | Where `NAME` is one of `{POSE2, POSE3, POINT2, POINT3}`, `IDX` is the variable identifier, and `VALUES` are the serialized datatype as described above. 78 | 79 | Each line in the file represents the values for a single hypothesis. Often there is only one line, but if there are multiple, it is the solution for each hypothesis. 80 | 81 | ### Modes `.txt` File 82 | Contains the mode selected by an algorithm for each entry in the IRL Log. File is space separated and modes are represented by integers. 83 | 84 | Each line in the file represents the modes for a single hypothesis. Often there is only one line, but if there are multiple, it is the modes for each hypothesis. 85 | 86 | ## MISC 87 | This directory houses the c++ (`./include/irl`) and python (`./python`) interfaces for parsing and writing Incremental Robot Log Files. 88 | 89 | An example for how to use the python interface can be found in `scripts/g2o-2-irl` 90 | 91 | An example for how to use the c++ interface can be found in `run-experiment.cpp` 92 | -------------------------------------------------------------------------------- /experiments/irl/include/irl/irl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Interface for using the Incremental Robot File Parsing functionality. 3 | * 4 | * @author Dan McGann 5 | * @date Jan 2022 6 | */ 7 | #pragma once 8 | #include 9 | 10 | #include "irl_types.h" 11 | 12 | namespace irl { 13 | namespace internal { 14 | 15 | /** @brief Parses header from an IRL file with specified global pose 16 | * @param text_file: ifstream completely unparsed 17 | */ 18 | Header parse_header(std::ifstream& text_file) { 19 | // Parse the header 20 | Header header; 21 | // Name 22 | std::getline(text_file, header.name); 23 | // Date 24 | std::getline(text_file, header.date); 25 | // Global Dim 26 | std::string dim_line; 27 | std::getline(text_file, dim_line); 28 | header.global_dim = std::stol(dim_line); 29 | // Linearity 30 | std::getline(text_file, header.linearity); 31 | // User notes 32 | std::getline(text_file, header.user_string); 33 | return header; 34 | } 35 | 36 | /** @brief Parses entries from an IRL file with specified global pose 37 | * @param text_file: ifstream with header already parsed 38 | */ 39 | template 40 | std::vector parse_entries(std::ifstream& text_file) { 41 | std::string current_line; 42 | std::vector entries; 43 | while (std::getline(text_file, current_line)) { 44 | std::deque components; 45 | boost::split(components, current_line, boost::is_any_of(" ")); 46 | std::string entry_type = parsing::next(components); 47 | if (entry_type == "PRIOR") { 48 | entries.push_back(boost::make_shared>(components)); 49 | } else if (entry_type == "ODOMETRY") { 50 | entries.push_back(boost::make_shared>(components)); 51 | } else if (entry_type == "LOOP") { 52 | entries.push_back(boost::make_shared>(components)); 53 | } else { 54 | throw std::runtime_error("Unknown entry type encountered: \"" + entry_type + "\""); 55 | } 56 | } 57 | return entries; 58 | } 59 | } // namespace internal 60 | 61 | /** @brief Parses an IRL file into a log 62 | * @param file_path: Path to the IRL text file 63 | */ 64 | Log parse_irl_file(std::string file_path) { 65 | std::ifstream text_file(file_path); 66 | 67 | try { 68 | // construct the log 69 | Log log; 70 | log.header = internal::parse_header(text_file); 71 | if (log.header.linearity == "nonlinear") { 72 | if (log.header.global_dim == 2) { 73 | log.entries = internal::parse_entries(text_file); 74 | } else if (log.header.global_dim == 3) { 75 | log.entries = internal::parse_entries(text_file); 76 | } else { 77 | throw std::runtime_error("IRL Parse Error: Invalid Global Dim = " + std::to_string(log.header.global_dim)); 78 | } 79 | } else if (log.header.linearity == "linear") { 80 | if (log.header.global_dim == 2) { 81 | log.entries = internal::parse_entries(text_file); 82 | } else if (log.header.global_dim == 3) { 83 | log.entries = internal::parse_entries(text_file); 84 | } else { 85 | throw std::runtime_error("IRL Parse Error: Invalid Global Dim = " + std::to_string(log.header.global_dim)); 86 | } 87 | } else { 88 | throw std::runtime_error("IRL Parse Error: Invalid Linearity = " + log.header.linearity); 89 | } 90 | return log; 91 | } catch (std::exception& e) { 92 | std::cout << "Parse IRL File encountered a " << e.what() << " exception. Rethrowing for details " << std::endl; 93 | throw; 94 | } 95 | } 96 | 97 | } // namespace irl 98 | -------------------------------------------------------------------------------- /experiments/irl/include/irl/irl_types.h: -------------------------------------------------------------------------------- 1 | /** irl_types.cpp 2 | * This file defines the data types found in a Incremental Log File. 3 | * 4 | * @author Dan McGann 5 | * @date Jan 2022 6 | */ 7 | #pragma once 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "risam/GraduatedFactor.h" 21 | 22 | using gtsam::symbol_shorthand::L; 23 | using gtsam::symbol_shorthand::X; 24 | 25 | namespace irl { 26 | 27 | /** 28 | * ## ## ######## ## ######## ######## ######## ###### 29 | * ## ## ## ## ## ## ## ## ## ## ## 30 | * ## ## ## ## ## ## ## ## ## ## 31 | * ######### ###### ## ######## ###### ######## ###### 32 | * ## ## ## ## ## ## ## ## ## 33 | * ## ## ## ## ## ## ## ## ## ## 34 | * ## ## ######## ######## ## ######## ## ## ###### 35 | */ 36 | /** @brief Internal namespace for templatized parsing of tokens from an std::deque 37 | * @note no template forms for sto{i/l/d} so we have to use template specialization here 38 | * this template specialization requires being defined at the namespace level so 39 | * we create a internal namespace here 40 | */ 41 | namespace parsing { 42 | /// @brief Parses the next token from the given entry components 43 | /// advancing the container and returning popped element as type T 44 | template 45 | T next(std::deque& c); 46 | } // namespace parsing 47 | 48 | /** @brief Parses a Pose of POSE_TYPE 49 | * @note Can handle gtsam::Pose2 and gtsam::Pose3 types 50 | * @param in: std::deque that must be advanced to start of Pose parameters 51 | */ 52 | template 53 | POSE_TYPE parse_pose(std::deque& in); 54 | 55 | /** @brief Parses a covariance matrix of specified size 56 | * @param dim: the dimensionality of the matrix to parse 57 | * @param in: std::deque that must be advanced to start of matrix coefficients 58 | */ 59 | gtsam::Matrix parse_cov_size(size_t dim, std::deque& in); 60 | 61 | /** @brief Covariance parsing specialized for different POSE TYPES 62 | * @note Can handle gtsam::Pose2 and gtsam::Pose3 types 63 | * @param in: std::deque that must be advanced to start of matrix coefficients 64 | */ 65 | template 66 | gtsam::Matrix parse_covariance(std::deque& in); 67 | 68 | /** 69 | * ## ## ######## ### ######## ######## ######## 70 | * ## ## ## ## ## ## ## ## ## ## 71 | * ## ## ## ## ## ## ## ## ## ## 72 | * ######### ###### ## ## ## ## ###### ######## 73 | * ## ## ## ######### ## ## ## ## ## 74 | * ## ## ## ## ## ## ## ## ## ## 75 | * ## ## ######## ## ## ######## ######## ## ## 76 | */ 77 | /// @brief container for header informaton in IRL files 78 | struct Header { 79 | std::string name; /// @brief the name of the dataset 80 | std::string date; /// @brief the date the dataset was generated 81 | size_t global_dim; /// @brief the global dim of the dataset (2 or 3) 82 | std::string linearity; /// @brief The linearity of the problem (linear or nonlinear) 83 | std::string user_string; /// @brief notes by the dataset creator 84 | }; 85 | 86 | /** 87 | * ## ## ######## ### ###### ## ## ######## ######## ## ## ######## ## ## ######## ###### 88 | * ### ### ## ## ## ## ## ## ## ## ## ## ### ### ## ### ## ## ## ## 89 | * #### #### ## ## ## ## ## ## ## ## ## #### #### ## #### ## ## ## 90 | * ## ### ## ###### ## ## ###### ## ## ######## ###### ## ### ## ###### ## ## ## ## ###### 91 | * ## ## ## ######### ## ## ## ## ## ## ## ## ## ## #### ## ## 92 | * ## ## ## ## ## ## ## ## ## ## ## ## ## ## ## ## ### ## ## ## 93 | * ## ## ######## ## ## ###### ####### ## ## ######## ## ## ######## ## ## ## ###### 94 | */ 95 | 96 | /// @brief Generic interface for a measurement found within entries 97 | struct Measurement { 98 | typedef boost::shared_ptr shared_ptr; 99 | /// @brief Returns true iff the measurement is a null hypothesis measurement 100 | virtual bool isNull() { return false; } 101 | }; 102 | 103 | /// @brief the Null Hypothesis measurement 104 | struct NullHypothesis : public Measurement { 105 | bool isNull() override { return true; } 106 | }; 107 | 108 | /// @brief a non-null pose measure (priors/odometry) 109 | template 110 | struct PoseMeasure : public Measurement { 111 | POSE_TYPE pose; 112 | gtsam::Matrix covariance; 113 | }; 114 | 115 | /// @brief a non-null between pose Measurement 116 | template 117 | struct LoopMeasure : public Measurement { 118 | gtsam::Key pose_b_key; 119 | POSE_TYPE rel_pose; 120 | gtsam::Matrix covariance; 121 | }; 122 | 123 | /** 124 | * ###### ######## ## ## ######## ######## ### ######## ####### ######## ###### 125 | * ## ## ## ### ## ## ## ## ## ## ## ## ## ## ## ## ## 126 | * ## ## #### ## ## ## ## ## ## ## ## ## ## ## ## 127 | * ## #### ###### ## ## ## ###### ######## ## ## ## ## ## ######## ###### 128 | * ## ## ## ## #### ## ## ## ######### ## ## ## ## ## ## 129 | * ## ## ## ## ### ## ## ## ## ## ## ## ## ## ## ## ## 130 | * ###### ######## ## ## ######## ## ## ## ## ## ####### ## ## ###### 131 | */ 132 | typedef std::function Generator; 133 | 134 | /// @brief Generator for Prior Measurements 135 | struct EmptyGenerator { 136 | EmptyGenerator(); 137 | gtsam::Values operator()(const gtsam::Values& vals); 138 | }; 139 | 140 | /// @brief Generator for Prior Measurements 141 | template 142 | struct PriorGen { 143 | gtsam::Key key; 144 | POSE_TYPE pose; 145 | 146 | PriorGen(gtsam::Key key, POSE_TYPE pose); 147 | gtsam::Values operator()(const gtsam::Values& vals); 148 | }; 149 | 150 | /// @brief Generator for Odometry and Loop Closure Measurements 151 | template 152 | struct BetweenPoseGen { 153 | gtsam::Key start_key; 154 | gtsam::Key end_key; 155 | POSE_TYPE rel_pose; 156 | 157 | BetweenPoseGen(gtsam::Key start_key, gtsam::Key end_key, POSE_TYPE rel_pose); 158 | gtsam::Values operator()(const gtsam::Values& vals); 159 | }; 160 | 161 | /** 162 | * ######## ## ## ######## ######## #### ######## ###### 163 | * ## ### ## ## ## ## ## ## ## ## 164 | * ## #### ## ## ## ## ## ## ## 165 | * ###### ## ## ## ## ######## ## ###### ###### 166 | * ## ## #### ## ## ## ## ## ## 167 | * ## ## ### ## ## ## ## ## ## ## 168 | * ######## ## ## ## ## ## #### ######## ###### 169 | */ 170 | 171 | /// @brief Base class for all entries 172 | struct Entry { 173 | typedef boost::shared_ptr shared_ptr; 174 | /// @brief The number of modes within this measurement (>0) 175 | size_t num_modes; 176 | /// @brief The index of the correct mode of this measurement 177 | size_t correct_mode_idx; 178 | /// @brief The measurements 179 | std::vector measurements; 180 | 181 | /** @brief returns the i'th mode as a GTSAM factor 182 | * @note pointer is null iff the mode is the null hypothesis 183 | */ 184 | virtual gtsam::NonlinearFactor::shared_ptr asFactor(size_t mode_idx) = 0; 185 | 186 | /// @brief Returns list of all keys involved in the entry 187 | virtual gtsam::KeyVector keys() = 0; 188 | 189 | /// @brief prints entry info 190 | virtual void print() = 0; 191 | }; 192 | 193 | /// @brief Prior Entry 194 | template 195 | struct Prior : public Entry { 196 | /** FIELDS **/ 197 | /// @brief the Pose that this prior is attached to 198 | gtsam::Key pose_key; 199 | 200 | /** INTERFACE **/ 201 | /** @brief Parses and Constructs a Prior Entry 202 | * @param std::deque advanced to start of Prior parameters 203 | */ 204 | Prior(std::deque& in); 205 | gtsam::NonlinearFactor::shared_ptr asFactor(size_t mode_idx); 206 | typename risam::GenericGraduatedFactor>::shared_ptr asGNCFactor( 207 | size_t mode_idx, double shape_param = 1.0); 208 | gtsam::KeyVector keys(); 209 | Generator getGenerator(size_t mode_idx); 210 | void print(); 211 | }; 212 | 213 | /// @brief Odometry Entry 214 | template 215 | struct Odometry : public Entry { 216 | /** FIELDS **/ 217 | /// @brief The start pose key for this odometry segment 218 | gtsam::Key start_pose_key; 219 | /// @brief The end pose key for this odometry segment 220 | gtsam::Key end_pose_key; 221 | 222 | /** INTERFACE **/ 223 | /** @brief Parses and Constructs a Odometry Entry 224 | * @param std::deque advanced to start of Odometry parameters 225 | */ 226 | Odometry(std::deque& in); 227 | gtsam::NonlinearFactor::shared_ptr asFactor(size_t mode_idx); 228 | typename risam::GenericGraduatedFactor>::shared_ptr asGNCFactor( 229 | size_t mode_idx, double shape_param = 1.0); 230 | gtsam::KeyVector keys(); 231 | Generator getGenerator(size_t mode_idx); 232 | void print(); 233 | }; 234 | 235 | /// @brief Loop Closure Entry 236 | template 237 | struct Loop : public Entry { 238 | /** FIELDS **/ 239 | /// @brief Pose A key for loop closure measurement ^aT_b 240 | gtsam::Key pose_a_key; 241 | 242 | /** INTERFACE **/ 243 | /** @brief Parses and Constructs a Loop Entry 244 | * @param std::deque advanced to start of Loop parameters 245 | */ 246 | Loop(std::deque& in); 247 | gtsam::NonlinearFactor::shared_ptr asFactor(size_t mode_idx); 248 | typename risam::GenericGraduatedFactor>::shared_ptr asGNCFactor( 249 | size_t mode_idx, double shape_param = 1.0); 250 | gtsam::KeyVector keys(); 251 | Generator getGenerator(size_t mode_idx); 252 | void print(); 253 | }; 254 | 255 | /** 256 | * ## ####### ###### 257 | * ## ## ## ## ## 258 | * ## ## ## ## 259 | * ## ## ## ## #### 260 | * ## ## ## ## ## 261 | * ## ## ## ## ## 262 | * ######## ####### ###### 263 | */ 264 | 265 | /// @brief contains the information from an entire IRL file 266 | struct Log { 267 | /// @brief dataset header information 268 | Header header; 269 | /// @brief temporally order list of events 270 | std::vector entries; 271 | }; 272 | 273 | } // namespace irl 274 | #include "irl/irl_types-inl.h" -------------------------------------------------------------------------------- /experiments/irl/python/irl_parsing.py: -------------------------------------------------------------------------------- 1 | import gtsam 2 | import numpy as np 3 | import irl_types 4 | from gtsam.symbol_shorthand import X, L 5 | 6 | """ 7 | ## ## ### ## ## ## ######## ###### 8 | ## ## ## ## ## ## ## ## ## ## 9 | ## ## ## ## ## ## ## ## ## 10 | ## ## ## ## ## ## ## ###### ###### 11 | ## ## ######### ## ## ## ## ## 12 | ## ## ## ## ## ## ## ## ## ## 13 | ### ## ## ######## ####### ######## ###### 14 | """ 15 | 16 | 17 | def parse_values_file(in_file): 18 | def parse_values_line(line): 19 | result = gtsam.Values() 20 | tokens = line.strip().split(" ") 21 | num_poses = 0 22 | 23 | i = 0 24 | while i < len(tokens): 25 | if tokens[i] == "POSE2": 26 | idx = int(tokens[i + 1]) 27 | x, y, theta = map(float, tokens[i + 2 : i + 5]) 28 | result.insert(X(idx), gtsam.Pose2(x, y, theta)) 29 | i += 5 30 | num_poses += 1 31 | elif tokens[i] == "POSE3": 32 | idx = int(tokens[i + 1]) 33 | x, y, z, rx, ry, rz = map(float, tokens[i + 2 : i + 8]) 34 | result.insert( 35 | X(idx), 36 | gtsam.Pose3(gtsam.Rot3.RzRyRx(rx, ry, rz), np.array([x, y, z])), 37 | ) 38 | i += 8 39 | num_poses += 1 40 | elif tokens[i] == "POINT2": 41 | idx = int(tokens[i + 1]) 42 | x, y = map(float, tokens[i + 2 : i + 4]) 43 | result.insert(X(idx), gtsam.Point2(x, y)) 44 | i += 4 45 | num_poses += 1 46 | elif tokens[i] == "POINT3": 47 | idx = int(tokens[i + 1]) 48 | x, y, z = map(float, tokens[i + 2 : i + 5]) 49 | result.insert(X(idx), gtsam.Point2(x, y, z)) 50 | i += 5 51 | num_poses += 1 52 | else: 53 | raise Exception("Unknown type found in values file") 54 | return (result, num_poses) 55 | 56 | result = [] 57 | with open(in_file) as f: 58 | for line in f.readlines(): 59 | result.append(parse_values_line(line)) 60 | return result 61 | 62 | 63 | def write_values_file(out_file, dim, lin, list_of_values): 64 | def write_values_line(f, values): 65 | if lin == "nonlinear": 66 | # Write Poses 67 | if dim == 3: 68 | poses = gtsam.utilities.allPose3s(values) 69 | for key in poses.keys(): 70 | pose = poses.atPose3(key) 71 | s = gtsam.Symbol(key) 72 | f.write( 73 | "POSE3 {} {} ".format( 74 | s.index(), irl_types.serialize_pose(dim, lin, pose) 75 | ) 76 | ) 77 | 78 | else: 79 | poses = gtsam.utilities.allPose2s(values) 80 | for key in poses.keys(): 81 | pose = poses.atPose2(key) 82 | s = gtsam.Symbol(key) 83 | f.write( 84 | "POSE2 {} {} ".format( 85 | s.index(), irl_types.serialize_pose(dim, lin, pose) 86 | ) 87 | ) 88 | elif lin == "linear": 89 | if dim == 3: 90 | points = gtsam.utilities.extractPoint3(values) 91 | for i in range(len(points)): 92 | f.write( 93 | "POINT3 {} {} ".format( 94 | i, irl_types.serialize_pose(dim, lin, pose) 95 | ) 96 | ) 97 | else: 98 | points = gtsam.utilities.extractPoint2(values) 99 | for i in range(len(points)): 100 | f.write( 101 | "POINT2 {} {} ".format( 102 | i, irl_types.serialize_pose(dim, lin, pose) 103 | ) 104 | ) 105 | 106 | with open(out_file, "w") as f: 107 | for vals in list_of_values: 108 | write_values_line(f, vals) 109 | f.write("\n") 110 | 111 | 112 | """ 113 | ## ## ####### ######## ######## ###### 114 | ### ### ## ## ## ## ## ## ## 115 | #### #### ## ## ## ## ## ## 116 | ## ### ## ## ## ## ## ###### ###### 117 | ## ## ## ## ## ## ## ## 118 | ## ## ## ## ## ## ## ## ## 119 | ## ## ####### ######## ######## ###### 120 | """ 121 | 122 | 123 | def parse_modes_file(in_file): 124 | result = [] 125 | with open(in_file) as f: 126 | for line in f.readlines(): 127 | result.append(list(map(int, line.strip().split(" ")))) 128 | return result 129 | 130 | 131 | def write_modes_file(out_file, list_of_modes): 132 | with open(out_file, "w") as f: 133 | for mode_seq in list_of_modes: 134 | f.write(" ".join(map(str, mode_seq))) 135 | f.write("\n") 136 | -------------------------------------------------------------------------------- /experiments/irl/python/irl_types.py: -------------------------------------------------------------------------------- 1 | import gtsam 2 | from collections import namedtuple 3 | from gtsam.symbol_shorthand import X, L 4 | import numpy as np 5 | 6 | """ 7 | ## ## ######## ### ###### ## ## ######## ######## ## ## ######## ## ## ######## ###### 8 | ### ### ## ## ## ## ## ## ## ## ## ## ### ### ## ### ## ## ## ## 9 | #### #### ## ## ## ## ## ## ## ## ## #### #### ## #### ## ## ## 10 | ## ### ## ###### ## ## ###### ## ## ######## ###### ## ### ## ###### ## ## ## ## ###### 11 | ## ## ## ######### ## ## ## ## ## ## ## ## ## ## #### ## ## 12 | ## ## ## ## ## ## ## ## ## ## ## ## ## ## ## ## ### ## ## ## 13 | ## ## ######## ## ## ###### ####### ## ## ######## ## ## ######## ## ## ## ###### 14 | """ 15 | 16 | 17 | def serialize_pose(dim, lin, pose): 18 | if lin == "nonlinear": 19 | if dim == 3: 20 | rx, ry, rz = pose.rotation().xyz() 21 | return "{:.5f} {:.5f} {:.5f} {:.5f} {:.5f} {:.5f}".format( 22 | pose.x(), pose.y(), pose.z(), rx, ry, rz 23 | ) 24 | else: 25 | return "{:.5f} {:.5f} {:.5f}".format(pose.x(), pose.y(), pose.theta()) 26 | else: 27 | if dim == 3: 28 | return "{:.5f} {:.5f} {:.5f}".format(pose[0], pose[1], pose[2]) 29 | else: 30 | return "{:.5f} {:.5f}".format(pose[0], pose[1]) 31 | 32 | 33 | def serialize_measurements(measurements): 34 | return " ".join(map(lambda m: m.serialize(), measurements)) 35 | 36 | 37 | def parse_measurements(dim, lin, num_hypos, tokens, MeasureClass): 38 | measurements = [] 39 | elems = MeasureClass.num_tokens(dim, lin) 40 | for i in range(num_hypos): 41 | if tokens[i * elems] == "NULL": 42 | measurements.append(NullHypo()) 43 | else: 44 | measurements.append( 45 | MeasureClass.parse(dim, lin, tokens[i * elems : (i + 1) * elems]) 46 | ) 47 | return measurements 48 | 49 | 50 | class NullHypo: 51 | def serialize(self): 52 | return "NULL" 53 | 54 | 55 | class PoseMeasure: 56 | def __init__(self, dim, lin, pose, covariance): 57 | self.dim = dim 58 | self.linearity = lin 59 | self.pose = pose 60 | self.covariance = covariance 61 | 62 | def serialize(self): 63 | return "{} {}".format( 64 | serialize_pose(self.dim, self.linearity, self.pose), 65 | " ".join(map(str, self.covariance.flatten())), 66 | ) 67 | 68 | @classmethod 69 | def parse(cls, dim, lin, tokens): 70 | if lin == "nonlinear": 71 | if dim == 3: 72 | x, y, z, rx, ry, rz = map(float, tokens[:6]) 73 | pose = gtsam.Pose3(gtsam.Rot3.RzRyRx(rx, ry, rz), np.array([x, y, z])) 74 | covariance = np.array(list(map(float, tokens[6:]))).reshape((6, 6)) 75 | else: 76 | pose = gtsam.Pose2(*map(float, tokens[:3])) 77 | covariance = np.array(list(map(float, tokens[3:]))).reshape((3, 3)) 78 | else: 79 | if dim == 3: 80 | x, y, z = map(float, tokens[:3]) 81 | pose = gtsam.Point3(np.array([x, y, z])) 82 | covariance = np.array(list(map(float, tokens[3:]))).reshape((3, 3)) 83 | else: 84 | pose = gtsam.Point2(*map(float, tokens[:2])) 85 | covariance = np.array(list(map(float, tokens[2:]))).reshape((2, 2)) 86 | return cls(dim, lin, pose, covariance) 87 | 88 | @classmethod 89 | def num_tokens(cls, dim, lin): 90 | if lin == "nonlinear": 91 | if dim == 3: 92 | return 6 + 6 * 6 93 | else: 94 | return 3 + 3 * 3 95 | else: 96 | if dim == 3: 97 | return 3 + 3 * 3 98 | else: 99 | return 2 + 2 * 2 100 | 101 | 102 | class LoopMeasure: 103 | def __init__(self, dim, lin, pose_b_key, pose, covariance): 104 | self.pose_b_key = pose_b_key 105 | self.dim = dim 106 | self.linearity = lin 107 | self.pose = pose 108 | self.covariance = covariance 109 | 110 | def serialize(self): 111 | symb = gtsam.Symbol(self.pose_b_key) 112 | return "{} {} {}".format( 113 | symb.index(), 114 | serialize_pose(self.dim, self.linearity, self.pose), 115 | " ".join(map(str, self.covariance.flatten())), 116 | ) 117 | 118 | @classmethod 119 | def parse(cls, dim, lin, tokens): 120 | k = X(int(tokens[0])) 121 | if lin == "nonlinear": 122 | if dim == 3: 123 | x, y, z, rx, ry, rz = map(float, tokens[1:7]) 124 | pose = gtsam.Pose3(gtsam.Rot3.RzRyRx(rx, ry, rz), np.array([x, y, z])) 125 | covariance = np.array(list(map(float, tokens[7:]))).reshape((6, 6)) 126 | else: 127 | pose = gtsam.Pose2(*map(float, tokens[1:4])) 128 | covariance = np.array(list(map(float, tokens[4:]))).reshape((3, 3)) 129 | else: 130 | if dim == 3: 131 | x, y, z = map(float, tokens[1:4]) 132 | pose = gtsam.Point3(np.array([x, y, z])) 133 | covariance = np.array(list(map(float, tokens[4:]))).reshape((3, 3)) 134 | else: 135 | pose = gtsam.Point2(*map(float, tokens[1:3])) 136 | covariance = np.array(list(map(float, tokens[3:]))).reshape((2, 2)) 137 | return cls(dim, lin, k, pose, covariance) 138 | 139 | @classmethod 140 | def num_tokens(cls, dim, lin): 141 | if lin == "nonlinear": 142 | if dim == 3: 143 | return 7 + 6 * 6 144 | else: 145 | return 4 + 3 * 3 146 | else: 147 | if dim == 3: 148 | return 4 + 3 * 3 149 | else: 150 | return 3 + 2 * 2 151 | 152 | 153 | """ 154 | ######## ## ## ######## ######## #### ######## ###### 155 | ## ### ## ## ## ## ## ## ## ## 156 | ## #### ## ## ## ## ## ## ## 157 | ###### ## ## ## ## ######## ## ###### ###### 158 | ## ## #### ## ## ## ## ## ## 159 | ## ## ### ## ## ## ## ## ## ## 160 | ######## ## ## ## ## ## #### ######## ###### 161 | """ 162 | 163 | 164 | class Prior: 165 | def __init__(self, num_modes, correct_mode, key, measurements): 166 | self.num_modes = num_modes 167 | self.correct_mode = correct_mode 168 | self.key = key 169 | self.measurements = measurements 170 | 171 | def serialize(self): 172 | symb = gtsam.Symbol(self.key) 173 | return "PRIOR {} {} {} {}".format( 174 | self.num_modes, 175 | self.correct_mode, 176 | symb.index(), 177 | serialize_measurements(self.measurements), 178 | ) 179 | 180 | @classmethod 181 | def parse(cls, dim, lin, tokens): 182 | return cls( 183 | int(tokens[0]), 184 | int(tokens[1]), 185 | X(int(tokens[2])), 186 | parse_measurements(dim, lin, int(tokens[0]), tokens[3:], PoseMeasure), 187 | ) 188 | 189 | 190 | class Odometry: 191 | def __init__(self, num_modes, correct_mode, start_key, end_key, measurements): 192 | self.num_modes = num_modes 193 | self.correct_mode = correct_mode 194 | self.start_key = start_key 195 | self.end_key = end_key 196 | self.measurements = measurements 197 | 198 | def serialize(self): 199 | s = gtsam.Symbol(self.start_key) 200 | e = gtsam.Symbol(self.end_key) 201 | return "ODOMETRY {} {} {} {} {}".format( 202 | self.num_modes, 203 | self.correct_mode, 204 | s.index(), 205 | e.index(), 206 | serialize_measurements(self.measurements), 207 | ) 208 | 209 | @classmethod 210 | def parse(cls, dim, lin, tokens): 211 | return cls( 212 | int(tokens[0]), 213 | int(tokens[1]), 214 | X(int(tokens[2])), 215 | X(int(tokens[3])), 216 | parse_measurements(dim, lin, int(tokens[0]), tokens[4:], PoseMeasure), 217 | ) 218 | 219 | 220 | class Loop: 221 | def __init__(self, num_modes, correct_mode, pose_a_key, measurements): 222 | self.num_modes = num_modes 223 | self.correct_mode = correct_mode 224 | self.pose_a_key = pose_a_key 225 | self.measurements = measurements 226 | 227 | def serialize(self): 228 | s = gtsam.Symbol(self.pose_a_key) 229 | return "LOOP {} {} {} {}".format( 230 | self.num_modes, 231 | self.correct_mode, 232 | s.index(), 233 | serialize_measurements(self.measurements), 234 | ) 235 | 236 | @classmethod 237 | def parse(cls, dim, lin, tokens): 238 | return cls( 239 | int(tokens[0]), 240 | int(tokens[1]), 241 | X(int(tokens[2])), 242 | parse_measurements(dim, lin, int(tokens[0]), tokens[3:], LoopMeasure), 243 | ) 244 | 245 | """ 246 | ######## ####### ######## ## ## ## ## 247 | ## ## ## ## ## ## ## ## ## 248 | ## ## ## ## ## ## ## ## ## 249 | ## ## ## ######## ## ## ## ## 250 | ## ## ## ## ## ## ## ## 251 | ## ## ## ## ## ## ## ## 252 | ## ####### ## ######## ### ######## 253 | """ 254 | 255 | 256 | class Header: 257 | def __init__(self, name, date, dim, lin, usr_str): 258 | self.name = name 259 | self.date = date 260 | self.dim = dim 261 | self.linearity = lin 262 | self.usr_str = usr_str 263 | 264 | def serialize(self): 265 | return "{}\n{}\n{}\n{}\n{}".format( 266 | self.name, self.date, self.dim, self.linearity, self.usr_str 267 | ) 268 | 269 | @classmethod 270 | def parse(cls, lines): 271 | return cls( 272 | lines[0].strip(), 273 | lines[1].strip(), 274 | int(lines[2].strip()), 275 | lines[3].strip(), 276 | lines[4].strip(), 277 | ) 278 | 279 | 280 | class Log: 281 | def __init__(self, header, entries): 282 | self.header = header 283 | self.entries = entries 284 | 285 | def write(self, out_file): 286 | with open(out_file, "w") as f: 287 | f.write(self.header.serialize() + "\n") 288 | for entry in self.entries: 289 | f.write(entry.serialize() + "\n") 290 | 291 | @classmethod 292 | def read(cls, in_file): 293 | with open(in_file) as f: 294 | lines = f.readlines() 295 | header = Header.parse(lines[0:5]) 296 | 297 | entries = [] 298 | for line in lines[5:]: 299 | tokens = line.strip().split(" ") 300 | if tokens[0] == "PRIOR": 301 | entries.append( 302 | Prior.parse(header.dim, header.linearity, tokens[1:]) 303 | ) 304 | elif tokens[0] == "ODOMETRY": 305 | entries.append( 306 | Odometry.parse(header.dim, header.linearity, tokens[1:]) 307 | ) 308 | elif tokens[0] == "LOOP": 309 | entries.append(Loop.parse(header.dim, header.linearity, tokens[1:])) 310 | else: 311 | raise Exception( 312 | "IRL Log.read found unknown token: {}".format(tokens[0]) 313 | ) 314 | return cls(header, entries) 315 | -------------------------------------------------------------------------------- /experiments/run-experiment.cpp: -------------------------------------------------------------------------------- 1 | /** @brief Entry Point for running MH-Inference experiments 2 | * @param cli_args: for cli arguments run program with --help or look at `handle_args` 3 | * 4 | * @author Dan McGann 5 | * @date January 2022 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include "exp_runner/Factory.h" 13 | #include "exp_runner/MaxMixRunner.h" 14 | #include "exp_runner/PseudoGtRunner.h" 15 | #include "exp_runner/Runner.h" 16 | #include "irl/irl.h" 17 | 18 | /** 19 | * ### ######## ###### ###### 20 | * ## ## ## ## ## ## ## ## 21 | * ## ## ## ## ## ## 22 | * ## ## ######## ## #### ###### 23 | * ######### ## ## ## ## ## 24 | * ## ## ## ## ## ## ## ## 25 | * ## ## ## ## ###### ###### 26 | */ 27 | 28 | namespace po = boost::program_options; 29 | po::variables_map handle_args(int argc, const char *argv[]) { 30 | // Define the options 31 | po::options_description options("Allowed options"); 32 | // clang-format off 33 | options.add_options() 34 | ("help,h", "Produce help message") 35 | ("irl_file,i", po::value()->required(), "(Required) Path to Incremental Robot Log file.") 36 | ("method,m", po::value()->required(), "(Required) The name of the method to run (e.g. pseudo-gt, see exp_runners/include/Factory.h for more options).") 37 | ("save_every_n,n",po::value()->required(), "(Required) Runner will save intermediate result files every N iterations.") 38 | ("output_dir,o", po::value()->required(), "(Required) Directory to which the results will be saved.") 39 | ("mode,d", po::value()->required(), "(Required) 0: original risam, 1: B-spline without adaptive, 2: our adaptive method"); 40 | // clang-format on 41 | 42 | // Parse and return the options 43 | po::variables_map var_map; 44 | po::store(po::parse_command_line(argc, argv, options), var_map); 45 | 46 | // Handle help special case 47 | if (var_map.count("help") || argc == 1) { 48 | std::cout << "run-experiment: Main entry point to run robust SLAM methods on IRL datasets. Please provide required " 49 | "arguments: " 50 | << std::endl; 51 | std::cout << options << "\n"; 52 | exit(1); 53 | } 54 | 55 | // Handle all other arguments 56 | po::notify(var_map); 57 | 58 | return var_map; 59 | } 60 | 61 | /** 62 | * ## ## ### #### ## ## 63 | * ### ### ## ## ## ### ## 64 | * #### #### ## ## ## #### ## 65 | * ## ### ## ## ## ## ## ## ## 66 | * ## ## ######### ## ## #### 67 | * ## ## ## ## ## ## ### 68 | * ## ## ## ## #### ## ## 69 | */ 70 | 71 | int main(int argc, const char *argv[]) { 72 | auto args = handle_args(argc, argv); 73 | irl::Log log = irl::parse_irl_file(args["irl_file"].as()); 74 | 75 | std::cout << "Running " << args["method"].as() << " on ..." << std::endl; 76 | std::cout << "Dataset" << std::endl; 77 | std::cout << "----------------------------------" << std::endl; 78 | std::cout << "Mode :" << args["mode"].as() << std::endl; 79 | std::cout << "----------------------------------" << std::endl; 80 | std::cout << log.header.name << std::endl; 81 | std::cout << log.header.date << std::endl; 82 | std::cout << "Pose Dim: " << log.header.global_dim << std::endl; 83 | std::cout << log.header.user_string << std::endl; 84 | std::cout << "----------------------------------" << std::endl; 85 | std::cout << "Status: Starting" << std::endl; 86 | if (log.header.linearity == "nonlinear") { 87 | if (log.header.global_dim == 2) { 88 | auto method = exp_runner::method_factory(args["method"].as()); 89 | exp_runner::ExperimentRunner runner(method, args["save_every_n"].as()); 90 | runner.run(log, args["output_dir"].as(), args["mode"].as()); 91 | 92 | } else { 93 | auto method = exp_runner::method_factory(args["method"].as()); 94 | exp_runner::ExperimentRunner runner(method, args["save_every_n"].as()); 95 | runner.run(log, args["output_dir"].as(), args["mode"].as()); 96 | } 97 | } else { 98 | if (log.header.global_dim == 2) { 99 | auto method = exp_runner::method_factory(args["method"].as()); 100 | exp_runner::ExperimentRunner runner(method, args["save_every_n"].as()); 101 | runner.run(log, args["output_dir"].as(), args["mode"].as()); 102 | 103 | } else { 104 | auto method = exp_runner::method_factory(args["method"].as()); 105 | exp_runner::ExperimentRunner runner(method, args["save_every_n"].as()); 106 | runner.run(log, args["output_dir"].as(), args["mode"].as()); 107 | } 108 | } 109 | std::cout << "Status: Done" << std::endl; 110 | 111 | return 0; 112 | } -------------------------------------------------------------------------------- /experiments/scripts/README.md: -------------------------------------------------------------------------------- 1 | # Scripts 2 | 3 | This directory contains a number of scripts for manipulating dataset formats, and evaluating results generated by `run-experiment`. 4 | 5 | Each script's use is self documented by running `script --help` or inspecting the the `handle_args` function in each scripts source file. 6 | 7 | In addition to scripts this directory contains two python modules `plot.py` and `comparisons.py` which implement functionality that is common between some scripts. -------------------------------------------------------------------------------- /experiments/scripts/animate-traj: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import argparse 3 | import glob 4 | import os 5 | import sys 6 | 7 | import gtsam 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | from gtsam.symbol_shorthand import L, X 11 | 12 | import comparisons, plot 13 | 14 | # OVERHEAD TO ACCESS IRL STUFF 15 | script_dir = os.path.dirname(__file__) 16 | irl_dir = os.path.join(script_dir, "..", "irl", "python") 17 | sys.path.append(irl_dir) 18 | import irl_parsing 19 | import irl_types 20 | 21 | """ 22 | ## ## ### #### ## ## 23 | ### ### ## ## ## ### ## 24 | #### #### ## ## ## #### ## 25 | ## ### ## ## ## ## ## ## ## 26 | ## ## ######### ## ## #### 27 | ## ## ## ## ## ## ### 28 | ## ## ## ## #### ## ## 29 | """ 30 | 31 | 32 | def handle_args(): 33 | parser = argparse.ArgumentParser() 34 | parser.add_argument( 35 | "-r", 36 | "--results_dir", 37 | type=str, 38 | required=True, 39 | help="Path to directory containing all values and modes files to animate. (e.g. the `interations` dir)", 40 | ) 41 | parser.add_argument( 42 | "-i", 43 | "--irl", 44 | type=str, 45 | required=True, 46 | help="Path to the IRL file from which the results are generated", 47 | ) 48 | parser.add_argument( 49 | "-s", 50 | "--start_idx", 51 | type=int, 52 | default=0, 53 | help="The file index to start the animation from", 54 | ) 55 | parser.add_argument( 56 | "-d", 57 | "--delay", 58 | type=float, 59 | default=0.1, 60 | help="The delay in seconds between each animated frame", 61 | ) 62 | parser.add_argument( 63 | "-o", 64 | "--output", 65 | type=str, 66 | default=None, 67 | help="If provided the program will save each animated frame into this directory.", 68 | ) 69 | parser.add_argument( 70 | "-k", 71 | "--wait_for_key", 72 | action="store_true", 73 | help="If provided the animation will wait for key input between each frame.", 74 | ) 75 | return parser.parse_args() 76 | 77 | 78 | def main(): 79 | args = handle_args() 80 | 81 | irl_log = irl_types.Log.read(args.irl) 82 | is3d = irl_log.header.dim == 3 83 | if is3d: 84 | fig = plt.figure() 85 | ax = plt.axes(projection="3d") 86 | else: 87 | figure = plt.figure(figsize=[6, 3]) 88 | ax = plt.gca() 89 | 90 | count = 0 91 | for val_file, mode_file in zip( 92 | sorted(glob.glob(os.path.join(args.results_dir, "*_values.txt")))[ 93 | args.start_idx : 94 | ], 95 | sorted(glob.glob(os.path.join(args.results_dir, "*_modes.txt")))[ 96 | args.start_idx : 97 | ], 98 | ): 99 | print(val_file) 100 | all_values = irl_parsing.parse_values_file(val_file) 101 | all_modes = irl_parsing.parse_modes_file(mode_file) 102 | p, r = comparisons.calc_precision_recall(all_modes[0], irl_log) 103 | print("iter:{} |Precision: {}, Recall: {}".format(count, p, r)) 104 | plot.plot_posegraph_results(ax, all_values[0][0], all_modes[0], irl_log, is3d) 105 | if not is3d: 106 | for i in range(1, len(all_modes)): 107 | plot.plot_2d_gt_traj(ax, all_values[i][0], alpha=0.4, color="grey") 108 | 109 | if is3d: 110 | plot.set_axes_equal(ax) 111 | plt.tight_layout() 112 | plt.pause(args.delay) 113 | if args.output is not None: 114 | plt.savefig(os.path.join(args.output, "{:06d}.png".format(count))) 115 | count += 1 116 | if args.wait_for_key: 117 | plt.waitforbuttonpress() 118 | ax.clear() 119 | plt.show() 120 | 121 | 122 | if __name__ == "__main__": 123 | main() 124 | -------------------------------------------------------------------------------- /experiments/scripts/comparisons.py: -------------------------------------------------------------------------------- 1 | """ 2 | This python module implements a number of methods to compute metrics on results from run-experiment. 3 | """ 4 | import os 5 | import sys 6 | from enum import Enum 7 | from unittest import result 8 | from enum import Enum 9 | import gtsam 10 | import numpy as np 11 | 12 | script_dir = os.path.dirname(__file__) 13 | irl_dir = os.path.join(script_dir, "..", "irl", "python") 14 | sys.path.append(irl_dir) 15 | import irl_parsing 16 | import irl_types 17 | 18 | 19 | class OutlierType(Enum): 20 | TRUE_POSITIVE = 1 21 | TRUE_NEGATIVE = 2 22 | FALSE_POSITIVE = 3 23 | FALSE_NEGATIVE = 4 24 | # identified correct mode as inlier but allowed other mutually exclusive modes 25 | PARTIAL_TRUE_POSITIVE = 5 26 | 27 | 28 | def calc_outlier_type(entry, chosen_mode): 29 | if entry.correct_mode == chosen_mode: # TRUE 30 | return ( 31 | OutlierType.TRUE_NEGATIVE 32 | if isinstance(entry.measurements[entry.correct_mode], irl_types.NullHypo) 33 | else OutlierType.TRUE_POSITIVE 34 | ) 35 | else: # FALSE 36 | return ( 37 | OutlierType.FALSE_NEGATIVE 38 | if isinstance(entry.measurements[chosen_mode], irl_types.NullHypo) 39 | else OutlierType.FALSE_POSITIVE 40 | ) 41 | 42 | 43 | def calc_precision_recall(modes, log): 44 | TP, TN, FP, FN = 0, 0, 0, 0 45 | 46 | contains_mh = False 47 | for mode, entry in zip(modes, log.entries): 48 | if entry.num_modes > 1: # only count MH measurements 49 | contains_mh = True 50 | mtype = calc_outlier_type(entry, mode) 51 | if mtype == OutlierType.TRUE_POSITIVE: 52 | TP += 1 53 | elif mtype == OutlierType.TRUE_NEGATIVE: 54 | TN += 1 55 | elif mtype == OutlierType.FALSE_POSITIVE: 56 | FP += 1 57 | elif mtype == OutlierType.FALSE_NEGATIVE: 58 | FN += 1 59 | 60 | if contains_mh: 61 | precision = TP / (TP + FP) if (TP + FP) > 0 else -1.0 62 | recall = TP / (TP + FN) if (TP + FN) > 0 else -1.0 63 | else: 64 | precision = 1.0 65 | recall = 1.0 66 | return precision, recall 67 | 68 | 69 | def umeyama_alignment(x: np.ndarray, y: np.ndarray): 70 | """ 71 | Computes the least squares transform that aligns point-set y to point-set x 72 | cite: 73 | [1] S. Umeyama, "Least-squares estimation of transformation parameters between two point patterns," 74 | in IEEE Transactions on Pattern Analysis and Machine Intelligence, 75 | vol. 13, no. 4, pp. 376-380, April 1991, doi: 10.1109/34.88573. 76 | 77 | x: Array of n points with dimension d (n,d) 78 | y: Array of n points with dimension d (n,d) 79 | Assumption: x[i] associates with y[i] 80 | """ 81 | num_pts, dim = x.shape 82 | # Find mean [1, Eq. 34] [1, Eq. 35] 83 | mu_x, mu_y = np.mean(x, axis=0), np.mean(y, axis=0) 84 | 85 | # Variance [1, Eq. 36] [1, Eq. 37] 86 | var_x = np.mean(np.linalg.norm(x - mu_x, axis=1)) 87 | var_y = np.mean(np.linalg.norm(y - mu_y, axis=1)) 88 | 89 | # Covariance [1, Eq. 38] 90 | cov = (1.0 / num_pts) * (y - mu_y).T @ (x - mu_x) 91 | 92 | # SVD 93 | U, _, Vt = np.linalg.svd(cov) 94 | 95 | # Matrix S [1, Eq. 43] 96 | S = np.eye(dim) 97 | if np.linalg.det(U) * np.linalg.det(Vt) < 0: 98 | S[-1, -1] = -1 99 | 100 | # Compute Transform 101 | R = U @ S @ Vt # [1, Eq. 40] 102 | t = mu_y - R @ mu_x # [1, Eq. 42] 103 | 104 | return R, t 105 | 106 | 107 | def align_trajectory( 108 | est_traj, 109 | ref_traj, 110 | ): 111 | """ 112 | Aligns the estimated trajectory to the reference trajectory with Uemyama alignment 113 | ref_traj: List-of-gtsam.Pose{2/3} the reference trajectory 114 | est_traj: List-of-gtsam.Pose{2/3} the estimated trajectory 115 | align_with_scale: If true preforms Umeyama Alignment with scale correction 116 | """ 117 | est_positions = [] 118 | ref_positions = [] 119 | for ep, rp in zip(est_traj, ref_traj): 120 | est_positions.append(ep.translation()) 121 | ref_positions.append(rp.translation()) 122 | est_positions = np.stack(est_positions) 123 | ref_positions = np.stack(ref_positions) 124 | R, t = umeyama_alignment(est_positions, ref_positions) 125 | 126 | if isinstance(est_traj[0], gtsam.Pose3): 127 | transform = gtsam.Pose3(gtsam.Rot3(R), t) 128 | else: 129 | transform = gtsam.Pose2(gtsam.Rot2(np.arccos(R[0, 0])), t) 130 | 131 | return [transform.compose(p) for p in est_traj] 132 | 133 | 134 | def calc_trajectory_metrics(ref_traj, est_traj): 135 | """ 136 | Compute the RMSE Absolute Trajectory Error and Relative Pose Error 137 | for both translation and rotation for detailed definitions see... 138 | https://rpg.ifi.uzh.ch/docs/IROS18_Zhang.pdf 139 | 140 | Params: 141 | ref_traj: List-of-gtsam.Pose{2/3} the reference trajectory 142 | est_traj: List-of-gtsam.Pose{2/3} the estimated trajectory 143 | 144 | We assume that each trajectory is already aligned and pose rej_traj[i] corresponds to pose est_traj[i] 145 | """ 146 | ate_pos, ate_rot = [], [] 147 | rpe_pos, rpe_rot = [], [] 148 | for i in range(len(ref_traj) - 1): 149 | # Compute Absolute Traj Error 150 | abs_error = ref_traj[i].inverse().compose(est_traj[i]) 151 | ate_pos.append(np.linalg.norm(abs_error.translation())) 152 | if isinstance(abs_error, gtsam.Pose2): 153 | ate_rot.append(abs_error.rotation().theta()) 154 | else: 155 | ate_rot.append(abs_error.rotation().axisAngle()[1]) 156 | 157 | # Compute Relative Pose Error 158 | de = est_traj[i + 1].inverse().compose(est_traj[i]) 159 | dr = ref_traj[i + 1].inverse().compose(ref_traj[i]) 160 | rel_error = de.inverse().compose(dr) 161 | rpe_pos.append(np.linalg.norm(rel_error.translation())) 162 | if isinstance(abs_error, gtsam.Pose2): 163 | rpe_rot.append(rel_error.rotation().theta()) 164 | else: 165 | rpe_rot.append(rel_error.rotation().axisAngle()[1]) 166 | 167 | # Convert to numpy 168 | ate_pos, ate_rot = np.array(ate_pos), np.array(ate_rot) 169 | rpe_pos, rpe_rot = np.array(rpe_pos), np.array(rpe_rot) 170 | 171 | # Generate Results (RMSE) 172 | results = {} 173 | N = ate_pos.shape[0] 174 | rmse = lambda x, n: np.sqrt((x ** 2).sum() / n) 175 | results["ate_pos_rmse"] = rmse(ate_pos, N) 176 | results["ate_rot_rmse"] = rmse(ate_rot, N) 177 | results["rpe_pos_rmse"] = rmse(rpe_pos, N - 1) 178 | results["rpe_rot_rmse"] = rmse(rpe_rot, N - 1) 179 | results["ate_pos"] = ate_pos 180 | results["ate_rot"] = ate_rot 181 | results["rpe_pos"] = rpe_pos 182 | results["rpe_rot"] = rpe_rot 183 | return results 184 | -------------------------------------------------------------------------------- /experiments/scripts/gtg2o-2-values: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import os 5 | import sys 6 | 7 | import gtsam 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | from gtsam.symbol_shorthand import X 11 | from gtsam.utils import plot 12 | 13 | # OVERHEAD TO ACCESS IRL STUFF 14 | script_dir = os.path.dirname(__file__) 15 | irl_dir = os.path.join(script_dir, "..", "irl", "python") 16 | sys.path.append(irl_dir) 17 | import irl_parsing 18 | 19 | 20 | def handle_args(): 21 | parser = argparse.ArgumentParser( 22 | description="Converts the vertex values in a g2o file into a values.txt format." 23 | ) 24 | parser.add_argument("-i", "--input", help="The g2o file.") 25 | parser.add_argument("-o", "--output", help="The name of the desired output file") 26 | parser.add_argument( 27 | "-p", 28 | "--plot", 29 | action="store_true", 30 | help="If provided the program will plot the values.", 31 | ) 32 | parser.add_argument( 33 | "--is3d", 34 | action="store_true", 35 | help="Flag to indicate that the g2o file contains a 3d pose-graph.", 36 | ) 37 | args = parser.parse_args() 38 | return args 39 | 40 | 41 | def main(): 42 | args = handle_args() 43 | graph, initial = gtsam.readG2o(args.input, args.is3d) 44 | 45 | irl_parsing.write_values_file( 46 | args.output, 3 if args.is3d else 2, "nonlinear", [initial] 47 | ) 48 | print("Done!") 49 | 50 | if args.plot: 51 | plot.plot_trajectory(0, initial, 0.1) 52 | plt.show() 53 | 54 | 55 | if __name__ == "__main__": 56 | main() 57 | -------------------------------------------------------------------------------- /experiments/scripts/make-gridworld-dataset: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import argparse 3 | import os 4 | import sys 5 | from datetime import date 6 | import gtsam 7 | import numpy as np 8 | from gtsam.symbol_shorthand import X 9 | from scipy.stats import chi2 10 | 11 | this_dir = os.path.dirname(__file__) 12 | irl_dir = os.path.join(this_dir, "..", "irl", "python") 13 | sys.path.append(irl_dir) 14 | import irl_parsing 15 | import irl_types 16 | 17 | 18 | def handle_args(): 19 | parser = argparse.ArgumentParser( 20 | description="Generates a random grid-world pose graph dataset in irl format. " 21 | + "Providing multiple noise models (translation and rotation are zipped together) " 22 | + "and multiple outlier probabilities will result in generating many datasets " 23 | + "from the cartesian product of noise models and outlier probabilities." 24 | ) 25 | parser.add_argument( 26 | "-o", "--output", type=str, required=True, help="Output Directory" 27 | ) 28 | parser.add_argument( 29 | "-ts", 30 | "--translation_sigma", 31 | type=float, 32 | default=[0.01], 33 | nargs="+", 34 | help="Sigma (m) for x, y in odometry and prior. Zipped with rotation sigma (lengths must match).", 35 | ) 36 | parser.add_argument( 37 | "-rs", 38 | "--rotation_sigma", 39 | type=float, 40 | default=[0.01], 41 | nargs="+", 42 | help="Sigma (degrees) for theta in odometry and prior. Zipped with translation sigma (lengths must match).", 43 | ) 44 | parser.add_argument( 45 | "-op", 46 | "--outlier_prob", 47 | type=float, 48 | default=[0.1], 49 | nargs="+", 50 | help="Probability of outlier measurement at any step without a true loop closure. Cartesian product taken with noise values.", 51 | ) 52 | 53 | parser.add_argument( 54 | "-nr", 55 | "--number_repeats_per_config", 56 | type=int, 57 | default=1, 58 | help="Number of repeats per config. Config=Cartesian Prod of variances and outlier_probs", 59 | ) 60 | parser.add_argument( 61 | "-np", 62 | "--number_poses", 63 | type=int, 64 | default=500, 65 | help="Number of poses in the Pose Graph", 66 | ) 67 | parser.add_argument( 68 | "--outlier_chi2_thresh", 69 | type=float, 70 | help="The chi2 threshold used to check that randomly generated measurements are in fact outliers", 71 | default=0.99, 72 | ) 73 | parser.add_argument( 74 | "--inlier_holdout_count", 75 | type=int, 76 | help="Number of poses required to pass before inliers are generated", 77 | default=0, 78 | ) 79 | return parser.parse_args() 80 | 81 | 82 | def gen_odom(): 83 | odom_options = [ 84 | gtsam.Pose2(1, 0, 0), # Move forward 85 | gtsam.Pose2(0, 0, np.pi / 2.0), # Turn left 86 | gtsam.Pose2(0, 0, -np.pi / 2.0), # Turn right 87 | ] 88 | return np.random.choice( 89 | odom_options, p=[0.7, 0.2, 0.1] 90 | ) # Bias towards moving and going left 91 | 92 | 93 | def get_close_pose_idx(poses, current_pose): 94 | cx, cy = current_pose.x(), current_pose.y() 95 | ci = len(poses) - 1 96 | for i, p in enumerate(poses): 97 | x, y = p.x(), p.y() 98 | if np.sqrt((x - cx) ** 2 + (y - cy) ** 2) < 2 and abs(i - ci) > 5: 99 | return i 100 | return None 101 | 102 | 103 | def generate_pose_graph(num_poses, op, ts, rs, chi2_thresh, inlier_holdout_count): 104 | def make_noise(): 105 | return gtsam.Pose2( 106 | np.random.normal(0, ts), 107 | np.random.normal(0, ts), 108 | np.random.normal(0, np.deg2rad(rs)), 109 | ) 110 | 111 | thresh = chi2.ppf(chi2_thresh, 3) # d=3 for 2d pose-graph 112 | covariance = np.diag([ts ** 2, ts ** 2, np.deg2rad(rs) ** 2]) 113 | gt_poses = [gtsam.Pose2(0, 0, 0)] 114 | entries = [ 115 | irl_types.Prior( 116 | 1, 117 | 0, 118 | X(0), 119 | [irl_types.PoseMeasure(2, "nonlinear", gtsam.Pose2(0, 0, 0), covariance)], 120 | ) 121 | ] 122 | for i in range(1, num_poses): 123 | odom = gen_odom() 124 | current_pose = gt_poses[-1].compose(odom) 125 | m = irl_types.PoseMeasure( 126 | 2, "nonlinear", odom.compose(make_noise()), covariance 127 | ) 128 | entries.append(irl_types.Odometry(1, 0, X(i - 1), X(i), [m])) 129 | gt_poses.append(current_pose) 130 | 131 | idx = get_close_pose_idx(gt_poses, current_pose) 132 | if idx is not None and i > inlier_holdout_count: 133 | delta = current_pose.inverse().compose(gt_poses[idx]).compose(make_noise()) 134 | m1 = irl_types.LoopMeasure(2, "nonlinear", X(idx), delta, covariance) 135 | entries.append(irl_types.Loop(2, 0, X(i), [m1, irl_types.NullHypo()])) 136 | elif np.random.rand() < op: 137 | idx = np.random.randint(0, len(gt_poses)) 138 | if i > 300: 139 | idx = i - 300 140 | else: 141 | continue 142 | sp, ep = gt_poses[-1], gt_poses[idx] 143 | error_vec = gtsam.Pose2().localCoordinates( 144 | sp.inverse().compose(ep) 145 | ) # Outlier measurement is always identity 146 | residual = np.sqrt(error_vec.T @ np.linalg.inv(covariance) @ error_vec) 147 | 148 | if residual > thresh: 149 | m1 = irl_types.LoopMeasure( 150 | 2, "nonlinear", X(idx), gtsam.Pose2(), covariance 151 | ) 152 | entries.append(irl_types.Loop(2, 1, X(i), [m1, irl_types.NullHypo()])) 153 | return entries 154 | 155 | 156 | def main(): 157 | args = handle_args() 158 | 159 | for ts, rs in zip(args.translation_sigma, args.rotation_sigma): 160 | for op in args.outlier_prob: 161 | for i in range(args.number_repeats_per_config): 162 | entries = generate_pose_graph( 163 | args.number_poses, 164 | op, 165 | ts, 166 | rs, 167 | args.outlier_chi2_thresh, 168 | args.inlier_holdout_count, 169 | ) 170 | output_name = "GridWorld_ts{}_rs{}_op{}_{}.irl".format(ts, rs, op, i) 171 | header = irl_types.Header( 172 | output_name.split(".irl")[0], 173 | date.today().strftime("%y-%m-%d"), 174 | 2, 175 | "nonlinear", 176 | "GridWorld Dataset with Translation Sigma: {}, Rotation Sigma: {}, Outlier Probability: {} (repeat: {})".format( 177 | ts, rs, op, i 178 | ), 179 | ) 180 | log = irl_types.Log(header, entries) 181 | log.write(os.path.join(args.output, output_name)) 182 | 183 | 184 | if __name__ == "__main__": 185 | main() 186 | -------------------------------------------------------------------------------- /experiments/scripts/multi-plot: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import argparse 3 | import os 4 | import sys 5 | 6 | import gtsam 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | from gtsam.symbol_shorthand import L, X 10 | import comparisons 11 | import plot 12 | 13 | # OVERHEAD TO ACCESS IRL STUFF 14 | script_dir = os.path.dirname(__file__) 15 | irl_dir = os.path.join(script_dir, "..", "irl", "python") 16 | sys.path.append(irl_dir) 17 | import irl_parsing 18 | import irl_types 19 | 20 | """ 21 | ## ## ### #### ## ## 22 | ### ### ## ## ## ### ## 23 | #### #### ## ## ## #### ## 24 | ## ### ## ## ## ## ## ## ## 25 | ## ## ######### ## ## #### 26 | ## ## ## ## ## ## ### 27 | ## ## ## ## #### ## ## 28 | """ 29 | 30 | 31 | def handle_args(): 32 | parser = argparse.ArgumentParser( 33 | description="Plots multiple results at once in the same figure." 34 | ) 35 | parser.add_argument( 36 | "-r", 37 | "--result_dirs", 38 | type=str, 39 | required=True, 40 | nargs="+", 41 | help="List of result dirs to plot.", 42 | ) 43 | parser.add_argument( 44 | "-i", 45 | "--irl_files", 46 | type=str, 47 | required=True, 48 | nargs="+", 49 | help="List of irl files corresponding to result dirs.", 50 | ) 51 | parser.add_argument( 52 | "-gt", 53 | "--ground_truth", 54 | type=str, 55 | help="Ground truth values file (used for all plots).", 56 | ) 57 | parser.add_argument( 58 | "-t", "--title", type=str, default="Results", help="The title for the figure." 59 | ) 60 | parser.add_argument( 61 | "--is3d", 62 | action="store_true", 63 | help="Flag to indicate that the dataset is 3d (used for all plots).", 64 | ) 65 | return parser.parse_args() 66 | 67 | 68 | def construct_grid(n: int, title: str): 69 | width = int(np.ceil(np.sqrt(n))) 70 | height = int(np.ceil(n / width)) 71 | fig, axes = plt.subplots(height, width) 72 | fig.suptitle(title) 73 | return fig, axes 74 | 75 | 76 | def main(): 77 | args = handle_args() 78 | if len(args.irl_files) == 1: 79 | irl_files = args.irl_files * len(args.result_dirs) 80 | else: 81 | irl_files = args.irl_files 82 | n = len(args.result_dirs) 83 | fig, axes = construct_grid(n, args.title) 84 | 85 | if args.ground_truth is not None: 86 | gt_values = irl_parsing.parse_values_file(args.ground_truth)[0][0] 87 | 88 | for ax, rdir, ifile in zip(axes.flat, sorted(args.result_dirs), sorted(irl_files)): 89 | irl_log = irl_types.Log.read(ifile) 90 | # print("-----\n", ifile, "\n", rdir) 91 | values_file = os.path.join(rdir, "final_values.txt") 92 | modes_file = os.path.join(rdir, "final_modes.txt") 93 | if not os.path.isfile(values_file): 94 | continue 95 | vcontent = irl_parsing.parse_values_file(values_file) 96 | mcontent = irl_parsing.parse_modes_file(modes_file) 97 | if len(vcontent) == 0: 98 | continue 99 | 100 | values = vcontent[0][0] 101 | modes = mcontent[0] 102 | 103 | p, r = comparisons.calc_precision_recall(modes, irl_log) 104 | 105 | if args.ground_truth is not None: 106 | plot.plot_2d_gt_traj(ax, gt_values) 107 | 108 | plot.plot_posegraph_results( 109 | ax, values, modes, irl_log, args.is3d, show_legend=False 110 | ) 111 | ax.set_title( 112 | os.path.basename(os.path.normpath(rdir)) 113 | + "\n Pre: {:.04f} Rec: {:.04f}".format(p, r) 114 | ) 115 | 116 | if not args.is3d: 117 | ax.set_aspect("equal") 118 | 119 | plt.tight_layout() 120 | plt.show() 121 | 122 | 123 | if __name__ == "__main__": 124 | main() 125 | -------------------------------------------------------------------------------- /experiments/scripts/plot-timing: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import argparse 3 | import os 4 | import sys 5 | 6 | import gtsam 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | from gtsam.symbol_shorthand import L, X 10 | import comparisons 11 | import plot 12 | 13 | # OVERHEAD TO ACCESS IRL STUFF 14 | script_dir = os.path.dirname(__file__) 15 | irl_dir = os.path.join(script_dir, "..", "irl", "python") 16 | sys.path.append(irl_dir) 17 | import irl_parsing 18 | import irl_types 19 | 20 | """ 21 | ## ## ### #### ## ## 22 | ### ### ## ## ## ### ## 23 | #### #### ## ## ## #### ## 24 | ## ### ## ## ## ## ## ## ## 25 | ## ## ######### ## ## #### 26 | ## ## ## ## ## ## ### 27 | ## ## ## ## #### ## ## 28 | """ 29 | 30 | 31 | def handle_args(): 32 | parser = argparse.ArgumentParser("Plots the iteration times for one or more methods.") 33 | parser.add_argument( 34 | "-i", 35 | "--inputs", 36 | type=str, 37 | required=True, 38 | nargs="+", 39 | help="List of iteration_times.txt files to plot", 40 | ) 41 | parser.add_argument( 42 | "-n", 43 | "--names", 44 | type=str, 45 | required=True, 46 | nargs="+", 47 | help="List of method names corresponding to inputs list", 48 | ) 49 | return parser.parse_args() 50 | 51 | 52 | def main(): 53 | args = handle_args() 54 | fig = plt.figure() 55 | for in_file, name in zip(args.inputs, args.names): 56 | with open(in_file) as f: 57 | times = np.array(list(map(float, f.readlines()))) * 1e-3 58 | smoothed_times = np.convolve(times, np.ones((10,)) / 10.0) 59 | l = plt.plot( 60 | np.arange(smoothed_times.shape[0]), smoothed_times, label=name 61 | )[0] 62 | plt.plot(np.arange(times.shape[0]), times, color=l.get_color(), alpha=0.3) 63 | ax = plt.gca() 64 | # ax.set_yscale("log") 65 | ax.set_xlabel("Iterations") 66 | ax.set_ylabel("Update Time (s)") 67 | plt.legend() 68 | plt.tight_layout() 69 | plt.show() 70 | 71 | 72 | if __name__ == "__main__": 73 | main() 74 | -------------------------------------------------------------------------------- /experiments/scripts/plot-traj: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import argparse 3 | import os 4 | import sys 5 | 6 | import gtsam 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | from gtsam.symbol_shorthand import L, X 10 | import comparisons 11 | import plot 12 | 13 | # OVERHEAD TO ACCESS IRL STUFF 14 | script_dir = os.path.dirname(__file__) 15 | irl_dir = os.path.join(script_dir, "..", "irl", "python") 16 | sys.path.append(irl_dir) 17 | import irl_parsing 18 | import irl_types 19 | 20 | """ 21 | ## ## ### #### ## ## 22 | ### ### ## ## ## ### ## 23 | #### #### ## ## ## #### ## 24 | ## ### ## ## ## ## ## ## ## 25 | ## ## ######### ## ## #### 26 | ## ## ## ## ## ## ### 27 | ## ## ## ## #### ## ## 28 | """ 29 | 30 | 31 | def handle_args(): 32 | parser = argparse.ArgumentParser( 33 | description="Plots results. If multiple results/irl files provided each is plotted in its own figure." 34 | ) 35 | parser.add_argument( 36 | "-r", 37 | "--result_dir", 38 | type=str, 39 | required=True, 40 | nargs="+", 41 | help="The result directory(ies) to plot.", 42 | ) 43 | parser.add_argument( 44 | "-i", 45 | "--irl", 46 | type=str, 47 | required=True, 48 | nargs="+", 49 | help="The irl file(s) corresponding to result_dir(s).", 50 | ) 51 | parser.add_argument( 52 | "-gt", 53 | "--ground_truth", 54 | type=str, 55 | nargs="+", 56 | help="Ground truth values file (used for all plots).", 57 | ) 58 | parser.add_argument( 59 | "-hl", 60 | "--hypo_limit", 61 | type=int, 62 | default=5, 63 | help="Max number of hypotheses to plot if method provides multiple hypotheses in results.", 64 | ) 65 | parser.add_argument( 66 | "--is3d", 67 | action="store_true", 68 | help="Flag to indicate that the dataset is 3d (used for all plots).", 69 | ) 70 | 71 | parser.add_argument( 72 | "--save", 73 | action="store_true", 74 | help="Flag to indicate to save the image in the working directory.", 75 | ) 76 | parser.add_argument( 77 | "--legend", 78 | action="store_true", 79 | help="Flag to indicate to include a legend in the image.", 80 | ) 81 | return parser.parse_args() 82 | 83 | 84 | def main(): 85 | args = handle_args() 86 | args.irl.sort() 87 | args.result_dir.sort() 88 | if args.ground_truth is not None: 89 | args.ground_truth.sort() 90 | for i, (rdir, irl_file) in enumerate(zip(args.result_dir, args.irl)): 91 | irl_log = irl_types.Log.read(irl_file) 92 | values_file_content = irl_parsing.parse_values_file( 93 | os.path.join(rdir, "final_values.txt") 94 | ) 95 | mode_file_content = irl_parsing.parse_modes_file( 96 | os.path.join(rdir, "final_modes.txt") 97 | ) 98 | if args.ground_truth is not None: 99 | gt_values = irl_parsing.parse_values_file( 100 | os.path.join(args.ground_truth[i], "final_values.txt") 101 | ) 102 | 103 | x, y = [], [] 104 | i = 0 105 | for (values, num_poses), modes in zip(values_file_content, mode_file_content): 106 | p, r = comparisons.calc_precision_recall(modes, irl_log) 107 | print("Precision: {}\tRecall: {}".format(p, r)) 108 | 109 | if args.is3d: 110 | fig = plt.figure() 111 | ax = plt.axes(projection="3d") 112 | else: 113 | figure = plt.figure(figsize=[8, 5]) 114 | ax = plt.gca() 115 | 116 | if args.ground_truth is not None: 117 | plot.plot_2d_gt_traj(ax, gt_values[0][0], "nonlinear") 118 | 119 | plot.plot_posegraph_results( 120 | ax, values, modes, irl_log, args.is3d, args.legend 121 | ) 122 | plt.tight_layout() 123 | print(os.path.basename(os.path.normpath(rdir))) 124 | # plt.title(os.path.basename(os.path.normpath(rdir)) + " H:{}".format(i)) 125 | if not args.is3d: 126 | ax.set_aspect("equal") 127 | else: 128 | plot.set_axes_equal(ax) 129 | i += 1 130 | if i >= args.hypo_limit: 131 | break 132 | if not args.is3d: 133 | plt.gca().set_aspect("equal") 134 | 135 | if args.save: 136 | plt.savefig("trajectory.png", dpi=300) 137 | plt.show() 138 | 139 | 140 | if __name__ == "__main__": 141 | main() 142 | -------------------------------------------------------------------------------- /experiments/scripts/plot.py: -------------------------------------------------------------------------------- 1 | """ 2 | plot.py 3 | 4 | This python module provides a variety of plotting utilities used in different scripts. 5 | """ 6 | 7 | 8 | import os 9 | import sys 10 | import numpy as np 11 | import gtsam 12 | 13 | from comparisons import OutlierType, calc_outlier_type 14 | 15 | # OVERHEAD TO ACCESS IRL STUFF 16 | script_dir = os.path.dirname(__file__) 17 | irl_dir = os.path.join(script_dir, "..", "irl", "python") 18 | sys.path.append(irl_dir) 19 | import irl_parsing 20 | import irl_types 21 | 22 | 23 | 24 | def set_axes_equal(ax) -> None: 25 | """ 26 | Make axes of 3D plot have equal scale so that spheres appear as spheres, 27 | cubes as cubes, etc.. This is one possible solution to Matplotlib's 28 | ax.set_aspect('equal') and ax.axis('equal') not working for 3D. 29 | Args: 30 | ax: the axes 31 | """ 32 | limits = np.array([ 33 | ax.get_xlim3d(), 34 | ax.get_ylim3d(), 35 | ax.get_zlim3d(), 36 | ]) 37 | 38 | origin = np.mean(limits, axis=1) 39 | radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0])) 40 | 41 | ax.set_xlim3d([origin[0] - radius, origin[0] + radius]) 42 | ax.set_ylim3d([origin[1] - radius, origin[1] + radius]) 43 | ax.set_zlim3d([origin[2] - radius, origin[2] + radius]) 44 | 45 | """ 46 | ###### ####### ## ####### ######## ###### 47 | ## ## ## ## ## ## ## ## ## ## ## 48 | ## ## ## ## ## ## ## ## ## 49 | ## ## ## ## ## ## ######## ###### 50 | ## ## ## ## ## ## ## ## ## 51 | ## ## ## ## ## ## ## ## ## ## ## 52 | ###### ####### ######## ####### ## ## ###### 53 | """ 54 | CORRECT_ODOM_COLOR = "black" 55 | WRONG_ODOM_COLOR = "darkred" 56 | 57 | TRUE_POSITIVE_LOOP_COLOR = "cornflowerblue" 58 | FALSE_POSITIVE_LOOP_COLOR = "red" 59 | TRUE_NEGATIVE_LOOP_COLOR = "grey" 60 | FALSE_NEGATIVE_LOOP_COLOR = "darkgreen" 61 | 62 | 63 | def get_loop_color(t, entry, mode): 64 | if t == OutlierType.TRUE_POSITIVE: 65 | return TRUE_POSITIVE_LOOP_COLOR, entry.measurements[mode].pose_b_key, "" 66 | elif t == OutlierType.FALSE_POSITIVE: 67 | return FALSE_POSITIVE_LOOP_COLOR, entry.measurements[mode].pose_b_key, "o" 68 | elif t == OutlierType.TRUE_NEGATIVE: 69 | return TRUE_NEGATIVE_LOOP_COLOR, entry.measurements[0].pose_b_key, "" 70 | elif t == OutlierType.FALSE_NEGATIVE: 71 | return ( 72 | FALSE_NEGATIVE_LOOP_COLOR, 73 | entry.measurements[entry.correct_mode].pose_b_key, 74 | "o", 75 | ) 76 | 77 | 78 | """ 79 | ######## ## ####### ######## #### ## ## ###### 80 | ## ## ## ## ## ## ## ### ## ## ## 81 | ## ## ## ## ## ## ## #### ## ## 82 | ######## ## ## ## ## ## ## ## ## ## #### 83 | ## ## ## ## ## ## ## #### ## ## 84 | ## ## ## ## ## ## ## ### ## ## 85 | ## ######## ####### ## #### ## ## ###### 86 | """ 87 | 88 | 89 | def plot_2d_gt_traj(ax, values, lin, **kwargs): 90 | if lin == "nonlinear": 91 | poses = gtsam.utilities.allPose2s(values) 92 | x, y = [], [] 93 | for key in poses.keys(): 94 | pose = poses.atPose2(key) 95 | x.append(pose.x()) 96 | y.append(pose.y()) 97 | ax.plot(x, y, **kwargs) 98 | else: 99 | points = gtsam.utilities.extractPoint2(values) 100 | ax.plot(points.T[0], points.T[1], **kwargs) 101 | 102 | 103 | def plot_3d_traj(ax, poses, lin, **kwargs): 104 | if lin == "nonlinear": 105 | x, y, z = [], [], [] 106 | for pose in poses: 107 | x.append(pose.x()) 108 | y.append(pose.y()) 109 | z.append(pose.z()) 110 | ax.plot(x, y, z, **kwargs) 111 | 112 | def plot_2d_traj(ax, poses, lin, **kwargs): 113 | if lin == "nonlinear": 114 | x, y = [], [] 115 | for pose in poses: 116 | x.append(pose.x()) 117 | y.append(pose.y()) 118 | ax.plot(x, y, **kwargs) 119 | 120 | def plot_link(ax, k1, k2, values, color, alpha, is3d, lin, marker): 121 | try: 122 | if lin == "nonlinear": 123 | if is3d: 124 | p1, p2 = values.atPose3(k1), values.atPose3(k2) 125 | ax.plot( 126 | [p1.x(), p2.x()], 127 | [p1.y(), p2.y()], 128 | [p1.z(), p2.z()], 129 | color=color, 130 | alpha=alpha, 131 | marker=marker, 132 | ) 133 | else: 134 | p1, p2 = values.atPose2(k1), values.atPose2(k2) 135 | ax.plot( 136 | [p1.x(), p2.x()], 137 | [p1.y(), p2.y()], 138 | color=color, 139 | alpha=alpha, 140 | marker=marker, 141 | ) 142 | else: 143 | if is3d: 144 | p1, p2 = values.atPoint3(k1), values.atPoint3(k2) 145 | ax.plot( 146 | [p1[0], p2[0]], 147 | [p1[1], p2[1]], 148 | [p1[2], p2[2]], 149 | color=color, 150 | alpha=alpha, 151 | marker=marker, 152 | ) 153 | else: 154 | p1, p2 = values.atPoint2(k1), values.atPoint2(k2) 155 | ax.plot( 156 | [p1[0], p2[0]], 157 | [p1[1], p2[1]], 158 | color=color, 159 | alpha=alpha, 160 | marker=marker, 161 | ) 162 | 163 | except Exception: 164 | print("WARNING could not plot link") 165 | 166 | 167 | def plot_dot(ax, k, values, color, alpha, is3d): 168 | p1 = values.atPose2(k) 169 | ax.plot([p1.x()], [p1.y()], marker="o", color=color, alpha=alpha) 170 | 171 | 172 | def plot_posegraph_results(ax, values, result_modes, irl_log, is3d, show_legend=True): 173 | """ 174 | Plots the results from a method in 2d 175 | """ 176 | for i in range(min(len(irl_log.entries), len(result_modes))): 177 | entry, mode = irl_log.entries[i], result_modes[i] 178 | if isinstance(entry, irl_types.Prior): 179 | pass # TODO (dan) how to handle multimodal prior 180 | elif isinstance(entry, irl_types.Odometry): 181 | color = ( 182 | CORRECT_ODOM_COLOR if entry.correct_mode == mode else WRONG_ODOM_COLOR 183 | ) 184 | plot_link( 185 | ax, 186 | entry.start_key, 187 | entry.end_key, 188 | values, 189 | color, # used for ex gridworld image "gray", 190 | 1, 191 | is3d, 192 | irl_log.header.linearity, 193 | "", 194 | ) 195 | 196 | elif isinstance(entry, irl_types.Loop): 197 | color, endpoint, marker = get_loop_color( 198 | calc_outlier_type(entry, mode), entry, mode 199 | ) 200 | plot_link( 201 | ax, 202 | entry.pose_a_key, 203 | endpoint, 204 | values, 205 | color, 206 | 0.25, 207 | is3d, 208 | irl_log.header.linearity, 209 | marker, 210 | ) 211 | else: 212 | raise Exception("Invalid Entry Found in Pose Graph") 213 | 214 | # Make empty Plots for Legend 215 | leg_entry = lambda c, t: ax.plot([], [], color=c, label=t, linewidth=4) 216 | 217 | # TEMP FOR DCIST SLIDES (aug 2022) 218 | # leg_entry(CORRECT_ODOM_COLOR, "Trajectory") 219 | # leg_entry(TRUE_POSITIVE_LOOP_COLOR, "Inlier Loop Closures") 220 | # leg_entry(TRUE_NEGATIVE_LOOP_COLOR, "Outlier Loop Closures") 221 | 222 | 223 | # OLD (CORRECT) 224 | #leg_entry(CORRECT_ODOM_COLOR, "Correct Odometry") 225 | #leg_entry(WRONG_ODOM_COLOR, "Incorrect Odometry") 226 | leg_entry(TRUE_POSITIVE_LOOP_COLOR, "True Positive Loop Closure") 227 | leg_entry(FALSE_POSITIVE_LOOP_COLOR, "False Positive Loop Closure") 228 | leg_entry(TRUE_NEGATIVE_LOOP_COLOR, "True Negative Loop Closure") 229 | leg_entry(FALSE_NEGATIVE_LOOP_COLOR, "False Negative Loop Closure") 230 | if show_legend: 231 | ax.legend(loc="center left", bbox_to_anchor=(1, 0.5)) 232 | 233 | 234 | def plot_initialization(ax, irl_log,c): 235 | poses = [] 236 | for entry in irl_log.entries: 237 | if isinstance(entry, irl_types.Prior): 238 | poses.append(entry.measurements[0].pose) 239 | elif isinstance(entry, irl_types.Odometry): 240 | poses.append(poses[-1].compose(entry.measurements[0].pose)) 241 | 242 | x,y=[],[] 243 | for pose in poses: 244 | x.append(pose.x()) 245 | y.append(pose.y()) 246 | 247 | ax.plot(x,y, "-.", color=c, alpha=1) 248 | 249 | 250 | -------------------------------------------------------------------------------- /experiments/scripts/precision-recall: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import argparse 3 | import os 4 | import sys 5 | 6 | import gtsam 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | from gtsam.symbol_shorthand import L, X 10 | import comparisons 11 | import plot 12 | 13 | # OVERHEAD TO ACCESS IRL STUFF 14 | script_dir = os.path.dirname(__file__) 15 | irl_dir = os.path.join(script_dir, "..", "irl", "python") 16 | sys.path.append(irl_dir) 17 | import irl_parsing 18 | import irl_types 19 | 20 | """ 21 | ## ## ### #### ## ## 22 | ### ### ## ## ## ### ## 23 | #### #### ## ## ## #### ## 24 | ## ### ## ## ## ## ## ## ## 25 | ## ## ######### ## ## #### 26 | ## ## ## ## ## ## ### 27 | ## ## ## ## #### ## ## 28 | """ 29 | 30 | 31 | def handle_args(): 32 | parser = argparse.ArgumentParser( 33 | description="Computes precision and recall statistics on results." 34 | ) 35 | parser.add_argument( 36 | "-r", 37 | "--result_dirs", 38 | type=str, 39 | required=True, 40 | nargs="+", 41 | help="The result directory(ies) to evaluate.", 42 | ) 43 | parser.add_argument( 44 | "-i", 45 | "--irl_files", 46 | type=str, 47 | required=True, 48 | nargs="+", 49 | help="The irl file(s) corresponding to result_dir(s).", 50 | ) 51 | return parser.parse_args() 52 | 53 | 54 | def main(): 55 | args = handle_args() 56 | if len(args.irl_files) == 1: 57 | irl_files = args.irl_files * len(args.result_dirs) 58 | else: 59 | irl_files = args.irl_files 60 | n = len(args.result_dirs) 61 | 62 | n = 0 63 | total_precision = 0.0 64 | total_recall = 0.0 65 | 66 | for rdir, ifile in zip(sorted(args.result_dirs), sorted(irl_files)): 67 | irl_log = irl_types.Log.read(ifile) 68 | # print("-----\n", ifile, "\n", rdir) 69 | values_file = os.path.join(rdir, "final_values.txt") 70 | modes_file = os.path.join(rdir, "final_modes.txt") 71 | if not os.path.isfile(values_file): 72 | continue 73 | vcontent = irl_parsing.parse_values_file(values_file) 74 | mcontent = irl_parsing.parse_modes_file(modes_file) 75 | if len(vcontent) == 0: 76 | continue 77 | 78 | values = vcontent[0][0] 79 | modes = mcontent[0] 80 | 81 | p, r = comparisons.calc_precision_recall(modes, irl_log) 82 | n += 1 83 | total_precision += p 84 | total_recall += r 85 | print( 86 | "Avg Precision: {} , Avg Recall: {}".format( 87 | total_precision / n, total_recall / n 88 | ) 89 | ) 90 | print("{} & {}\\\\".format(total_precision / n, total_recall / n)) 91 | 92 | 93 | if __name__ == "__main__": 94 | main() 95 | -------------------------------------------------------------------------------- /experiments/scripts/run-batch-g2o: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import os 5 | import sys 6 | 7 | import gtsam 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | from gtsam.symbol_shorthand import X 11 | from gtsam.utils import plot 12 | 13 | # OVERHEAD TO ACCESS IRL STUFF 14 | script_dir = os.path.dirname(__file__) 15 | irl_dir = os.path.join(script_dir, "..", "irl", "python") 16 | sys.path.append(irl_dir) 17 | import irl_parsing 18 | 19 | 20 | def handle_args(): 21 | parser = argparse.ArgumentParser( 22 | description="Runs a g2o scenario in batch (no outliers/ambiguity)." 23 | ) 24 | parser.add_argument("-i", "--input", help="input file g2o format") 25 | parser.add_argument("-o", "--output", help="Output file for Values") 26 | parser.add_argument( 27 | "-p", "--plot", action="store_true", help="Flag to plot results" 28 | ) 29 | parser.add_argument("--is3d", action="store_true", help="PoseGraph is 3d") 30 | args = parser.parse_args() 31 | return args 32 | 33 | 34 | def main(): 35 | args = handle_args() 36 | graph, initial = gtsam.readG2o(args.input, args.is3d) 37 | 38 | # Add prior on the pose having index (key) = 0 39 | if args.is3d: 40 | priorModel = gtsam.noiseModel.Diagonal.Variances( 41 | np.array([1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-8]) 42 | ) 43 | graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) 44 | else: 45 | priorModel = gtsam.noiseModel.Diagonal.Variances(gtsam.Point3(1e-6, 1e-6, 1e-8)) 46 | graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) 47 | 48 | # Made convergence criteria tight to gget a good solution 49 | params = gtsam.LevenbergMarquardtParams() 50 | params.setMaxIterations(1000) 51 | params.setRelativeErrorTol(1e-12) 52 | params.setAbsoluteErrorTol(1e-12) 53 | optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) 54 | result = optimizer.optimize() 55 | 56 | print("Optimization complete") 57 | print("initial error = ", graph.error(initial)) 58 | print("final error = ", graph.error(result)) 59 | 60 | if args.output is None: 61 | print("\nFactor Graph:\n{}".format(graph)) 62 | print("\nInitial Estimate:\n{}".format(initial)) 63 | # print("Final Result:\n{}".format(result)) 64 | else: 65 | print("Writing results to file: ", args.output) 66 | irl_parsing.write_values_file( 67 | args.output, 3 if args.is3d else 2, "nonlinear", [result] 68 | ) 69 | print("Done!") 70 | 71 | if args.plot: 72 | plot.plot_trajectory(0, result, 0.1) 73 | plt.show() 74 | 75 | 76 | if __name__ == "__main__": 77 | main() 78 | -------------------------------------------------------------------------------- /experiments/scripts/run-dataset-parallel: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import os 5 | import glob 6 | import multiprocessing as mp 7 | import subprocess 8 | 9 | SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__)) 10 | BUILD_DIR = os.path.join(SCRIPT_DIR, "../", "../", "build") 11 | RUNNER = os.path.join(BUILD_DIR, "experiments", "run-experiment") 12 | 13 | 14 | def handle_args(): 15 | parser = argparse.ArgumentParser( 16 | description="Parallelizes running methods on a dataset." 17 | + " NOTE! Do not trust timing results from parallel running." 18 | + " I have no idea how python multiprocessing will affect runtime accuracy." 19 | ) 20 | parser.add_argument( 21 | "--datasets", help="Path to dataset directrory", nargs="+", required=True 22 | ) 23 | parser.add_argument( 24 | "--results", 25 | help="Path to output directory ORDER MUST MATCH --datasets", 26 | nargs="+", 27 | required=True, 28 | ) 29 | parser.add_argument( 30 | "--dims", 31 | help="Dims for datasets ORDER MUST MATCH --datasets", 32 | nargs="+", 33 | type=int, 34 | required=True, 35 | ) 36 | parser.add_argument( 37 | "--save_every_n", 38 | help="Write Intermediate results rate for datasets ORDER MUST MATCH --datasets", 39 | nargs="+", 40 | type=int, 41 | required=True, 42 | ) 43 | parser.add_argument( 44 | "--methods", 45 | type=str, 46 | nargs="+", 47 | help="The methods to run on this dataset by factory name.", 48 | required=True, 49 | ) 50 | parser.add_argument( 51 | "--outper", 52 | type=str, 53 | nargs="+", 54 | help="The outlier percentages (i.e. 10) to run on this dataset", 55 | required=True, 56 | ) 57 | parser.add_argument( 58 | "--nworkers", type=int, help="Number of pool workers to use", required=True 59 | ) 60 | 61 | args = parser.parse_args() 62 | return args 63 | 64 | 65 | def check_results_exist(method, dataset_file, output_dir): 66 | # returns true if there are already results for this command 67 | dataset_base = os.path.basename(dataset_file).split(".irl")[0] 68 | matches = glob.glob( 69 | os.path.join( 70 | output_dir, dataset_base + "_" + method + "*" + "/final_values.txt" 71 | ) 72 | ) 73 | if len(matches) > 0: 74 | return True 75 | else: 76 | # check if there is a directory from a past failed exp and remove 77 | old_dirs = glob.glob( 78 | os.path.join(output_dir, dataset_base + "_" + method + "*/") 79 | ) 80 | for od in old_dirs: 81 | os.system("rm -rf {}".format(od)) 82 | 83 | 84 | def worker(method, dataset_file, output_dir, save_every_n, is3d): 85 | if check_results_exist(method, dataset_file, output_dir): 86 | print("skipping: " + method + " + " + dataset_file) 87 | return 88 | log_file = os.path.join( 89 | output_dir, 90 | os.path.basename(dataset_file).split(".irl")[0] + "_{}_stdout.txt".format(method), 91 | ) 92 | with open(log_file, "a") as outfile: 93 | command = "{} -m {} -i {} -o {} -n {} {}".format( 94 | RUNNER, 95 | method, 96 | dataset_file, 97 | output_dir, 98 | save_every_n, 99 | "--is3d" if is3d else "", 100 | ) 101 | subprocess.call(command.split(" "), stdout=outfile, stderr=outfile) 102 | 103 | 104 | args = handle_args() 105 | pool = mp.Pool(args.nworkers) 106 | for dataset_dir, result_dir, dim, save_every_n in zip( 107 | args.datasets, args.results, args.dims, args.save_every_n 108 | ): 109 | for outlier_percent in args.outper: 110 | subproblem_dir = os.path.join(dataset_dir, outlier_percent) 111 | irl_files = glob.glob(subproblem_dir + "/*.irl") 112 | irl_files.sort() 113 | for dataset_file in irl_files: 114 | for method in args.methods: 115 | pool.apply_async( 116 | worker, 117 | args=( 118 | method, 119 | dataset_file, 120 | os.path.join(result_dir, outlier_percent), 121 | save_every_n, 122 | "--is3d" if dim == 3 else "", 123 | ), 124 | ) 125 | pool.close() 126 | pool.join() 127 | -------------------------------------------------------------------------------- /experiments/scripts/toro-2-g2o: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import argparse 3 | import numpy as np 4 | 5 | """ 6 | ## ## ### #### ## ## 7 | ### ### ## ## ## ### ## 8 | #### #### ## ## ## #### ## 9 | ## ### ## ## ## ## ## ## ## 10 | ## ## ######### ## ## #### 11 | ## ## ## ## ## ## ### 12 | ## ## ## ## #### ## ## 13 | """ 14 | 15 | 16 | def handle_args(): 17 | parser = argparse.ArgumentParser( 18 | description="Converts a 2d toro file to a 2d g2o file (used for datasets/csail)" 19 | ) 20 | parser.add_argument( 21 | "-i", "--input", type=str, required=True, help="The input TORO file." 22 | ) 23 | parser.add_argument( 24 | "-o", 25 | "--output", 26 | type=str, 27 | required=True, 28 | help="The name of the file to save out the g2o file.", 29 | ) 30 | return parser.parse_args() 31 | 32 | 33 | def main(): 34 | args = handle_args() 35 | 36 | with open(args.input) as infile: 37 | with open(args.output, "w") as outfile: 38 | for line in infile.readlines(): 39 | tokens = line.strip().split(" ") 40 | if tokens[0] == "VERTEX2": 41 | """ 42 | Converts a TORO VERTEX2 to g2o VERTEX_SE2 43 | """ 44 | idx, x, y, th = tokens[1:] 45 | outfile.write("VERTEX_SE2 {} {} {} {}\n".format(idx, x, y, th)) 46 | 47 | elif tokens[0] == "EDGE2": 48 | """ 49 | Parses a TORO SE(2) Edge line 50 | VERTEX2 IDout IDin dx dy dth I11 I12 I22 I33 I13 I23 51 | """ 52 | ida, idb = tokens[1:3] 53 | x, y, th = float(tokens[3]), float(tokens[4]), float(tokens[5]) 54 | I11, I12, I22, I33, I13, I23 = map(float, tokens[6:]) 55 | I = np.array([[I11, I12, I13], [I12, I22, I23], [I13, I23, I33]]) 56 | outfile.write( 57 | "EDGE_SE2 {} {} {} {} {} {} {} {} {} {} {} \n".format( 58 | ida, 59 | idb, 60 | x, 61 | y, 62 | th, 63 | I[0, 0], 64 | I[0, 1], 65 | I[0, 2], 66 | I[1, 1], 67 | I[1, 2], 68 | I[2, 2], 69 | ) 70 | ) 71 | 72 | 73 | if __name__ == "__main__": 74 | main() 75 | -------------------------------------------------------------------------------- /experiments/thirdparty/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | message(STATUS "================ THIRDPARTY EXPERIMENT DEPENDENCIES ======================") 2 | 3 | message(STATUS "====================== Kimera RPGO ======================") 4 | add_subdirectory(Kimera-RPGO) # PCM 5 | 6 | message(STATUS "====================== DCSAM ======================") 7 | add_subdirectory(dcsam) # DC-SAM -------------------------------------------------------------------------------- /media/result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SNU-DLLAB/EGNC-PGO/9d51d32d4de6ef9d601186ce9572a1faa479b87f/media/result.png -------------------------------------------------------------------------------- /risam/.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | ColumnLimit: 120 -------------------------------------------------------------------------------- /risam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | message(STATUS "================ riSAM ======================") 2 | 3 | file(GLOB_RECURSE risam_srcs "*.cpp" "*.h") 4 | add_library(risam ${risam_srcs}) 5 | target_link_libraries(risam gtsam) 6 | target_include_directories(risam PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") -------------------------------------------------------------------------------- /risam/README.md: -------------------------------------------------------------------------------- 1 | # riSAM 2 | This directory contains a generic implementation of the riSAM algorithm. 3 | 4 | # EGNC-PGO 5 | riSAM.cpp and riSAM.h are modified to implement EGNC-PGO. 6 | 7 | ## Modules 8 | This package contains 4 modules that composed make up the riSAM algorithm. 9 | * `DoglegLineSearch` (DLLS): Implements the dog-leg linesearch algorithm (riSAM Paper Alg. 3) 10 | * Note: There are also many variants of DLLS implemented and can be enabled by changing around the `DoglegLineSearchParams`. The parameters of the algorithm used for the paper can be found in `experiments/exp_runner/include/exp_runner/Factory.h`. 11 | * `GraduatedKernel` : Implements the graduated kernels presented in the original GNC work, as well as the SIG Kernel (riSAM Paper Eq. 2) 12 | * `GraduatedFactor` : Implements a generic GTSAM factor augmented with a graduated kernel. 13 | * `RISAM2` : Implements the riSAM algorithm as an extension of `gtsam::ISAM2`. Note there is a non-trivial chunk of code copied from the ISAM2 implementation as riSAM modifies ISAM2 behavior in ways that could not be injected with "better" coding practices. (riSAM Paper Alg. 4) 14 | 15 | ## Documentation 16 | Documentation for the code itself is provided in-line and at a per-function level. Most of the code is well documented in this fashion, but it is research code so there are gaps. If anything is unclear open a issue and I will address gaps in documentation on an as-needed basis! -------------------------------------------------------------------------------- /risam/include/risam/DoglegLineSearch.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #pragma once 5 | using namespace gtsam; 6 | 7 | namespace risam { 8 | 9 | enum DoglegLineSearchType { OUTWARD, MIDPOINT, GOLDEN, GRID }; 10 | 11 | struct DoglegLineSearchParams { 12 | DoglegLineSearchType search_type{OUTWARD}; // The search type to perform 13 | double min_delta{1e-3}; // Minimum allowed small delta 14 | double init_delta{1}; // Initial Delta to test 15 | double max_delta{1e2}; // Maximum allowed delta 16 | double out_step_multiple{1.5}; // Increase of trust region for outward steps 17 | double in_step_multiple{0.5}; // Increase of trust region for inward steps 18 | double sufficent_decrease_coeff{1e-4}; // Coefficient for sufficient decrease check 19 | double num_grid_samples{100}; // Number of samples to use for grid search 20 | double wildfire_threshold{1e-3}; 21 | }; 22 | 23 | struct DoglegLineSearchImpl { 24 | template // OUTWARD SEARCH IS THE BEST ACCORDING TO EXPERIMENTAL RESULTS 25 | static DoglegOptimizerImpl::IterationResult OutwardSearch(const double min_delta, const double max_delta, 26 | const double step_multiple, 27 | const double sufficent_decrease_coeff, 28 | const VectorValues& dx_u, const VectorValues& dx_n, 29 | const Rd& risam, const Values& x0) { 30 | const double F_init_error = risam.robustError(x0); 31 | const double gn_step = dx_n.norm(); 32 | const double max_step = std::min(max_delta, gn_step); 33 | double step = std::min(min_delta, gn_step); 34 | 35 | DoglegOptimizerImpl::IterationResult result; 36 | result.dx_d = DoglegOptimizerImpl::ComputeDoglegPoint(step, dx_u, dx_n, false); 37 | result.f_error = risam.robustError(x0.retract(result.dx_d)); 38 | result.delta = step; 39 | 40 | // search Increase delta 41 | bool stay = step < max_step; 42 | while (stay) { 43 | step *= step_multiple; 44 | VectorValues dx_d = DoglegOptimizerImpl::ComputeDoglegPoint(step, dx_u, dx_n, false); 45 | Values x_d(x0.retract(dx_d)); 46 | double new_F_error = risam.robustError(x_d); 47 | bool update_increased_error = new_F_error > result.f_error; 48 | bool step_has_sufficient_decrease = new_F_error < (F_init_error - sufficent_decrease_coeff * step); 49 | stay = !(step_has_sufficient_decrease && update_increased_error) && step < max_step; 50 | 51 | if (step_has_sufficient_decrease) { 52 | result.f_error = new_F_error; 53 | result.dx_d = dx_d; 54 | result.delta = step; 55 | } 56 | } 57 | return result; 58 | } 59 | 60 | template 61 | static DoglegOptimizerImpl::IterationResult MidpointSearch(const double min_delta, const double init_delta, 62 | const double max_delta, const double outward_step_multiple, 63 | const double inward_step_multiple, 64 | const VectorValues& dx_u, const VectorValues& dx_n, 65 | const Rd& risam, const Values& x0) { 66 | const double F_init_error = risam.robustError(x0); 67 | const double gn_step = dx_n.norm(); 68 | const double max_step = std::min(max_delta, gn_step); 69 | double step = std::min(init_delta, gn_step); 70 | 71 | DoglegOptimizerImpl::IterationResult result; 72 | result.dx_d = DoglegOptimizerImpl::ComputeDoglegPoint(step, dx_u, dx_n, false); 73 | result.f_error = risam.robustError(x0.retract(result.dx_d)); 74 | result.delta = step; 75 | 76 | // Search outward or inward depending on init_error 77 | bool stay = min_delta < step && step < max_delta; 78 | double step_dir = result.f_error < F_init_error ? outward_step_multiple : inward_step_multiple; 79 | while (stay) { 80 | step *= step_dir; 81 | VectorValues dx_d = DoglegOptimizerImpl::ComputeDoglegPoint(step, dx_u, dx_n, false); 82 | Values x_d(x0.retract(dx_d)); 83 | double new_F_error = risam.robustError(x_d); 84 | stay = (new_F_error <= result.f_error) && min_delta < step && step < max_delta; 85 | 86 | if (new_F_error <= result.f_error) { 87 | result.f_error = new_F_error; 88 | result.dx_d = dx_d; 89 | result.delta = step; 90 | } 91 | } 92 | return result; 93 | } 94 | 95 | template 96 | static DoglegOptimizerImpl::IterationResult GoldenSearch(const double min_delta, const double max_delta, 97 | const VectorValues& dx_u, const VectorValues& dx_n, 98 | const Rd& risam, const Values& x0) { 99 | double a = min_delta; 100 | double b = std::min(max_delta, dx_n.norm()); 101 | const double gr = (std::sqrt(5) + 1 / 2); 102 | double c = b - (b - a) / gr; 103 | double d = a + (b - a) / gr; 104 | 105 | while (std::abs(b - a) > 0.1) { 106 | double f_error_c = risam.robustError(DoglegOptimizerImpl::ComputeDoglegPoint(c, dx_u, dx_n, false)); 107 | double f_error_d = risam.robustError(DoglegOptimizerImpl::ComputeDoglegPoint(d, dx_u, dx_n, false)); 108 | if (f_error_c < f_error_d) { 109 | b = d; 110 | } else { 111 | a = c; 112 | } 113 | 114 | c = b - (b - a) / gr; 115 | d = a + (b - a) / gr; 116 | } 117 | double best_delta = (a + b) / 2.0; 118 | DoglegOptimizerImpl::IterationResult result; 119 | result.dx_d = DoglegOptimizerImpl::ComputeDoglegPoint(best_delta, dx_u, dx_n, false); 120 | result.f_error = risam.robustError(x0.retract(result.dx_d)); 121 | result.delta = best_delta; 122 | return result; 123 | } 124 | 125 | template 126 | static DoglegOptimizerImpl::IterationResult GridSearch(const double min_delta, const double max_delta, 127 | const double num_samples, const VectorValues& dx_u, 128 | const VectorValues& dx_n, const Rd& risam, const Values& x0) { 129 | const double gn_step = dx_n.norm(); 130 | const double max_step = std::min(max_delta, gn_step); 131 | double step = std::min(min_delta, gn_step); 132 | const double step_increment = ((max_step - step) / num_samples) + 1e-6; 133 | 134 | DoglegOptimizerImpl::IterationResult result; 135 | result.dx_d = DoglegOptimizerImpl::ComputeDoglegPoint(step, dx_u, dx_n, false); 136 | result.f_error = risam.robustError(x0.retract(result.dx_d)); 137 | result.delta = step; 138 | 139 | while (step <= max_step) { 140 | VectorValues dx_d = DoglegOptimizerImpl::ComputeDoglegPoint(step, dx_u, dx_n, false); 141 | double error = risam.robustError(x0.retract(dx_d)); 142 | if (error < result.f_error) { 143 | result.dx_d = dx_d; 144 | result.f_error = error; 145 | result.delta = step; 146 | } 147 | step += step_increment; 148 | } 149 | return result; 150 | } 151 | 152 | /** @brief Computes one iteration of linesearch on the DogLeg Trajectory 153 | * Step locations are computed by the dogleg point for increasing sizes of the Dogleg trust region 154 | * @param init_delta: The initial step size usually a small number 155 | * @param max_delta: The maximum step size (may be clipped if magnitude of dx_n is smaller) 156 | * @param dx_u: Cauchy Point 157 | * @param dx_n: Gauss Newton Point 158 | * @param risam: The incremental solver used to compute linear and non-linear error 159 | * @param x0: The current estimate of values 160 | */ 161 | template 162 | static DoglegOptimizerImpl::IterationResult Iterate(const DoglegLineSearchParams& params, const VectorValues& dx_u, 163 | const VectorValues& dx_n, const Rd& risam, const Values& x0) { 164 | switch (params.search_type) { 165 | case DoglegLineSearchType::OUTWARD: 166 | return OutwardSearch(params.init_delta, params.max_delta, params.out_step_multiple, 167 | params.sufficent_decrease_coeff, dx_u, dx_n, risam, x0); 168 | case DoglegLineSearchType::MIDPOINT: 169 | return MidpointSearch(params.min_delta, params.init_delta, params.max_delta, params.in_step_multiple, 170 | params.out_step_multiple, dx_u, dx_n, risam, x0); 171 | case DoglegLineSearchType::GOLDEN: 172 | return GoldenSearch(params.min_delta, params.max_delta, dx_u, dx_n, risam, x0); 173 | case DoglegLineSearchType::GRID: 174 | return GridSearch(params.min_delta, params.max_delta, params.num_grid_samples, dx_u, dx_n, risam, x0); 175 | default: 176 | throw std::runtime_error("Provided DoglegSearchType not implemented"); 177 | }; 178 | } 179 | }; 180 | 181 | } // namespace risam -------------------------------------------------------------------------------- /risam/include/risam/GraduatedFactor.h: -------------------------------------------------------------------------------- 1 | /** @brief Factor that implements a graduated robust cost function. 2 | * A Graduated factor overrides a portion of the NoiseModelFactor interface to implement a graduated robust cost 3 | * function for the factor. 4 | * 5 | * @author Dan McGann 6 | * @date Mar 2022 7 | */ 8 | #pragma once 9 | #include 10 | 11 | #include "risam/GraduatedKernel.h" 12 | 13 | namespace risam { 14 | class GraduatedFactor { 15 | /** TYPES **/ 16 | public: 17 | typedef boost::shared_ptr shared_ptr; 18 | 19 | /** FIELDS **/ 20 | protected: 21 | /// @brief the Graduated Robust Kernel for this factor 22 | GraduatedKernel::shared_ptr kernel_; 23 | 24 | /**INTERFACE**/ 25 | public: 26 | /** @brief Constructor 27 | * @param kernel: The graduated kernel to apply to this factor 28 | * @param args: The arguments required to construct the FACTOR_TYPE 29 | */ 30 | GraduatedFactor(GraduatedKernel::shared_ptr kernel); 31 | 32 | /// @brief Copy constructor 33 | GraduatedFactor(const GraduatedFactor& other); 34 | 35 | /** @brief Linearize this factor at a specific point, using the specified convexification parameter mu 36 | * @param current_estimate: the variable estimate at which to linearize the Factor 37 | * @param mu: the current value of the convexification parameter for this factor 38 | */ 39 | virtual gtsam::GaussianFactor::shared_ptr linearizeRobust(const gtsam::Values& current_estimate, 40 | const double& mu) const = 0; 41 | 42 | /// @brief returns the residual of the factor 43 | virtual double residual(const gtsam::Values& current_estimate) const = 0; 44 | 45 | /// @brief returns \rho(r) of the factor 46 | virtual double robustResidual(const gtsam::Values& current_estimate, const double& mu) const = 0; 47 | 48 | /// @brief Returns the value of \mu_{init} for this graduated kernel 49 | const GraduatedKernel::shared_ptr kernel() const; 50 | }; 51 | 52 | template 53 | class GenericGraduatedFactor : public FACTOR_TYPE, public GraduatedFactor { 54 | static_assert(std::is_base_of::value, 55 | "GraduatedFactor Must be instantiated with a Factor Derived from gtsam::NonlinearFactor."); 56 | 57 | /** TYPES **/ 58 | public: 59 | typedef boost::shared_ptr> shared_ptr; 60 | 61 | /**INTERFACE**/ 62 | public: 63 | /** @brief Constructor 64 | * @param args: The arguments required to construct the FACTOR_TYPE 65 | */ 66 | template 67 | GenericGraduatedFactor(GraduatedKernel::shared_ptr kernel, Args&&... args) 68 | : FACTOR_TYPE(std::forward(args)...), GraduatedFactor(kernel) {} 69 | 70 | /// @brief Copy constructor 71 | GenericGraduatedFactor(const GenericGraduatedFactor& other) 72 | : FACTOR_TYPE(other), GraduatedFactor(other.kernel_) {} 73 | 74 | /// @brief makes a deep copy 75 | gtsam::NonlinearFactor::shared_ptr clone() const override { 76 | return boost::static_pointer_cast( 77 | gtsam::NonlinearFactor::shared_ptr(new GenericGraduatedFactor(*this))); 78 | } 79 | 80 | /** GRADUATED INTERFACE **/ 81 | /* ************************************************************************* */ 82 | gtsam::GaussianFactor::shared_ptr linearizeRobust(const gtsam::Values& current_estimate, 83 | const double& mu) const override { 84 | // Compute the current residual of the underlying factor 85 | double r = residual(current_estimate); 86 | 87 | // Linearize the underlying factor 88 | gtsam::Matrix A; 89 | gtsam::Vector b; 90 | auto whitened_linear_system = FACTOR_TYPE::linearize(current_estimate); 91 | std::tie(A, b) = whitened_linear_system->jacobian(); 92 | size_t output_dim = b.size(); 93 | 94 | // Extract the non-dense linear system 95 | auto keys = whitened_linear_system->keys(); 96 | std::vector Ablocks; 97 | size_t idx_start = 0; 98 | for (const auto& key : keys) { 99 | size_t d = current_estimate.at(key).dim(); 100 | gtsam::Matrix vblock = gtsam::Matrix::Zero(output_dim, d); 101 | Ablocks.push_back(A.block(0, idx_start, output_dim, d)); 102 | idx_start += d; 103 | } 104 | 105 | // Weight the Linearized Blocks using given control parameter value 106 | kernel_->weightSystem(Ablocks, b, r, mu); 107 | 108 | // Construct a jacobian factor from the weighted system 109 | FastMap Ablock_map; 110 | for (int i = 0; i < Ablocks.size(); i++) { 111 | Ablock_map[keys[i]] = Ablocks[i]; 112 | } 113 | return boost::make_shared(Ablock_map, b); 114 | } 115 | /* ************************************************************************* */ 116 | double residual(const gtsam::Values& current_estimate) const override { 117 | return sqrt(2.0 * FACTOR_TYPE::error(current_estimate)); 118 | } 119 | 120 | double robustResidual(const gtsam::Values& current_estimate, const double& mu) const override { 121 | return kernel_->error(residual(current_estimate), mu); 122 | } 123 | }; 124 | 125 | /** HELPERS **/ 126 | template 127 | GenericGraduatedFactor make_graduated(Args&&... args) { 128 | return GenericGraduatedFactor(std::forward(args)...); 129 | } 130 | 131 | template 132 | typename GenericGraduatedFactor::shared_ptr make_shared_graduated(Args&&... args) { 133 | return boost::make_shared>(std::forward(args)...); 134 | } 135 | } // namespace risam 136 | -------------------------------------------------------------------------------- /risam/include/risam/GraduatedKernel.h: -------------------------------------------------------------------------------- 1 | /** @brief Interface for a graduated robust kernel. 2 | * That is a robust kernel \rho(r_i, \mu) that varies from highly convex (quadratic) when \mu = \mu_{init} to non-convex 3 | * (robust cost) as \mu trends towards \mu_{final} 4 | * 5 | * @author Dan McGann 6 | * @date Mar 2022 7 | */ 8 | #pragma once 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | using namespace gtsam; 15 | namespace risam { 16 | class GraduatedKernel { 17 | /** TYPES **/ 18 | public: 19 | typedef boost::shared_ptr shared_ptr; 20 | 21 | /** FIELDS **/ 22 | protected: 23 | /// @brief The initial Value for mu 24 | const double mu_init_; 25 | /// @brief The threshold at which to consider mu to be converged 26 | const double convergence_thresh_; 27 | 28 | /** INTERFACE **/ 29 | public: 30 | GraduatedKernel(double mu_init, double convergence_thresh) 31 | : mu_init_(mu_init), convergence_thresh_(convergence_thresh) {} 32 | 33 | /** @brief Computes the graduated robust error of the function 34 | * Error = \rho(r_i) 35 | * 36 | * @param residual: r_i the current (whitened) residual 37 | * @param mu: \mu the current convexity parameter 38 | */ 39 | virtual double error(const double &residual, const double &mu) const = 0; 40 | 41 | /** @brief Computes the weight of the graduated robust cost function 42 | * weight = w(r_i) 43 | * 44 | * @param residual: r_i the current (whitened) residual 45 | * @param mu: \mu the current convexity parameter 46 | */ 47 | virtual double weight(const double &residual, const double &mu) const = 0; 48 | 49 | /** @brief Weights a linearized system 50 | * Weight results in the solving of: w_i * r_i^2 = \sqrt(w_i) (A_i x + b_i) = \sqrt(w_i)* A_i x + \sqrt(w_i) * b_i 51 | * 52 | * @param A: Whitened linear system from a factor (\Sigma^{-0.5} H_i) 53 | * @param b: Whitened linear system error from a factor (0.5 * \Sigma^(-0.5) ||h_i(x_i) - z_i||^2 = 0.5 * r_i^2) 54 | */ 55 | void weightSystem(std::vector &A, Vector &b, const double &residual, const double &mu) const { 56 | double sqrt_weight = sqrt(weight(residual, mu)); 57 | // std::cout << "w: " << sqrt_weight << " mu: " << mu << std::endl; 58 | for (Matrix &Aj : A) { 59 | Aj *= sqrt_weight; 60 | } 61 | b *= sqrt_weight; 62 | } 63 | 64 | /// @brief Returns the value of \mu_{init} for this graduated kernel 65 | double muInit() const { return mu_init_; } 66 | 67 | /// @brief Returns the next value of \mu for this graduated kernel 68 | virtual double updateMu(const double &mu) const = 0; 69 | 70 | /// @brief Returns the previous value of \mu for this graduated kernel 71 | virtual double updateMuInv(const double &mu) const = 0; 72 | 73 | /// @brief Returns true iff the value of \mu has converged to a non-convex state 74 | virtual bool isMuConverged(const double &mu) const = 0; 75 | }; 76 | 77 | /* ************************************************************************* */ 78 | class GraduatedTLSKernel : public GraduatedKernel { 79 | /**Graduated version of the truncated least squares kernel 80 | * ref: Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection 81 | */ 82 | 83 | /** FIELDS **/ 84 | public: 85 | /// @brief The sigma thresh at which to truncate from quadratic to constant cost 86 | const double truncation_thresh_; 87 | 88 | /** @brief Constructor 89 | * @param truncation_thresh: the normalized distance from the mean at which to truncate the error 90 | */ 91 | GraduatedTLSKernel(double truncation_thresh = 1.0, double mu_init = 1e-2, double convergence_thresh = 1e5) 92 | : GraduatedKernel(mu_init, convergence_thresh), truncation_thresh_(truncation_thresh){}; 93 | 94 | // Interface 95 | double error(const double &residual, const double &mu) const override; 96 | double weight(const double &residual, const double &mu) const; 97 | double updateMu(const double &mu) const override; 98 | double updateMuInv(const double &mu) const override; 99 | bool isMuConverged(const double &mu) const override; 100 | }; 101 | 102 | /* ************************************************************************* */ 103 | class GraduatedGMKernel : public GraduatedKernel { 104 | /**Graduated version of the Geman-McClure kernel 105 | * ref: Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection 106 | */ 107 | 108 | /** FIELDS **/ 109 | public: 110 | /// @brief The shape parameter of the GM kernel 111 | const double shape_param_; 112 | 113 | /** @brief Constructor 114 | * @param shape_param: the shape parameter for the GM 115 | */ 116 | GraduatedGMKernel(double shape_param = 1.0, double mu_init = 1e4, double convergence_thresh = 1.1) 117 | : GraduatedKernel(mu_init, convergence_thresh), shape_param_(shape_param){}; 118 | 119 | // Interface 120 | double error(const double &residual, const double &mu) const override; 121 | double weight(const double &residual, const double &mu) const override; 122 | double updateMu(const double &mu) const override; 123 | double updateMuInv(const double &mu) const override; 124 | bool isMuConverged(const double &mu) const override; 125 | }; 126 | 127 | /* ************************************************************************* */ 128 | class SIGKernel : public GraduatedKernel { 129 | /** My version of the Graduated version of the Geman-McClure kernel */ 130 | 131 | /** FIELDS **/ 132 | public: 133 | /// @brief The shape parameter of the GM kernel 134 | const double shape_param_; 135 | 136 | /** @brief Constructor 137 | * @param shape_param: the shape parameter for the GM 138 | */ 139 | SIGKernel(double shape_param = 3.0) : GraduatedKernel(0.0, 1.0), shape_param_(shape_param){}; 140 | 141 | // Interface 142 | double error(const double &residual, const double &mu) const override; 143 | double weight(const double &residual, const double &mu) const override; 144 | double updateMu(const double &mu) const override; 145 | double updateMuInv(const double &mu) const override; 146 | bool isMuConverged(const double &mu) const override; 147 | }; 148 | } // namespace risam -------------------------------------------------------------------------------- /risam/src/GraduatedFactor.cpp: -------------------------------------------------------------------------------- 1 | #include "risam/GraduatedFactor.h" 2 | 3 | namespace risam { 4 | /* ************************************************************************* */ 5 | GraduatedFactor::GraduatedFactor(GraduatedKernel::shared_ptr kernel) : kernel_(kernel) {} 6 | 7 | /* ************************************************************************* */ 8 | GraduatedFactor::GraduatedFactor(const GraduatedFactor& other) : kernel_(other.kernel_) {} 9 | 10 | /* ************************************************************************* */ 11 | const GraduatedKernel::shared_ptr GraduatedFactor::kernel() const { return kernel_; } 12 | 13 | } // namespace risam -------------------------------------------------------------------------------- /risam/src/GraduatedKernel.cpp: -------------------------------------------------------------------------------- 1 | #include "risam/GraduatedKernel.h" 2 | 3 | #include 4 | namespace risam { 5 | 6 | /** 7 | * ######## ## ###### 8 | * ## ## ## ## 9 | * ## ## ## 10 | * ## ## ###### 11 | * ## ## ## 12 | * ## ## ## ## 13 | * ## ######## ###### 14 | */ 15 | /* ************************************************************************* */ 16 | double GraduatedTLSKernel::error(const double &residual, const double &mu) const { 17 | // Yang et al. 2020 eq: 6 18 | double c2 = truncation_thresh_ * truncation_thresh_; 19 | double r2 = residual * residual; 20 | if (r2 > ((mu + 1) / mu) * c2) { 21 | return r2; 22 | } else if (r2 < (mu / (mu + 1)) * c2) { 23 | return c2; 24 | } else { 25 | return 2 * truncation_thresh_ * abs(residual) * sqrt(mu * (mu + 1)) - mu * (c2 + r2); 26 | } 27 | } 28 | /* ************************************************************************* */ 29 | double GraduatedTLSKernel::weight(const double &residual, const double &mu) const { 30 | // Yang et al. 2020 eq: 14 31 | double c2 = truncation_thresh_ * truncation_thresh_; 32 | double r2 = residual * residual; 33 | if (r2 > ((mu + 1) / mu) * c2) { 34 | return 0; 35 | } else if (r2 < (mu / (mu + 1)) * c2) { 36 | return 1; 37 | } else { 38 | return (truncation_thresh_ / residual) * sqrt(mu * (mu + 1)) - 1; 39 | } 40 | } 41 | 42 | /* ************************************************************************* */ 43 | double GraduatedTLSKernel::updateMu(const double &mu) const { 44 | // Yang et al. 2020 Remark 5 45 | return mu * 1.4; 46 | } 47 | 48 | /* ************************************************************************* */ 49 | double GraduatedTLSKernel::updateMuInv(const double &mu) const { 50 | double prev_mu = mu / 1.4; 51 | return prev_mu < mu_init_ ? 1.0 : prev_mu; 52 | } 53 | 54 | /* ************************************************************************* */ 55 | bool GraduatedTLSKernel::isMuConverged(const double &mu) const { return mu >= convergence_thresh_; } 56 | 57 | /** 58 | * ###### ## ## 59 | * ## ## ### ### 60 | * ## #### #### 61 | * ## #### ## ### ## 62 | * ## ## ## ## 63 | * ## ## ## ## 64 | * ###### ## ## 65 | */ 66 | /* ************************************************************************* */ 67 | double GraduatedGMKernel::error(const double &residual, const double &mu) const { 68 | // Yang et al. 2020 eq: 4 69 | double r2 = residual * residual; 70 | double c2 = shape_param_ * shape_param_; 71 | return 0.5 * (mu * c2 * r2) / (mu * c2 + r2); 72 | } 73 | 74 | /* ************************************************************************* */ 75 | double GraduatedGMKernel::weight(const double &residual, const double &mu) const { 76 | // Yang et al. 2020 eq: 12 77 | double r2 = residual * residual; 78 | double c2 = shape_param_ * shape_param_; 79 | double sqrt_weight = ((mu * c2) / (r2 + mu * c2)); 80 | return sqrt_weight * sqrt_weight; 81 | } 82 | 83 | /* ************************************************************************* */ 84 | double GraduatedGMKernel::updateMu(const double &mu) const { 85 | // Yang et al. 2020 Remark 5 86 | double next_mu = mu / 1.8; 87 | return next_mu < convergence_thresh_ ? 1.0 : next_mu; 88 | } 89 | 90 | /* ************************************************************************* */ 91 | double GraduatedGMKernel::updateMuInv(const double &mu) const { 92 | double prev_mu = mu * 1.8; 93 | return prev_mu > mu_init_ ? mu_init_ : prev_mu; 94 | } 95 | 96 | /* ************************************************************************* */ 97 | bool GraduatedGMKernel::isMuConverged(const double &mu) const { return mu <= convergence_thresh_; } 98 | 99 | /** 100 | * ###### ## ## ###### ######## ####### ## ## ###### ## ## 101 | * ## ## ## ## ## ## ## ## ## ### ### ## ## ### ### 102 | * ## ## ## ## ## ## ## #### #### ## #### #### 103 | * ## ## ## ###### ## ## ## ## ### ## ## #### ## ### ## 104 | * ## ## ## ## ## ## ## ## ## ## ## ## ## 105 | * ## ## ## ## ## ## ## ## ## ## ## ## ## ## ## 106 | * ###### ####### ###### ## ####### ## ## ###### ## ## 107 | */ 108 | /* ************************************************************************* */ 109 | double SIGKernel::error(const double &residual, const double &mu) const { 110 | double r2 = residual * residual; 111 | double c2 = shape_param_ * shape_param_; 112 | return 0.5 * (c2 * r2) / (c2 + pow(r2, mu)); 113 | } 114 | 115 | /* ************************************************************************* */ 116 | double SIGKernel::weight(const double &residual, const double &mu) const { 117 | double r2 = residual * residual; 118 | double c2 = shape_param_ * shape_param_; 119 | double sqrt_denom = c2 + pow(r2, mu); 120 | return (c2 * (c2 + pow(r2, mu) * (1 - mu))) / (sqrt_denom * sqrt_denom); 121 | } 122 | 123 | /* ************************************************************************* */ 124 | double SIGKernel::updateMu(const double &mu) const { 125 | return std::min(1.0, mu + (mu - mu_init_ + 0.1) * 1.2); 126 | // return std::min(1.0, mu + 0.2); 127 | } 128 | 129 | /* ************************************************************************* */ 130 | double SIGKernel::updateMuInv(const double &mu) const { 131 | return std::max(0.0, mu - 0.1); 132 | // return std::max(0.0, (mu + 1.2 * mu_init_ - 0.12) / 2.2); 133 | } 134 | 135 | /* ************************************************************************* */ 136 | bool SIGKernel::isMuConverged(const double &mu) const { return mu >= convergence_thresh_; } 137 | } // namespace risam --------------------------------------------------------------------------------