diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..c680be6
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,4 @@
+build/
+.vscode/
+__pycache__/
+*.so
\ No newline at end of file
diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 0000000..3521305
--- /dev/null
+++ b/.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
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..f3d8b1e
--- /dev/null
+++ b/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)
diff --git a/DEPENDENCIES.md b/DEPENDENCIES.md
new file mode 100644
index 0000000..e84a674
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..23e907c
--- /dev/null
+++ b/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.
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..ec1e87e
--- /dev/null
+++ b/README.md
@@ -0,0 +1,75 @@
+# Robust Incremental Smoothing and Mapping (riSAM)
+
+ [](https://opensource.org/licenses/MIT) [
](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)
+
+
+
+
+More over riSAM is able to handle outlier measurements even with poor initialization. A situation in which prior works appear to struggle.
+
+
+
+
+# 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`
diff --git a/experiments/.clang-format b/experiments/.clang-format
new file mode 100644
index 0000000..454068e
--- /dev/null
+++ b/experiments/.clang-format
@@ -0,0 +1,3 @@
+---
+BasedOnStyle: Google
+ColumnLimit: 120
\ No newline at end of file
diff --git a/experiments/CMakeLists.txt b/experiments/CMakeLists.txt
new file mode 100644
index 0000000..fdc6851
--- /dev/null
+++ b/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)
\ No newline at end of file
diff --git a/experiments/README.md b/experiments/README.md
new file mode 100644
index 0000000..8d3fe87
--- /dev/null
+++ b/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 ~//risam_test_environment`
+ * `mkdir ~//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 ~//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 `~//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 ~//risam_test_environment/results`
+* Now lets run riSAM
+ * `../build/experiments/run-experiment -i ~//risam_test_environment/datasets/10/CSAIL_10_random_0.irl -o ~//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 ~//risam_test_environment/datasets/10/CSAIL_10_random_0.irl -r ~//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 ~//risam_test_environment/datasets/10/CSAIL_10_random_0.irl -r ~/