riSAM Open Source Release

.gitignore

@@ -0,0 +1,4 @@
+build/
+.vscode/
+__pycache__/
+*.so

.gitmodules

@@ -0,0 +1,6 @@
+[submodule "experiments/thirdparty/Kimera-RPGO"]
+	path = experiments/thirdparty/Kimera-RPGO
+	url = https://github.com/MIT-SPARK/Kimera-RPGO.git
+[submodule "experiments/thirdparty/dcsam"]
+	path = experiments/thirdparty/dcsam
+	url = https://github.com/MarineRoboticsGroup/dcsam.git

CMakeLists.txt

@@ -0,0 +1,13 @@
+project(robust-inference CXX C)
+cmake_minimum_required(VERSION 2.8.3)
+
+message(STATUS "================  ROBUST-INFERENCE  ======================")
+
+find_package(GTSAM REQUIRED)
+include_directories(${GTSAM_INCLUDE_DIR})
+
+# build riSAM
+add_subdirectory(risam)
+
+# Build Experiments directory
+add_subdirectory(experiments)

DEPENDENCIES.md

@@ -0,0 +1,28 @@
+# Dependencies
+
+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.
+
+# riSAM
+* C++ and its standard libraries
+* GTSAM [BSD license] (unmodified linked dependency), (Modified source code to create riSAM algorithm see: RISAM2.h, RISAM2.cpp)
+
+# Experiments
+* Runners:
+    * C++ and its builtin libraries
+    * Kimera-RPGO [BSD 2-Clause License] (referenced as submodule)
+    * dcsam [MIT License] (referenced as submodule)
+    * GTSAM [BSD License] (unmodified linked dependency)
+    * Boost [Boost Software License] (unmodified linked dependency)
+
+* Scripts
+    * Python and its builtin Standard libraries
+    * GTSAM [BSD license] (unmodified linked dependency)
+    * NUMPY [Numpy License] (unmodified linked dependency)
+    * Matplotlib [Matplotlib License] (unmodified linked dependency)
+    * SciPy [BSD licensed] (unmodified linked dependency)
+
+* Datasets
+    * Derived from various published works. Source reported in readme for each dataset.
+
+# Media
+* RPL Logo

LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2023 Daniel McGann
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

README.md

@@ -0,0 +1,75 @@
+# Robust Incremental Smoothing and Mapping (riSAM)
+
+ [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT)  &nbsp;[<img src="media/rpl.png" height=20px>](https://rpl.ri.cmu.edu/)
+
+This package contains the implementation of the robust incremental Smoothing and Mapping (riSAM) algorithm (see `risam/`) as presented in our ICRA 2023 paper. If you use this package please cite our paper:
+
+```
+@inproceedings{mcgann_risam_2023, 
+    title = {Robust Incremental Smoothing and Mapping ({riSAM})},
+    author = {D. McGann and J.G. Rogers III and M. Kaess},
+    fullauthor = {Daniel McGann and John G. Rogers III and Michael Kaess},
+    year = 2023,
+    booktitle = {Proc. IEEE Intl. Conf. on Robotics and Automation (ICRA)},
+    address = {London, {GB}}
+    pages = {?--?}, % Note: To be determined
+}
+```
+
+This paper can be accessed via [arXiv](https://arxiv.org/abs/2209.14359) and will soon be up on IEEE Explore.
+
+riSAM is able to handle large amounts of outlier measurements (tested up to 90% outliers)
+<p align="center">
+<img src="media/risam_solved_csail_90.jpg"
+    alt="riSAM correctly solves CSAIL Dataset even with very large numbers of outliers" 
+    width="70%"/>
+</p>
+
+More over riSAM is able to handle outlier measurements even with poor initialization. A situation in which prior works appear to struggle.
+<p align="center">
+<img src="media/prior_work_comparison_large_noise.png"
+    alt="riSAM correctly solves synthetic trajectories even with large noise" 
+    width="70%"/>
+</p>
+
+# Structure
+* `experiments`: Contains implementations of prior works, interface for running the experiments published in the riSAM paper, and scripts for working with datasets.
+* `risam`: Contains the implementation of the riSAM algorithm.
+
+Each of these subdirectories contains its own README with relevant information. If you want to get straight to running riSAM follow the installation directions below and then see the instructions in the `experiments` directory.
+
+# Installation
+
+First, one must setup dependencies. Unfortunately, there is a sensitivity to versioning due to the implementation of prior works These are detailed below.
+
+## Known issues / Building Notes
+* When using Kimera-RPGO w/ gtsam v4.1.1+ you may get a `std::out_of_range` in `std::map` from `gtsam::GaussianBayesTree::optimize`
+    * Similar Issues Reported with RTABMap (https://github.com/borglab/gtsam/issues/1092#issue-1126734835)
+    * [Fix](https://github.com/borglab/gtsam/pull/1158) uncluded in GTSAM pre-release [4.2a7+](https://github.com/borglab/gtsam/releases/tag/4.2a7)
+* Kimera-RPGO requires a number of gtsam compile time options to be set. See the Kimera-RPGO repo for details.
+* In older gtsam releases there was bug with the robust cost function loss in GTSAM. For details see the [issue](https://github.com/borglab/gtsam/issues/1129)
+    * [Fix](https://github.com/borglab/gtsam/pull/1161) included in GTSAM [4.2a8+](https://github.com/borglab/gtsam/releases/tag/4.2a8)
+
+## Building Instructions (Validated as of Mar 2023)
+* Version Summary (tested and confirmed with the following dependency versions)
+    * GTSAM: Tag=4.2a8, exact hash=9902ccc0a4f62123e91f057babe3612a95c15c20
+    * KimeraRPGO: exact hash=8c5e163ba38345ff583d87403ad53bf966c0221b
+    * dcsam: exact hash=b7f62295eec201fb00ee6d1d828fa551ac1f4bd7
+* These should be checked out when the git submodules are initialized, but are included here for completeness
+
+* GTSAM 
+    * Download [GTSAM version 4.2a8](https://github.com/borglab/gtsam/releases/tag/4.2a8)
+    * Setup compile time options [required by KimeraRPGO](https://github.com/MIT-SPARK/Kimera-RPGO)
+    * Build and optionally install GTSAM (Scripts assume GTSAM python is installed in the active python environment)
+* Clone riSAM and Submodules
+    * [HTTPS]: `git clone --recurse-submodules https://github.com/rpl-cmu/risam.git`
+    * [SSH]: `git clone --recurse-submodules git@github.com:rpl-cmu/risam.git`
+* Link GTSAM
+    * If you `install` GTSAM this should be automatic
+    * If you are working with a local build of GTSAM set `GTSAM_DIR` and `GTSAM_INCLUDE_DIR` to the appropriate directories.
+* Build riSAM and prior-works
+    * `cd risam`
+    * `mkdir build`
+    * `cd build`
+    * `cmake ..`
+    * `make`

experiments/.clang-format

@@ -0,0 +1,3 @@
+---
+BasedOnStyle: Google
+ColumnLimit: 120

experiments/CMakeLists.txt

@@ -0,0 +1,16 @@
+message(STATUS "================  EXPERIMENTS  ======================")
+
+# Prior Works for Comparison
+add_subdirectory(thirdparty)
+
+# Subprojects
+add_subdirectory(irl)
+add_subdirectory(exp_runner)
+
+# Extra Boost Libraries needed by experiments
+FIND_PACKAGE(Boost COMPONENTS program_options REQUIRED)
+INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
+
+# Declare and configure experiment runner executable
+add_executable(run-experiment "run-experiment.cpp")
+target_link_libraries(run-experiment gtsam ${Boost_LIBRARIES} exp_runner irl)

experiments/README.md

@@ -0,0 +1,107 @@
+# Experimental Validation
+
+This folder contains scripts, data and results used for experimental validation of riSAM and relative prior works.
+This file provides general documentation on the process, file formats, and how to use. 
+
+# Directory Structure
+* `dataset`: Contains benchmark SLAM datasets in g2o/toro format
+* `exp_runner`: Implements wrappers around all methods so that they all provide a standard interface
+* `scripts`: Contains a number of useful pythons scripts for manipulating datasets, evaluating results, and visualizing
+* `thirdparty`: Contains thirdparty code that implements prior works
+* `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`).
+
+Each subdirectory contains its own README with relevant information.
+
+# Experiment Runner
+The executable `run-experiment` is the main entry point to running riSAM and prior works. Its arguments are as follows:
+* `irl_file`, `i`: Path to Incremental Robot Log file. (see `experiments/irl/README.md`)
+* `method`, `m`: The name of the method to run (e.g. pseudo-gt, see `exp_runners/include/Factory.h` for more options).
+* `save_every_n`, `n`: Runner will save intermediate result files every N iterations.
+* `output_dir`, `o`: Directory to which the results will be saved.
+
+All options are required. This script will produce an output whos contents are specified in the next section. 
+
+## Experiment Runner Quirks
+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.
+
+# Quick Start Guide
+The following is an example work-flow to play with riSAM and the prior works included in this repo. In this we assume the working directory is `experiments`
+
+### 1. Corrupt a Gridworld dataset
+* First lets make a directory to store all of our data and results. 
+    * `mkdir ~/<some-path>/risam_test_environment`
+    * `mkdir ~/<some-path>/risam_test_environment/datasets`
+* Next lets corrupt a 2d dataset with outliers and convert to IRL format using the `g2o-2-irl` script
+    * `./scripts/g2o-2-irl -i datasets/csail/csail.g2o -n CSAIL -o ~/<some-path>/risam_test_environment/datasets/ --add_outliers --outlier_percentiles 10 --outlier_covariance 0.015 0.01 0.001 --outlier_chi2_thresh 0.95`
+    * `-i` is the input dataset, in this case CSAIL
+    * `-o` is the output directory for the datasets generated by the script
+    * `--add_outliers` Flags that we are adding outliers while converting the dataset
+    * `--outlier_percentiles` Are the percentiles of total loop closures that outliers should make up. If multiple are given the script generates multiple datasets. 
+    * `--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.
+    * `--outlier_chi2_thresh` specifies the chi-squared value that an outlier must have relative to the un-corrupted solution.
+    * For more options run the script with `--help`
+
+* You should now see `~/<some-path>/risam_test_environment/datasets/10` that contains one CSAIL dataset with 10% outliers.
+
+### 2. Run riSAM on the dataset
+* First lets make a directory to store the results
+    * `mkdir ~/<some-path>/risam_test_environment/results`
+* Now lets run riSAM
+    * `../build/experiments/run-experiment -i ~/<some-path>/risam_test_environment/datasets/10/CSAIL_10_random_0.irl -o ~/<some-path>/risam_test_environment/results/ -m risam -n 1`
+* We should now see a directory in results like `results/CSAIL_10_random_0_risam_YYYY-MM-DD_HH-MM-SS`
+* You can run prior works bu changing the `-m` ("method") option
+
+### 3. Plot our results
+* Lets visualize the results
+    * `./scripts/plot-traj -i ~/<some-path>/risam_test_environment/datasets/10/CSAIL_10_random_0.irl -r ~/<some-path>/risam_test_environment/results/CSAIL_10_random_0_risam_2023-03-09_14-28-37/ --legend`
+* You should see a plot of the final solution and print outs of the trajectory.
+* Further, lets animate the trajectory so we can see all the incremental steps
+    * `./scripts/animate-traj -i ~/<some-path>/risam_test_environment/datasets/10/CSAIL_10_random_0.irl -r ~/<some-path>/risam_test_environment/results/CSAIL_10_random_0_risam_2023-03-09_14-28-37/iterations/`
+
+# Prior Works
+
+Each entry the name is followed by common abbreviation then "Method Name" as defined in `exp_runner/include/exp_runner/Factor.h`
+
+
+### Graduated Non-Convexity (GNC, gnc-batch)
+```
+H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated non-
+convexity for robust spatial perception: From non-minimal solvers to
+global outlier rejection,” IEEE Robotics and Automation Letters (RA-
+L), vol. 5, no. 2, pp. 1127–1134, 2020.
+```
+Implementation provided in GTSAM
+
+### Pairwise Consistency Maximization (PCM, pcm)
+Note: different method names correspond to different hyper-parameters.
+```
+J. Mangelson, D. Dominic, R. Eustice, and R. Vasudevan, “Pairwise
+consistent measurement set maximization for robust multi-robot map
+merging,” in Proc. IEEE Intl. Conf. on Robotics and Automation
+(ICRA), Brisbane, AU, 2018, pp. 2916–2923.
+```
+Implementation provided in KimeraRPGO (`thirdparty/Kimera-RPGO`)
+
+### MaxMixture (MaxMix, maxmix)
+```
+E. Olson and P. Agarwal, “Inference on networks of mixtures for robust
+robot mapping,” Intl. J. of Robotics Research (IJRR), vol. 32, no. 7,
+pp. 826–840, 2013.
+```
+Implementation internal.
+
+### MEstimator (MEst, method name dependent on the M-Estimator used)
+```
+Z. Zhang, “Parameter estimation techniques: a tutorial with application
+to conic fitting,” Image and Vision Computing, vol. 15, no. 1, pp. 59–
+76, 1997.
+```
+Implementation baked into GTSAM.
+
+### Discrete Continuous Smoothing and Mapping (DC-SAM, dcsam)
+```
+K. Doherty, Z. Lu, K. Singh, and J. Leonard, “Discrete-continuous
+smoothing and mapping,” arXiv preprint arXiv:2204.11936v2 [cs],
+2022
+```
+Implementation provided by DC-SAM (`thirdparty/dcsam`)

experiments/datasets/README.md

@@ -0,0 +1,15 @@
+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.
+
+Datasets:
+* City1000 \[2d\] (also referred to as City10k) - Very long synthetic grid world dataset 
+* CSAIL \[2d\] - Short real-world dataset with very few loop closures
+* Cubicle \[3d\] (broken) - longer term 3d, but still largely planar dataset
+* Garage \[3d\] - longer real-world 3d dataset with interesting elevation changes, however, the noise models are very inaccurate to reality
+* Manhattan \[2d\] - mid length synthetic grid world dataset
+* rim \[3d\] (broken) - longer term real-world 3d dataset, like cubicle is largely planar 
+* sphere_a \[3d\] - Larger synthetic 3d dataset
+* sphere2500 \[3d\] - Larger synthetic 3d dataset, different (not sure how) from sphere_a
+
+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.
+
+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

@@ -0,0 +1,8 @@
+LOOPS: 10688
+
+This dataset is courtesy of Michael Kaess and was published along with iSAM2: 
+
+M. Kaess, H. Johannsson, R. Roberts, V. Ila, J.J. Leonard, and F. Dellaert
+"iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree",
+International Journal of Robotics Research,
+vol. 31, Feb. 2012, pp. 217-236.