Merge branch 'develop' into fix/doxygen

2022-08-22 17:37:03 -04:00 · 2022-08-22 17:37:03 -04:00 · 1f6816d974
parent 150ddae41f 525e512f6e
commit 1f6816d974
86 changed files with 1668 additions and 2780 deletions
--- a/.github/scripts/python.sh
+++ b/.github/scripts/python.sh
@ -43,46 +43,68 @@ if [ -z ${PYTHON_VERSION+x} ]; then
    exit 127
 fi

-PYTHON="python${PYTHON_VERSION}"
+export PYTHON="python${PYTHON_VERSION}"

-if [[ $(uname) == "Darwin" ]]; then
+function install_dependencies()
+{
+  if [[ $(uname) == "Darwin" ]]; then
    brew install wget
-else
+  else
    # Install a system package required by our library
    sudo apt-get install -y wget libicu-dev python3-pip python3-setuptools
-fi
+  fi

-PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
+  export PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin

-[ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
+  [ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
+
+  $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
+}
+
+function build()
+{
+  mkdir $GITHUB_WORKSPACE/build
+  cd $GITHUB_WORKSPACE/build
+
+  BUILD_PYBIND="ON"
+
+  cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
+      -DGTSAM_BUILD_TESTS=OFF \
+      -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
+      -DGTSAM_USE_QUATERNIONS=OFF \
+      -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
+      -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
+      -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
+      -DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \
+      -DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \
+      -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
+      -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \
+      -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \
+      -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install


-BUILD_PYBIND="ON"
+  # Set to 2 cores so that Actions does not error out during resource provisioning.
+  make -j2 install

-sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
+  cd $GITHUB_WORKSPACE/build/python
+  $PYTHON -m pip install --user .
+}

-mkdir $GITHUB_WORKSPACE/build
-cd $GITHUB_WORKSPACE/build
+function test()
+{
+  cd $GITHUB_WORKSPACE/python/gtsam/tests
+  $PYTHON -m unittest discover -v
+}

-cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
-    -DGTSAM_BUILD_TESTS=OFF \
-    -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
-    -DGTSAM_USE_QUATERNIONS=OFF \
-    -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
-    -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-    -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-    -DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \
-    -DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \
-    -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
-    -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \
-    -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \
-    -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install
-
-
-# Set to 2 cores so that Actions does not error out during resource provisioning.
-make -j2 install
-
-cd $GITHUB_WORKSPACE/build/python
-$PYTHON -m pip install --user .
-cd $GITHUB_WORKSPACE/python/gtsam/tests
-$PYTHON -m unittest discover -v
+# select between build or test
+case $1 in
+  -d)
+    install_dependencies
+    ;;
+  -b)
+    build
+    ;;
+  -t)
+    test
+    ;;
+esac
--- a/.github/workflows/build-linux.yml
+++ b/.github/workflows/build-linux.yml
@ -20,26 +20,26 @@ jobs:
        # Github Actions requires a single row to be added to the build matrix.
        # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
        name: [
-          ubuntu-18.04-gcc-5,
-          ubuntu-18.04-gcc-9,
-          ubuntu-18.04-clang-9,
+          ubuntu-20.04-gcc-7,
+          ubuntu-20.04-gcc-9,
+          ubuntu-20.04-clang-9,
        ]

        build_type: [Debug, Release]
        build_unstable: [ON]
        include:
-          - name: ubuntu-18.04-gcc-5
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-gcc-7
+            os: ubuntu-20.04
            compiler: gcc
-            version: "5"
+            version: "7"

-          - name: ubuntu-18.04-gcc-9
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-gcc-9
+            os: ubuntu-20.04
            compiler: gcc
            version: "9"

-          - name: ubuntu-18.04-clang-9
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-clang-9
+            os: ubuntu-20.04
            compiler: clang
            version: "9"

@ -60,9 +60,9 @@ jobs:
            gpg -a --export $LLVM_KEY | sudo apt-key add -
            sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
          fi
-          sudo apt-get -y update

-          sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
+          sudo apt-get -y update
+          sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev

          if [ "${{ matrix.compiler }}" = "gcc" ]; then
            sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
--- a/.github/workflows/build-python.yml
+++ b/.github/workflows/build-python.yml
@ -19,34 +19,34 @@ jobs:
        # Github Actions requires a single row to be added to the build matrix.
        # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
        name: [
-          ubuntu-18.04-gcc-5,
-          ubuntu-18.04-gcc-9,
-          ubuntu-18.04-clang-9,
+          ubuntu-20.04-gcc-7,
+          ubuntu-20.04-gcc-9,
+          ubuntu-20.04-clang-9,
          macOS-11-xcode-13.4.1,
-          ubuntu-18.04-gcc-5-tbb,
+          ubuntu-20.04-gcc-7-tbb,
        ]

        build_type: [Debug, Release]
        python_version: [3]
        include:
-          - name: ubuntu-18.04-gcc-5
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-gcc-7
+            os: ubuntu-20.04
            compiler: gcc
-            version: "5"
+            version: "7"

-          - name: ubuntu-18.04-gcc-9
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-gcc-9
+            os: ubuntu-20.04
            compiler: gcc
            version: "9"

-          - name: ubuntu-18.04-clang-9
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-clang-9
+            os: ubuntu-20.04
            compiler: clang
            version: "9"

          # NOTE temporarily added this as it is a required check.
-          - name: ubuntu-18.04-clang-9
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-clang-9
+            os: ubuntu-20.04
            compiler: clang
            version: "9"
            build_type: Debug
@ -57,10 +57,10 @@ jobs:
            compiler: xcode
            version: "13.4.1"

-          - name: ubuntu-18.04-gcc-5-tbb
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-gcc-7-tbb
+            os: ubuntu-20.04
            compiler: gcc
-            version: "5"
+            version: "7"
            flag: tbb

    steps:
@ -79,9 +79,9 @@ jobs:
            gpg -a --export $LLVM_KEY | sudo apt-key add -
            sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
          fi
+
          sudo apt-get -y update
-          
-          sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
+          sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libboost-all-dev
          
          if [ "${{ matrix.compiler }}" = "gcc" ]; then
            sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
@ -117,11 +117,12 @@ jobs:
        uses: pierotofy/set-swap-space@master
        with:
          swap-size-gb: 6
-      - name: Build (Linux)
-        if: runner.os == 'Linux'
+      - name: Install Dependencies
        run: |
-          bash .github/scripts/python.sh
-      - name: Build (macOS)
-        if: runner.os == 'macOS'
+          bash .github/scripts/python.sh -d
+      - name: Build
        run: |
-          bash .github/scripts/python.sh
+          bash .github/scripts/python.sh -b
+      - name: Test
+        run: |
+          bash .github/scripts/python.sh -t
--- a/.github/workflows/build-special.yml
+++ b/.github/workflows/build-special.yml
@ -32,31 +32,31 @@ jobs:

        include:
          - name: ubuntu-gcc-deprecated
-            os: ubuntu-18.04
+            os: ubuntu-20.04
            compiler: gcc
            version: "9"
            flag: deprecated

          - name: ubuntu-gcc-quaternions
-            os: ubuntu-18.04
+            os: ubuntu-20.04
            compiler: gcc
            version: "9"
            flag: quaternions

          - name: ubuntu-gcc-tbb
-            os: ubuntu-18.04
+            os: ubuntu-20.04
            compiler: gcc
            version: "9"
            flag: tbb

          - name: ubuntu-cayleymap
-            os: ubuntu-18.04
+            os: ubuntu-20.04
            compiler: gcc
            version: "9"
            flag: cayley

          - name: ubuntu-system-libs
-            os: ubuntu-18.04
+            os: ubuntu-20.04
            compiler: gcc
            version: "9"
            flag: system-libs
@ -74,9 +74,9 @@ jobs:
            gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add -
            sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
          fi
-          sudo apt-get -y update

-          sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
+          sudo apt-get -y update
+          sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev

          if [ "${{ matrix.compiler }}" = "gcc" ]; then
            sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
--- a/README.md
+++ b/README.md
@ -64,6 +64,29 @@ GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionali

 We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md) wrappers for GTSAM. Please refer to the linked documents for more details.

+## Citation
+
+If you are using GTSAM for academic work, please use the following citation:
+
+```
+@software{gtsam,
+  author       = {Frank Dellaert and Richard Roberts and Varun Agrawal and Alex Cunningham and Chris Beall and Duy-Nguyen Ta and Fan Jiang and lucacarlone and nikai and Jose Luis Blanco-Claraco and Stephen Williams and ydjian and John Lambert and Andy Melim and Zhaoyang Lv and Akshay Krishnan and Jing Dong and Gerry Chen and Krunal Chande and balderdash-devil and DiffDecisionTrees and Sungtae An and mpaluri and Ellon Paiva Mendes and Mike Bosse and Akash Patel and Ayush Baid and Paul Furgale and matthewbroadwaynavenio and roderick-koehle},
+  title        = {borglab/gtsam},
+  month        = may,
+  year         = 2022,
+  publisher    = {Zenodo},
+  version      = {4.2a7},
+  doi          = {10.5281/zenodo.5794541},
+  url          = {https://doi.org/10.5281/zenodo.5794541}
+}
+```
+
+You can also get the latest citation available from Zenodo below:
+
+[![DOI](https://zenodo.org/badge/86362856.svg)](https://doi.org/10.5281/zenodo.5794541)
+
+Specific formats are available in the bottom-right corner of the Zenodo page.
+
 ## The Preintegrated IMU Factor

 GTSAM includes a state of the art IMU handling scheme based on
@ -73,7 +96,7 @@ GTSAM includes a state of the art IMU handling scheme based on
 Our implementation improves on this using integration on the manifold, as detailed in

 - Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, _"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors"_, Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483)
- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf)
+- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, _"IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation"_, Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf)

 If you are using the factor in academic work, please cite the publications above.

--- a/cmake/FindBoost.cmake
+++ b/cmake/FindBoost.cmake
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@ -190,7 +190,7 @@ if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
 endif()

 if (NOT MSVC)
-  option(GTSAM_BUILD_WITH_MARCH_NATIVE  "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" ON)
+  option(GTSAM_BUILD_WITH_MARCH_NATIVE  "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" OFF)
  if(GTSAM_BUILD_WITH_MARCH_NATIVE AND (APPLE AND NOT CMAKE_SYSTEM_PROCESSOR STREQUAL "arm64"))
    # Add as public flag so all dependant projects also use it, as required
    # by Eigen to avid crashes due to SIMD vectorization:
--- a/cmake/HandleEigen.cmake
+++ b/cmake/HandleEigen.cmake
@ -1,7 +1,10 @@
 ###############################################################################
 # Option for using system Eigen or GTSAM-bundled Eigen
-
-option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
+# Default: Use system's Eigen if found automatically:
+find_package(Eigen3 QUIET)
+set(USE_SYSTEM_EIGEN_INITIAL_VALUE ${Eigen3_FOUND})
+option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" ${USE_SYSTEM_EIGEN_INITIAL_VALUE})
+unset(USE_SYSTEM_EIGEN_INITIAL_VALUE)

 if(NOT GTSAM_USE_SYSTEM_EIGEN)
  # This option only makes sense if using the embedded copy of Eigen, it is
@ -11,7 +14,7 @@ endif()

 # Switch for using system Eigen or GTSAM-bundled Eigen
 if(GTSAM_USE_SYSTEM_EIGEN)
-    find_package(Eigen3 REQUIRED)
+    find_package(Eigen3 REQUIRED) # need to find again as REQUIRED

    # Use generic Eigen include paths e.g. <Eigen/Core>
    set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
--- a/cmake/HandlePrintConfiguration.cmake
+++ b/cmake/HandlePrintConfiguration.cmake
@ -33,6 +33,7 @@ print_build_options_for_target(gtsam)

 print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
 print_config("Use System Metis" "${GTSAM_USE_SYSTEM_METIS}")
+print_config("Using Boost version" "${Boost_VERSION}")

 if(GTSAM_USE_TBB)
    print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})")
--- a/doc/ImuFactor.lyx
+++ b/doc/ImuFactor.lyx
--- a/doc/ImuFactor.pdf
+++ b/doc/ImuFactor.pdf
--- a/doc/PreintegratedIMUJacobians.pdf
+++ b/doc/PreintegratedIMUJacobians.pdf
--- a/doc/refs.bib
+++ b/doc/refs.bib
@ -1,26 +1,72 @@
+%% This BibTeX bibliography file was created using BibDesk.
+%% https://bibdesk.sourceforge.io/
+
+%% Created for Varun Agrawal at 2021-09-27 17:39:09 -0400 
+
+
+%% Saved with string encoding Unicode (UTF-8) 
+
+
+
+@article{Lupton12tro,
+	author = {Lupton, Todd and Sukkarieh, Salah},
+	date-added = {2021-09-27 17:38:56 -0400},
+	date-modified = {2021-09-27 17:39:09 -0400},
+	doi = {10.1109/TRO.2011.2170332},
+	journal = {IEEE Transactions on Robotics},
+	number = {1},
+	pages = {61-76},
+	title = {Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions},
+	volume = {28},
+	year = {2012},
+	Bdsk-Url-1 = {https://doi.org/10.1109/TRO.2011.2170332}}
+
+@inproceedings{Forster15rss,
+	author = {Christian Forster and Luca Carlone and Frank Dellaert and Davide Scaramuzza},
+	booktitle = {Robotics: Science and Systems},
+	date-added = {2021-09-26 20:44:41 -0400},
+	date-modified = {2021-09-26 20:45:03 -0400},
+	title = {IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation},
+	year = {2015}}
+
@article{Iserles00an,
-  title =	 {Lie-group methods},
-  author =	 {Iserles, Arieh and Munthe-Kaas, Hans Z and
-                  N{\o}rsett, Syvert P and Zanna, Antonella},
-  journal =	 {Acta Numerica 2000},
-  volume =	 {9},
-  pages =	 {215--365},
-  year =	 {2000},
-  publisher =	 {Cambridge Univ Press}
-}
+	author = {Iserles, Arieh and Munthe-Kaas, Hans Z and N{\o}rsett, Syvert P and Zanna, Antonella},
+	journal = {Acta Numerica 2000},
+	pages = {215--365},
+	publisher = {Cambridge Univ Press},
+	title = {Lie-group methods},
+	volume = {9},
+	year = {2000}}

@book{Murray94book,
-  title =	 {A mathematical introduction to robotic manipulation},
-  author =	 {Murray, Richard M and Li, Zexiang and Sastry, S
-                  Shankar and Sastry, S Shankara},
-  year =	 {1994},
-  publisher =	 {CRC press}
-}
+	author = {Murray, Richard M and Li, Zexiang and Sastry, S Shankar and Sastry, S Shankara},
+	publisher = {CRC press},
+	title = {A mathematical introduction to robotic manipulation},
+	year = {1994}}

@book{Spivak65book,
-  title =	 {Calculus on manifolds},
-  author =	 {Spivak, Michael},
-  volume =	 {1},
-  year =	 {1965},
-  publisher =	 {WA Benjamin New York}
-}
+	author = {Spivak, Michael},
+	publisher = {WA Benjamin New York},
+	title = {Calculus on manifolds},
+	volume = {1},
+	year = {1965}}
+
+@phdthesis{Nikolic16thesis,
+  title={Characterisation, calibration, and design of visual-inertial sensor systems for robot navigation},
+  author={Nikolic, Janosch},
+  year={2016},
+  school={ETH Zurich}
+}
+
+@book{Simon06book,
+  title={Optimal state estimation: Kalman, H infinity, and nonlinear approaches},
+  author={Simon, Dan},
+  year={2006},
+  publisher={John Wiley \& Sons}
+}
+
+@inproceedings{Trawny05report_IndirectKF,
+  title={Indirect Kalman Filter for 3 D Attitude Estimation},
+  author={Nikolas Trawny and Stergios I. Roumeliotis},
+  year={2005}
+}
--- a/examples/CombinedImuFactorsExample.cpp
+++ b/examples/CombinedImuFactorsExample.cpp
@ -60,13 +60,14 @@ namespace po = boost::program_options;

 po::variables_map parseOptions(int argc, char* argv[]) {
  po::options_description desc;
-  desc.add_options()("help,h", "produce help message")(
-      "data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
-      "path to the CSV file with the IMU data")(
-      "output_filename",
-      po::value<string>()->default_value("imuFactorExampleResults.csv"),
-      "path to the result file to use")("use_isam", po::bool_switch(),
-                                        "use ISAM as the optimizer");
+  desc.add_options()("help,h", "produce help message")  // help message
+      ("data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
+       "path to the CSV file with the IMU data")  // path to the data file
+      ("output_filename",
+       po::value<string>()->default_value("imuFactorExampleResults.csv"),
+       "path to the result file to use")  // filename to save results to
+      ("use_isam", po::bool_switch(),
+       "use ISAM as the optimizer");  // flag for ISAM optimizer

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
@ -106,7 +107,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
      I_3x3 * 1e-8;  // error committed in integrating position from velocities
  Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
  Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
-  Matrix66 bias_acc_omega_int =
+  Matrix66 bias_acc_omega_init =
      I_6x6 * 1e-5;  // error in the bias used for preintegration

  auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
@ -122,7 +123,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
  // PreintegrationCombinedMeasurements params:
  p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
  p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
-  p->biasAccOmegaInt = bias_acc_omega_int;
+  p->biasAccOmegaInt = bias_acc_omega_init;

  return p;
 }
--- a/examples/ImuFactorsExample.cpp
+++ b/examples/ImuFactorsExample.cpp
@ -94,7 +94,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
      I_3x3 * 1e-8;  // error committed in integrating position from velocities
  Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
  Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
-  Matrix66 bias_acc_omega_int =
+  Matrix66 bias_acc_omega_init =
      I_6x6 * 1e-5;  // error in the bias used for preintegration

  auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
@ -110,7 +110,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
  // PreintegrationCombinedMeasurements params:
  p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
  p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
-  p->biasAccOmegaInt = bias_acc_omega_int;
+  p->biasAccOmegaInt = bias_acc_omega_init;

  return p;
 }
--- a/examples/README.md
+++ b/examples/README.md
@ -33,7 +33,6 @@ The following examples illustrate some concepts from Georgia Tech's research pap
 ## 2D Pose SLAM 

 * **LocalizationExample.cpp**: modeling robot motion
-* **LocalizationExample2.cpp**: example with GPS like measurements
 * **Pose2SLAMExample**: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
 * **Pose2SLAMExample_advanced**: same, but uses an Optimizer object
 * **Pose2SLAMwSPCG**: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
--- a/gtsam/base/Group.h
+++ b/gtsam/base/Group.h
@ -95,7 +95,7 @@ template<class Class>
 struct MultiplicativeGroupTraits {
  typedef group_tag structure_category;
  typedef multiplicative_group_tag group_flavor;
-  static Class Identity() { return Class::identity(); }
+  static Class Identity() { return Class::Identity(); }
  static Class Compose(const Class &g, const Class & h) { return g * h;}
  static Class Between(const Class &g, const Class & h) { return g.inverse() * h;}
  static Class Inverse(const Class &g) { return g.inverse();}
@ -111,7 +111,7 @@ template<class Class>
 struct AdditiveGroupTraits {
  typedef group_tag structure_category;
  typedef additive_group_tag group_flavor;
-  static Class Identity() { return Class::identity(); }
+  static Class Identity() { return Class::Identity(); }
  static Class Compose(const Class &g, const Class & h) { return g + h;}
  static Class Between(const Class &g, const Class & h) { return h - g;}
  static Class Inverse(const Class &g) { return -g;}
@ -147,7 +147,7 @@ public:
  DirectProduct(const G& g, const H& h):std::pair<G,H>(g,h) {}

  // identity
-  static DirectProduct identity() { return DirectProduct(); }
+  static DirectProduct Identity() { return DirectProduct(); }

  DirectProduct operator*(const DirectProduct& other) const {
    return DirectProduct(traits<G>::Compose(this->first, other.first),
@ -181,7 +181,7 @@ public:
  DirectSum(const G& g, const H& h):std::pair<G,H>(g,h) {}

  // identity
-  static DirectSum identity() { return DirectSum(); }
+  static DirectSum Identity() { return DirectSum(); }

  DirectSum operator+(const DirectSum& other) const {
    return DirectSum(g()+other.g(), h()+other.h());
--- a/gtsam/base/Lie.h
+++ b/gtsam/base/Lie.h
@ -177,7 +177,7 @@ struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
  /// @name Group
  /// @{
  typedef multiplicative_group_tag group_flavor;
-  static Class Identity() { return Class::identity();}
+  static Class Identity() { return Class::Identity();}
  /// @}

  /// @name Manifold
--- a/gtsam/base/ProductLieGroup.h
+++ b/gtsam/base/ProductLieGroup.h
@ -48,7 +48,7 @@ public:
  /// @name Group
  /// @{
  typedef multiplicative_group_tag group_flavor;
-  static ProductLieGroup identity() {return ProductLieGroup();}
+  static ProductLieGroup Identity() {return ProductLieGroup();}

  ProductLieGroup operator*(const ProductLieGroup& other) const {
    return ProductLieGroup(traits<G>::Compose(this->first,other.first),
--- a/gtsam/base/Vector.h
+++ b/gtsam/base/Vector.h
@ -60,6 +60,7 @@ GTSAM_MAKE_VECTOR_DEFS(9)
 GTSAM_MAKE_VECTOR_DEFS(10)
 GTSAM_MAKE_VECTOR_DEFS(11)
 GTSAM_MAKE_VECTOR_DEFS(12)
+GTSAM_MAKE_VECTOR_DEFS(15)

 typedef Eigen::VectorBlock<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;
--- a/gtsam/base/VectorSpace.h
+++ b/gtsam/base/VectorSpace.h
@ -169,7 +169,7 @@ struct HasVectorSpacePrereqs {
  Vector v;

  BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) {
-    p = Class::identity();  // identity
+    p = Class::Identity();  // identity
    q = p + p;              // addition
    q = p - p;              // subtraction
    v = p.vector();         // conversion to vector
@ -192,7 +192,7 @@ struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
  /// @name Group
  /// @{
  typedef additive_group_tag group_flavor;
-  static Class Identity() { return Class::identity();}
+  static Class Identity() { return Class::Identity();}
  /// @}

  /// @name Manifold
--- a/gtsam/base/tests/testGroup.cpp
+++ b/gtsam/base/tests/testGroup.cpp
@ -29,7 +29,7 @@ class Symmetric: private Eigen::PermutationMatrix<N> {
      Eigen::PermutationMatrix<N>(P) {
  }
 public:
-  static Symmetric identity() { return Symmetric(); }
+  static Symmetric Identity() { return Symmetric(); }
  Symmetric() {
    Eigen::PermutationMatrix<N>::setIdentity();
  }
--- a/gtsam/basis/ParameterMatrix.h
+++ b/gtsam/basis/ParameterMatrix.h
@ -189,9 +189,9 @@ class ParameterMatrix {
   * NOTE: The size at compile time is unknown so this identity is zero
   * length and thus not valid.
   */
-  inline static ParameterMatrix identity() {
+  inline static ParameterMatrix Identity() {
    // throw std::runtime_error(
-    //     "ParameterMatrix::identity(): Don't use this function");
+    //     "ParameterMatrix::Identity(): Don't use this function");
    return ParameterMatrix(0);
  }

--- a/gtsam/basis/basis.i
+++ b/gtsam/basis/basis.i
@ -137,7 +137,7 @@ class FitBasis {
  static gtsam::GaussianFactorGraph::shared_ptr LinearGraph(
      const std::map<double, double>& sequence,
      const gtsam::noiseModel::Base* model, size_t N);
-  This::Parameters parameters() const;
+  gtsam::This::Parameters parameters() const;
 };

 }  // namespace gtsam
--- a/gtsam/basis/tests/testParameterMatrix.cpp
+++ b/gtsam/basis/tests/testParameterMatrix.cpp
@ -133,7 +133,7 @@ TEST(ParameterMatrix, VectorSpace) {
  // vector
  EXPECT(assert_equal(Vector::Ones(M * N), params.vector()));
  // identity
-  EXPECT(assert_equal(ParameterMatrix<M>::identity(),
+  EXPECT(assert_equal(ParameterMatrix<M>::Identity(),
                      ParameterMatrix<M>(Matrix::Zero(M, 0))));
 }

--- a/gtsam/geometry/Cyclic.h
+++ b/gtsam/geometry/Cyclic.h
@ -38,7 +38,7 @@ public:
  /// Default constructor yields identity
  Cyclic():i_(0) {
  }
-  static Cyclic identity() { return Cyclic();}
+  static Cyclic Identity() { return Cyclic();}

  /// Cast to size_t
  operator size_t() const {
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@ -213,7 +213,7 @@ public:
  }

  /// for Canonical
-  static PinholeCamera identity() {
+  static PinholeCamera Identity() {
    return PinholeCamera(); // assumes that the default constructor is valid
  }

--- a/gtsam/geometry/PinholePose.h
+++ b/gtsam/geometry/PinholePose.h
@ -412,7 +412,7 @@ public:
  }

  /// for Canonical
-  static PinholePose identity() {
+  static PinholePose Identity() {
    return PinholePose(); // assumes that the default constructor is valid
  }

--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@ -119,7 +119,7 @@ public:
  /// @{

  /// identity for group operation
-  inline static Pose2 identity() { return Pose2(); }
+  inline static Pose2 Identity() { return Pose2(); }

  /// inverse
  GTSAM_EXPORT Pose2 inverse() const;
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@ -103,7 +103,7 @@ public:
  /// @{

  /// identity for group operation
-  static Pose3 identity() {
+  static Pose3 Identity() {
    return Pose3();
  }

--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@ -107,8 +107,8 @@ namespace gtsam {
    /// @name Group
    /// @{

-    /** identity */
-    inline static Rot2 identity() {  return Rot2(); }
+    /** Identity */
+    inline static Rot2 Identity() {  return Rot2(); }

    /** The inverse rotation - negative angle */
    Rot2 inverse() const { return Rot2(c_, -s_);}
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@ -297,7 +297,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
    /// @{

    /// identity rotation for group operation
-    inline static Rot3 identity() {
+    inline static Rot3 Identity() {
      return Rot3();
    }

--- a/gtsam/geometry/SOn.h
+++ b/gtsam/geometry/SOn.h
@ -179,13 +179,13 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {

  /// SO<N> identity for N >= 2
  template <int N_ = N, typename = IsFixed<N_>>
-  static SO identity() {
+  static SO Identity() {
    return SO();
  }

  /// SO<N> identity for N == Eigen::Dynamic
  template <int N_ = N, typename = IsDynamic<N_>>
-  static SO identity(size_t n = 0) {
+  static SO Identity(size_t n = 0) {
    return SO(n);
  }

--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@ -134,7 +134,7 @@ void Similarity2::print(const std::string& s) const {
            << std::endl;
 }

-Similarity2 Similarity2::identity() { return Similarity2(); }
+Similarity2 Similarity2::Identity() { return Similarity2(); }

 Similarity2 Similarity2::operator*(const Similarity2& S) const {
  return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
--- a/gtsam/geometry/Similarity2.h
+++ b/gtsam/geometry/Similarity2.h
@ -83,7 +83,7 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
  /// @{

  /// Return an identity transform
-  static Similarity2 identity();
+  static Similarity2 Identity();

  /// Composition
  Similarity2 operator*(const Similarity2& S) const;
--- a/gtsam/geometry/Similarity3.cpp
+++ b/gtsam/geometry/Similarity3.cpp
@ -122,7 +122,7 @@ void Similarity3::print(const std::string& s) const {
  std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl;
 }

-Similarity3 Similarity3::identity() {
+Similarity3 Similarity3::Identity() {
  return Similarity3();
 }
 Similarity3 Similarity3::operator*(const Similarity3& S) const {
--- a/gtsam/geometry/Similarity3.h
+++ b/gtsam/geometry/Similarity3.h
@ -84,7 +84,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
  /// @{

  /// Return an identity transform
-  static Similarity3 identity();
+  static Similarity3 Identity();

  /// Composition
  Similarity3 operator*(const Similarity3& S) const;
--- a/gtsam/geometry/SphericalCamera.h
+++ b/gtsam/geometry/SphericalCamera.h
@ -87,7 +87,7 @@ class GTSAM_EXPORT SphericalCamera {

  /// Default constructor
  SphericalCamera()
-      : pose_(Pose3::identity()), emptyCal_(boost::make_shared<EmptyCal>()) {}
+      : pose_(Pose3()), emptyCal_(boost::make_shared<EmptyCal>()) {}

  /// Constructor with pose
  explicit SphericalCamera(const Pose3& pose)
@ -197,9 +197,9 @@ class GTSAM_EXPORT SphericalCamera {
  }

  /// for Canonical
-  static SphericalCamera identity() {
+  static SphericalCamera Identity() {
    return SphericalCamera(
-        Pose3::identity());  // assumes that the default constructor is valid
+        Pose3::Identity());  // assumes that the default constructor is valid
  }

  /// for Linear Triangulation
--- a/gtsam/geometry/StereoPoint2.h
+++ b/gtsam/geometry/StereoPoint2.h
@ -71,7 +71,7 @@ public:
  /// @{

  /// identity
-  inline static StereoPoint2 identity() {
+  inline static StereoPoint2 Identity() {
    return StereoPoint2();
  }

--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@ -16,7 +16,7 @@ class Point2 {
  bool equals(const gtsam::Point2& point, double tol) const;

  // Group
-  static gtsam::Point2 identity();
+  static gtsam::Point2 Identity();

  // Standard Interface
  double x() const;
@ -73,7 +73,7 @@ class StereoPoint2 {
  bool equals(const gtsam::StereoPoint2& point, double tol) const;

  // Group
-  static gtsam::StereoPoint2 identity();
+  static gtsam::StereoPoint2 Identity();
  gtsam::StereoPoint2 inverse() const;
  gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const;
  gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const;
@ -115,7 +115,7 @@ class Point3 {
  bool equals(const gtsam::Point3& p, double tol) const;

  // Group
-  static gtsam::Point3 identity();
+  static gtsam::Point3 Identity();

  // Standard Interface
  Vector vector() const;
@ -149,7 +149,7 @@ class Rot2 {
  bool equals(const gtsam::Rot2& rot, double tol) const;

  // Group
-  static gtsam::Rot2 identity();
+  static gtsam::Rot2 Identity();
  gtsam::Rot2 inverse();
  gtsam::Rot2 compose(const gtsam::Rot2& p2) const;
  gtsam::Rot2 between(const gtsam::Rot2& p2) const;
@ -198,7 +198,7 @@ class SO3 {
  bool equals(const gtsam::SO3& other, double tol) const;

  // Group
-  static gtsam::SO3 identity();
+  static gtsam::SO3 Identity();
  gtsam::SO3 inverse() const;
  gtsam::SO3 between(const gtsam::SO3& R) const;
  gtsam::SO3 compose(const gtsam::SO3& R) const;
@ -228,7 +228,7 @@ class SO4 {
  bool equals(const gtsam::SO4& other, double tol) const;

  // Group
-  static gtsam::SO4 identity();
+  static gtsam::SO4 Identity();
  gtsam::SO4 inverse() const;
  gtsam::SO4 between(const gtsam::SO4& Q) const;
  gtsam::SO4 compose(const gtsam::SO4& Q) const;
@ -258,7 +258,7 @@ class SOn {
  bool equals(const gtsam::SOn& other, double tol) const;

  // Group
-  static gtsam::SOn identity();
+  static gtsam::SOn Identity();
  gtsam::SOn inverse() const;
  gtsam::SOn between(const gtsam::SOn& Q) const;
  gtsam::SOn compose(const gtsam::SOn& Q) const;
@ -322,7 +322,7 @@ class Rot3 {
  bool equals(const gtsam::Rot3& rot, double tol) const;

  // Group
-  static gtsam::Rot3 identity();
+  static gtsam::Rot3 Identity();
  gtsam::Rot3 inverse() const;
  gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
  gtsam::Rot3 between(const gtsam::Rot3& p2) const;
@ -380,7 +380,7 @@ class Pose2 {
  bool equals(const gtsam::Pose2& pose, double tol) const;

  // Group
-  static gtsam::Pose2 identity();
+  static gtsam::Pose2 Identity();
  gtsam::Pose2 inverse() const;
  gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
  gtsam::Pose2 between(const gtsam::Pose2& p2) const;
@ -444,7 +444,7 @@ class Pose3 {
  bool equals(const gtsam::Pose3& pose, double tol) const;

  // Group
-  static gtsam::Pose3 identity();
+  static gtsam::Pose3 Identity();
  gtsam::Pose3 inverse() const;
  gtsam::Pose3 inverse(Eigen::Ref<Eigen::MatrixXd> H) const;
  gtsam::Pose3 compose(const gtsam::Pose3& pose) const;
@ -1126,10 +1126,10 @@ class TriangulationResult {
  Status status;
  TriangulationResult(const gtsam::Point3& p);
  const gtsam::Point3& get() const;
-  static TriangulationResult Degenerate();
-  static TriangulationResult Outlier();
-  static TriangulationResult FarPoint();
-  static TriangulationResult BehindCamera();
+  static gtsam::TriangulationResult Degenerate();
+  static gtsam::TriangulationResult Outlier();
+  static gtsam::TriangulationResult FarPoint();
+  static gtsam::TriangulationResult BehindCamera();
  bool valid() const;
  bool degenerate() const;
  bool outlier() const;
--- a/gtsam/geometry/tests/testPose2.cpp
+++ b/gtsam/geometry/tests/testPose2.cpp
@ -902,7 +902,7 @@ TEST(Pose2 , TransformCovariance3) {

 /* ************************************************************************* */
 TEST(Pose2, Print) {
-  Pose2 pose(Rot2::identity(), Point2(1, 2));
+  Pose2 pose(Rot2::Identity(), Point2(1, 2));

  // Generate the expected output
  string s = "Planar Pose";
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@ -1133,7 +1133,7 @@ Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { retu

 TEST(Pose3, interpolateJacobians) {
  {
-    Pose3 X = Pose3::identity();
+    Pose3 X = Pose3::Identity();
    Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
    double t = 0.5;
    Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation
@ -1147,10 +1147,10 @@ TEST(Pose3, interpolateJacobians) {
    EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
  }
  {
-    Pose3 X = Pose3::identity();
-    Pose3 Y(Rot3::identity(), Point3(1, 0, 0));
+    Pose3 X = Pose3::Identity();
+    Pose3 Y(Rot3::Identity(), Point3(1, 0, 0));
    double t = 0.3;
-    Pose3 expectedPoseInterp(Rot3::identity(), Point3(0.3, 0, 0));
+    Pose3 expectedPoseInterp(Rot3::Identity(), Point3(0.3, 0, 0));
    Matrix actualJacobianX, actualJacobianY;
    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));

@ -1161,7 +1161,7 @@ TEST(Pose3, interpolateJacobians) {
    EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
  }
  {
-    Pose3 X = Pose3::identity();
+    Pose3 X = Pose3::Identity();
    Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0));
    double t = 0.5;
    Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
@ -1204,7 +1204,7 @@ TEST(Pose3, Create) {

 /* ************************************************************************* */
 TEST(Pose3, Print) {
-  Pose3 pose(Rot3::identity(), Point3(1, 2, 3));
+  Pose3 pose(Rot3::Identity(), Point3(1, 2, 3));

  // Generate the expected output
  std::string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 1 2 3\n";
--- a/gtsam/geometry/tests/testSimilarity3.cpp
+++ b/gtsam/geometry/tests/testSimilarity3.cpp
@ -203,11 +203,11 @@ TEST(Similarity3, ExpLogMap) {

  Vector7 zeros;
  zeros << 0, 0, 0, 0, 0, 0, 0;
-  Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity());
+  Vector7 logIdentity = Similarity3::Logmap(Similarity3::Identity());
  EXPECT(assert_equal(zeros, logIdentity));

  Similarity3 expZero = Similarity3::Expmap(zeros);
-  Similarity3 ident = Similarity3::identity();
+  Similarity3 ident = Similarity3::Identity();
  EXPECT(assert_equal(expZero, ident));

  // Compare to matrix exponential, using expm in Lie.h
@ -391,7 +391,7 @@ TEST(Similarity3, Optimization) {
  NonlinearFactorGraph graph;
  graph.addPrior(key, prior, model);

-  // Create initial estimate with identity transform
+  // Create initial estimate with Identity transform
  Values initial;
  initial.insert<Similarity3>(key, Similarity3());

--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@ -66,27 +66,6 @@ class KeySet {
  void serialize() const;
 };

-// Actually a vector<Key>
-class KeyVector {
-  KeyVector();
-  KeyVector(const gtsam::KeyVector& other);
-
-  // Note: no print function
-
-  // common STL methods
-  size_t size() const;
-  bool empty() const;
-  void clear();
-
-  // structure specific methods
-  size_t at(size_t i) const;
-  size_t front() const;
-  size_t back() const;
-  void push_back(size_t key) const;
-
-  void serialize() const;
-};
-
 // Actually a FastMap<Key,int>
 class KeyGroupMap {
  KeyGroupMap();
--- a/gtsam/navigation/CombinedImuFactor.cpp
+++ b/gtsam/navigation/CombinedImuFactor.cpp
@ -93,9 +93,14 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
 //------------------------------------------------------------------------------
 void PreintegratedCombinedMeasurements::integrateMeasurement(
    const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
+  if (dt <= 0) {
+    throw std::runtime_error(
+        "PreintegratedCombinedMeasurements::integrateMeasurement: dt <=0");
+  }
+
  // Update preintegrated measurements.
-  Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
-  Matrix93 B, C;
+  Matrix9 A; // Jacobian wrt preintegrated measurements without bias (df/dx)
+  Matrix93 B, C;  // Jacobian of state wrpt accel bias and omega bias respectively.
  PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);

  // Update preintegrated measurements covariance: as in [2] we consider a first
@ -105,47 +110,78 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
  // and preintegrated measurements

  // Single Jacobians to propagate covariance
-  // TODO(frank): should we not also account for bias on position?
-  Matrix3 theta_H_biasOmega = -C.topRows<3>();
-  Matrix3 vel_H_biasAcc = -B.bottomRows<3>();
+  Matrix3 theta_H_biasOmega = C.topRows<3>();
+  Matrix3 pos_H_biasAcc = B.middleRows<3>(3);
+  Matrix3 vel_H_biasAcc = B.bottomRows<3>();
+
+  Matrix3 theta_H_biasOmegaInit = -theta_H_biasOmega;
+  Matrix3 pos_H_biasAccInit = -pos_H_biasAcc;
+  Matrix3 vel_H_biasAccInit = -vel_H_biasAcc;

  // overall Jacobian wrt preintegrated measurements (df/dx)
  Eigen::Matrix<double, 15, 15> F;
  F.setZero();
  F.block<9, 9>(0, 0) = A;
  F.block<3, 3>(0, 12) = theta_H_biasOmega;
+  F.block<3, 3>(3, 9) = pos_H_biasAcc;
  F.block<3, 3>(6, 9) = vel_H_biasAcc;
  F.block<6, 6>(9, 9) = I_6x6;

+  // Update the uncertainty on the state (matrix F in [4]).
+  preintMeasCov_ = F * preintMeasCov_ * F.transpose();
+
  // propagate uncertainty
  // TODO(frank): use noiseModel routine so we can have arbitrary noise models.
  const Matrix3& aCov = p().accelerometerCovariance;
  const Matrix3& wCov = p().gyroscopeCovariance;
  const Matrix3& iCov = p().integrationCovariance;
+  const Matrix6& bInitCov = p().biasAccOmegaInt;

  // first order uncertainty propagation
-  // Optimized matrix multiplication   (1/dt) * G * measurementCovariance *
-  // G.transpose()
+  // Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose()
  Eigen::Matrix<double, 15, 15> G_measCov_Gt;
  G_measCov_Gt.setZero(15, 15);

+  const Matrix3& bInitCov11 = bInitCov.block<3, 3>(0, 0) / dt;
+  const Matrix3& bInitCov12 = bInitCov.block<3, 3>(0, 3) / dt;
+  const Matrix3& bInitCov21 = bInitCov.block<3, 3>(3, 0) / dt;
+  const Matrix3& bInitCov22 = bInitCov.block<3, 3>(3, 3) / dt;
+
  // BLOCK DIAGONAL TERMS
-  D_t_t(&G_measCov_Gt) = dt * iCov;
-  D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
-      * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0))
-      * (vel_H_biasAcc.transpose());
-  D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
-      * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
-      * (theta_H_biasOmega.transpose());
+  D_R_R(&G_measCov_Gt) =
+      (theta_H_biasOmega * (wCov / dt) * theta_H_biasOmega.transpose())  //
+      +
+      (theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose());
+
+  D_t_t(&G_measCov_Gt) =
+      (pos_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose())           //
+      + (pos_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose())  //
+      + (dt * iCov);
+
+  D_v_v(&G_measCov_Gt) =
+      (vel_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose())  //
+      + (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
+
  D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
  D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;

  // OFF BLOCK DIAGONAL TERMS
-  Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
-      * theta_H_biasOmega.transpose();
-  D_v_R(&G_measCov_Gt) = temp;
-  D_R_v(&G_measCov_Gt) = temp.transpose();
-  preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
+  D_R_t(&G_measCov_Gt) =
+      theta_H_biasOmegaInit * bInitCov21 * pos_H_biasAccInit.transpose();
+  D_R_v(&G_measCov_Gt) =
+      theta_H_biasOmegaInit * bInitCov21 * vel_H_biasAccInit.transpose();
+  D_t_R(&G_measCov_Gt) =
+      pos_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
+  D_t_v(&G_measCov_Gt) =
+      (pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) +
+      (pos_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
+  D_v_R(&G_measCov_Gt) =
+      vel_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
+  D_v_t(&G_measCov_Gt) =
+      (vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) +
+      (vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose());
+
+  preintMeasCov_.noalias() += G_measCov_Gt;
 }

 //------------------------------------------------------------------------------
@ -253,6 +289,5 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
  os << "  noise model sigmas: " << f.noiseModel_->sigmas().transpose();
  return os;
 }
-}
- /// namespace gtsam

+}  // namespace gtsam
--- a/gtsam/navigation/CombinedImuFactor.h
+++ b/gtsam/navigation/CombinedImuFactor.h
@ -51,6 +51,7 @@ typedef ManifoldPreintegration PreintegrationType;
 *     TRO, 28(1):61-76, 2012.
 * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
 *     Computation of the Jacobian Matrices", Tech. Report, 2013.
+ *     Available in this repo as "PreintegratedIMUJacobians.pdf".
 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
 *     Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
 *     Robotics: Science and Systems (RSS), 2015.
@ -61,7 +62,7 @@ typedef ManifoldPreintegration PreintegrationType;
 struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
  Matrix3 biasAccCovariance;    ///< continuous-time "Covariance" describing accelerometer bias random walk
  Matrix3 biasOmegaCovariance;  ///< continuous-time "Covariance" describing gyroscope bias random walk
-  Matrix6 biasAccOmegaInt;     ///< covariance of bias used for pre-integration
+  Matrix6 biasAccOmegaInt;     ///< covariance of bias used as initial estimate.

  /// Default constructor makes uninitialized params struct.
  /// Used for serialization.
@ -92,11 +93,11 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {

  void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
  void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
-  void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
+  void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; }
  
  const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
  const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
-  const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
+  const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
  
 private:

--- a/gtsam/navigation/ImuBias.h
+++ b/gtsam/navigation/ImuBias.h
@ -109,7 +109,7 @@ public:
  /// @{

  /** identity for group operation */
-  static ConstantBias identity() {
+  static ConstantBias Identity() {
    return ConstantBias();
  }

--- a/gtsam/navigation/ImuFactor.cpp
+++ b/gtsam/navigation/ImuFactor.cpp
@ -59,7 +59,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(

  // Update preintegrated measurements (also get Jacobian)
  Matrix9 A;  // overall Jacobian wrt preintegrated measurements (df/dx)
-  Matrix93 B, C;
+  Matrix93 B, C;  // Jacobian of state wrpt accel bias and omega bias respectively.
  PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);

  // first order covariance propagation:
@ -73,11 +73,13 @@ void PreintegratedImuMeasurements::integrateMeasurement(
  const Matrix3& iCov = p().integrationCovariance;

  // (1/dt) allows to pass from continuous time noise to discrete time noise
+  // Update the uncertainty on the state (matrix A in [4]).
  preintMeasCov_ = A * preintMeasCov_ * A.transpose();
+  // These 2 updates account for uncertainty on the IMU measurement (matrix B in [4]).
  preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose();
  preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();

-  // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3
+  // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 (9x3 matrix)
  preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt;
 }

--- a/gtsam/navigation/ImuFactor.h
+++ b/gtsam/navigation/ImuFactor.h
@ -53,6 +53,7 @@ typedef ManifoldPreintegration PreintegrationType;
 *     TRO, 28(1):61-76, 2012.
 * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
 *     Computation of the Jacobian Matrices", Tech. Report, 2013.
+ *     Available in this repo as "PreintegratedIMUJacobians.pdf".
 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
 *     Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
 *     Robotics: Science and Systems (RSS), 2015.
--- a/gtsam/navigation/PreintegrationBase.cpp
+++ b/gtsam/navigation/PreintegrationBase.cpp
@ -157,9 +157,9 @@ Vector9 PreintegrationBase::computeError(const NavState& state_i,
      state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0,
                               H1 || H3 ? &D_error_predict : 0);

-  if (H1) *H1 << D_error_predict* D_predict_state_i;
+  if (H1) *H1 << D_error_predict * D_predict_state_i;
  if (H2) *H2 << D_error_state_j;
-  if (H3) *H3 << D_error_predict* D_predict_bias_i;
+  if (H3) *H3 << D_error_predict * D_predict_bias_i;

  return error;
 }
--- a/gtsam/navigation/ScenarioRunner.cpp
+++ b/gtsam/navigation/ScenarioRunner.cpp
@ -15,8 +15,8 @@
 * @author  Frank Dellaert
 */

-#include <gtsam/navigation/ScenarioRunner.h>
 #include <gtsam/base/timing.h>
+#include <gtsam/navigation/ScenarioRunner.h>

 #include <boost/assign.hpp>
 #include <cmath>
@ -105,4 +105,62 @@ Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
  return Q / (N - 1);
 }

+PreintegratedCombinedMeasurements CombinedScenarioRunner::integrate(
+    double T, const Bias& estimatedBias, bool corrupted) const {
+  gttic_(integrate);
+  PreintegratedCombinedMeasurements pim(p_, estimatedBias);
+
+  const double dt = imuSampleTime();
+  const size_t nrSteps = T / dt;
+  double t = 0;
+  for (size_t k = 0; k < nrSteps; k++, t += dt) {
+    Vector3 measuredOmega =
+        corrupted ? measuredAngularVelocity(t) : actualAngularVelocity(t);
+    Vector3 measuredAcc =
+        corrupted ? measuredSpecificForce(t) : actualSpecificForce(t);
+    pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
+  }
+
+  return pim;
+}
+
+NavState CombinedScenarioRunner::predict(
+    const PreintegratedCombinedMeasurements& pim,
+    const Bias& estimatedBias) const {
+  const NavState state_i(scenario().pose(0), scenario().velocity_n(0));
+  return pim.predict(state_i, estimatedBias);
+}
+
+Eigen::Matrix<double, 15, 15> CombinedScenarioRunner::estimateCovariance(
+    double T, size_t N, const Bias& estimatedBias) const {
+  gttic_(estimateCovariance);
+
+  // Get predict prediction from ground truth measurements
+  NavState prediction = predict(integrate(T));
+
+  // Sample !
+  Matrix samples(15, N);
+  Vector15 sum = Vector15::Zero();
+  for (size_t i = 0; i < N; i++) {
+    auto pim = integrate(T, estimatedBias, true);
+    NavState sampled = predict(pim);
+    Vector15 xi = Vector15::Zero();
+    xi << sampled.localCoordinates(prediction),
+        (estimatedBias_.vector() - estimatedBias.vector());
+    samples.col(i) = xi;
+    sum += xi;
+  }
+
+  // Compute MC covariance
+  Vector15 sampleMean = sum / N;
+  Eigen::Matrix<double, 15, 15> Q;
+  Q.setZero();
+  for (size_t i = 0; i < N; i++) {
+    Vector15 xi = samples.col(i) - sampleMean;
+    Q += xi * xi.transpose();
+  }
+
+  return Q / (N - 1);
+}
+
 }  // namespace gtsam
--- a/gtsam/navigation/ScenarioRunner.h
+++ b/gtsam/navigation/ScenarioRunner.h
@ -16,9 +16,10 @@
 */

 #pragma once
+#include <gtsam/linear/Sampler.h>
+#include <gtsam/navigation/CombinedImuFactor.h>
 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/navigation/Scenario.h>
-#include <gtsam/linear/Sampler.h>

 namespace gtsam {

@ -66,10 +67,10 @@ class GTSAM_EXPORT ScenarioRunner {
  // also, uses g=10 for easy debugging
  const Vector3& gravity_n() const { return p_->n_gravity; }

+  const Scenario& scenario() const { return scenario_; }
+
  // A gyro simply measures angular velocity in body frame
-  Vector3 actualAngularVelocity(double t) const {
-    return scenario_.omega_b(t);
-  }
+  Vector3 actualAngularVelocity(double t) const { return scenario_.omega_b(t); }

  // An accelerometer measures acceleration in body, but not gravity
  Vector3 actualSpecificForce(double t) const {
@ -106,4 +107,39 @@ class GTSAM_EXPORT ScenarioRunner {
  Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
 };

+/*
+ *  Simple class to test navigation scenarios with CombinedImuMeasurements.
+ *  Takes a trajectory scenario as input, and can generate IMU measurements
+ */
+class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
+ public:
+  typedef boost::shared_ptr<PreintegrationCombinedParams> SharedParams;
+
+ private:
+  const SharedParams p_;
+  const Bias estimatedBias_;
+
+ public:
+  CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
+                         double imuSampleTime = 1.0 / 100.0,
+                         const Bias& bias = Bias())
+      : ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
+                       imuSampleTime, bias),
+        p_(p),
+        estimatedBias_(bias) {}
+
+  /// Integrate measurements for T seconds into a PIM
+  PreintegratedCombinedMeasurements integrate(
+      double T, const Bias& estimatedBias = Bias(),
+      bool corrupted = false) const;
+
+  /// Predict predict given a PIM
+  NavState predict(const PreintegratedCombinedMeasurements& pim,
+                   const Bias& estimatedBias = Bias()) const;
+
+  /// Compute a Monte Carlo estimate of the predict covariance using N samples
+  Eigen::Matrix<double, 15, 15> estimateCovariance(
+      double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const;
+};
+
 }  // namespace gtsam
--- a/gtsam/navigation/navigation.i
+++ b/gtsam/navigation/navigation.i
@ -17,7 +17,7 @@ class ConstantBias {
  bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const;

  // Group
-  static gtsam::imuBias::ConstantBias identity();
+  static gtsam::imuBias::ConstantBias Identity();

  // Operator Overloads
  gtsam::imuBias::ConstantBias operator-() const;
@ -165,11 +165,11 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {

  void setBiasAccCovariance(Matrix cov);
  void setBiasOmegaCovariance(Matrix cov);
-  void setBiasAccOmegaInt(Matrix cov);
+  void setBiasAccOmegaInit(Matrix cov);
  
  Matrix getBiasAccCovariance() const ;
  Matrix getBiasOmegaCovariance() const ;
-  Matrix getBiasAccOmegaInt() const;
+  Matrix getBiasAccOmegaInit() const;
 
 };

--- a/gtsam/navigation/tests/testCombinedImuFactor.cpp
+++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp
@ -16,18 +16,19 @@
 * @author  Frank Dellaert
 * @author  Richard Roberts
 * @author  Stephen Williams
+ * @author  Varun Agrawal
 */

-#include <gtsam/navigation/ImuFactor.h>
-#include <gtsam/navigation/CombinedImuFactor.h>
-#include <gtsam/navigation/ImuBias.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/inference/Symbol.h>
+#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
-
-#include <CppUnitLite/TestHarness.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/navigation/CombinedImuFactor.h>
+#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/navigation/ScenarioRunner.h>
+#include <gtsam/nonlinear/Values.h>

 #include <list>

@ -40,12 +41,15 @@ static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
  p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
  p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
  p->integrationCovariance = 0.0001 * I_3x3;
+  p->biasAccCovariance = Z_3x3;
+  p->biasOmegaCovariance = Z_3x3;
+  p->biasAccOmegaInt = Z_6x6;
  return p;
 }
-}
+}  // namespace testing

 /* ************************************************************************* */
-TEST( CombinedImuFactor, PreintegratedMeasurements ) {
+TEST(CombinedImuFactor, PreintegratedMeasurements ) {
  // Linearization point
  Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases

@ -71,8 +75,9 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
  DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
 }

+
 /* ************************************************************************* */
-TEST( CombinedImuFactor, ErrorWithBiases ) {
+TEST(CombinedImuFactor, ErrorWithBiases ) {
  Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
  Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
  Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
@ -203,6 +208,114 @@ TEST(CombinedImuFactor, PredictRotation) {
  EXPECT(assert_equal(expectedPose, actual.pose(), tol));
 }

+/* ************************************************************************* */
+// Testing covariance to check if all the jacobians are accounted for.
+TEST(CombinedImuFactor, CheckCovariance) {
+  auto params = PreintegrationCombinedParams::MakeSharedU(9.81);
+
+  params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
+  params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
+  params->setIntegrationCovariance(pow(0.0, 2) * I_3x3);
+  params->setOmegaCoriolis(Vector3::Zero());
+
+  imuBias::ConstantBias currentBias;
+
+  PreintegratedCombinedMeasurements actual(params, currentBias);
+
+  // Measurements
+  Vector3 measuredAcc(0.1577, -0.8251, 9.6111);
+  Vector3 measuredOmega(-0.0210, 0.0311, 0.0145);
+  double deltaT = 0.01;
+
+  actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
+
+  Eigen::Matrix<double, 15, 15> expected;
+  expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,          //
+      0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,                  //
+      0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,                  //
+      0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, 0,  //
+      0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0,  //
+      0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0,  //
+      0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0,     //
+      0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0,     //
+      0, 0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0,     //
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,                  //
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,                  //
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0,                  //
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,                  //
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,                  //
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01;
+
+  // regression
+  EXPECT(assert_equal(expected, actual.preintMeasCov()));
+}
+
+// Test that the covariance values for the ImuFactor and the CombinedImuFactor
+// (top-left 9x9) are the same
+TEST(CombinedImuFactor, SameCovariance) {
+  // IMU measurements and time delta
+  Vector3 accMeas(0.1577, -0.8251, 9.6111);
+  Vector3 omegaMeas(-0.0210, 0.0311, 0.0145);
+  double deltaT = 0.01;
+
+  // Assume zero bias
+  imuBias::ConstantBias currentBias;
+
+  // Define params for ImuFactor
+  auto params = PreintegrationParams::MakeSharedU();
+  params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
+  params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
+  params->setIntegrationCovariance(pow(0, 2) * I_3x3);
+  params->setOmegaCoriolis(Vector3::Zero());
+
+  // The IMU preintegration object for ImuFactor
+  PreintegratedImuMeasurements pim(params, currentBias);
+  pim.integrateMeasurement(accMeas, omegaMeas, deltaT);
+
+  // Define params for CombinedImuFactor
+  auto combined_params = PreintegrationCombinedParams::MakeSharedU();
+  combined_params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
+  combined_params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
+  // Set bias integration covariance explicitly to zero
+  combined_params->setIntegrationCovariance(Z_3x3);
+  combined_params->setOmegaCoriolis(Z_3x1);
+  // Set bias initial covariance explicitly to zero
+  combined_params->setBiasAccOmegaInit(Z_6x6);
+
+  // The IMU preintegration object for CombinedImuFactor
+  PreintegratedCombinedMeasurements cpim(combined_params, currentBias);
+  cpim.integrateMeasurement(accMeas, omegaMeas, deltaT);
+
+  // Assert if the noise covariance
+  EXPECT(assert_equal(pim.preintMeasCov(),
+                      cpim.preintMeasCov().block(0, 0, 9, 9)));
+}
+
+/* ************************************************************************* */
+TEST(CombinedImuFactor, Accelerating) {
+  const double a = 0.2, v = 50;
+
+  // Set up body pointing towards y axis, and start at 10,20,0 with velocity
+  // going in X The body itself has Z axis pointing down
+  const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
+  const Point3 initial_position(10, 20, 0);
+  const Vector3 initial_velocity(v, 0, 0);
+
+  const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
+                                      Vector3(a, 0, 0));
+
+  const double T = 3.0;  // seconds
+
+  CombinedScenarioRunner runner(scenario, testing::Params(), T / 10);
+
+  PreintegratedCombinedMeasurements pim = runner.integrate(T);
+  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
+
+  auto estimatedCov = runner.estimateCovariance(T, 100);
+  Eigen::Matrix<double, 15, 15> expected = pim.preintMeasCov();
+  EXPECT(assert_equal(estimatedCov, expected, 0.1));
+}
+
 /* ************************************************************************* */
 int main() {
  TestResult tr;
--- a/gtsam/nonlinear/CustomFactor.h
+++ b/gtsam/nonlinear/CustomFactor.h
@ -19,8 +19,6 @@

 #include <gtsam/nonlinear/NonlinearFactor.h>

-using namespace gtsam;
-
 namespace gtsam {

 using JacobianVector = std::vector<Matrix>;
--- a/gtsam/nonlinear/GncOptimizer.h
+++ b/gtsam/nonlinear/GncOptimizer.h
@ -42,7 +42,7 @@ static double Chi2inv(const double alpha, const size_t dofs) {

 /* ************************************************************************* */
 template<class GncParameters>
-class GTSAM_EXPORT GncOptimizer {
+class GncOptimizer {
 public:
  /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
  typedef typename GncParameters::OptimizerType BaseOptimizer;
--- a/gtsam/nonlinear/GncParams.h
+++ b/gtsam/nonlinear/GncParams.h
@ -39,7 +39,7 @@ enum GncLossType {
 };

 template<class BaseOptimizerParameters>
-class GTSAM_EXPORT GncParams {
+class GncParams {
 public:
  /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
  typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;
@ -72,8 +72,14 @@ class GTSAM_EXPORT GncParams {
  double relativeCostTol = 1e-5;  ///< If relative cost change is below this threshold, stop iterating
  double weightsTol = 1e-4;  ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
  Verbosity verbosity = SILENT;  ///< Verbosity level
-  std::vector<size_t> knownInliers = std::vector<size_t>();  ///< Slots in the factor graph corresponding to measurements that we know are inliers
-  std::vector<size_t> knownOutliers = std::vector<size_t>();  ///< Slots in the factor graph corresponding to measurements that we know are outliers
+
+  //TODO(Varun) replace IndexVector with vector<size_t> once pybind11/stl.h is globally enabled.
+  /// Use IndexVector for inliers and outliers since it is fast + wrapping
+  using IndexVector = FastVector<uint64_t>;
+  ///< Slots in the factor graph corresponding to measurements that we know are inliers
+  IndexVector knownInliers = IndexVector();
+  ///< Slots in the factor graph corresponding to measurements that we know are outliers
+  IndexVector knownOutliers = IndexVector();

  /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
  void setLossType(const GncLossType type) {
@ -114,7 +120,7 @@ class GTSAM_EXPORT GncParams {
   * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and
   * only apply GNC to prune outliers from the loop closures.
   * */
-  void setKnownInliers(const std::vector<size_t>& knownIn) {
+  void setKnownInliers(const IndexVector& knownIn) {
    for (size_t i = 0; i < knownIn.size(); i++){
      knownInliers.push_back(knownIn[i]);
    }
@ -125,7 +131,7 @@ class GTSAM_EXPORT GncParams {
   * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg,
   * and you provide  knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15].
   * */
-  void setKnownOutliers(const std::vector<size_t>& knownOut) {
+  void setKnownOutliers(const IndexVector& knownOut) {
    for (size_t i = 0; i < knownOut.size(); i++){
      knownOutliers.push_back(knownOut[i]);
    }
@ -163,7 +169,7 @@ class GTSAM_EXPORT GncParams {
      std::cout << "knownInliers: " << knownInliers[i] << "\n";
    for (size_t i = 0; i < knownOutliers.size(); i++)
      std::cout << "knownOutliers: " << knownOutliers[i] << "\n";
-    baseOptimizerParams.print(str);
+    baseOptimizerParams.print("Base optimizer params: ");
  }
 };

--- a/gtsam/nonlinear/NonlinearFactor.cpp
+++ b/gtsam/nonlinear/NonlinearFactor.cpp
@ -167,8 +167,9 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
    return GaussianFactor::shared_ptr(
        new JacobianFactor(terms, b,
            boost::static_pointer_cast<Constrained>(noiseModel_)->unit()));
-  else
+  else {
    return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
+  }
 }

 /* ************************************************************************* */
--- a/gtsam/nonlinear/custom.i
+++ b/gtsam/nonlinear/custom.i
@ -0,0 +1,38 @@
+//*************************************************************************
+// Custom Factor wrapping
+//*************************************************************************
+
+namespace gtsam {
+
+#include <gtsam/nonlinear/CustomFactor.h>
+virtual class CustomFactor : gtsam::NoiseModelFactor {
+  /*
+   * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting
+   * machinery there. This is achieved by adding `gtsam::CustomFactor` to the
+   * ignore list in `matlab/CMakeLists.txt`.
+   */
+  CustomFactor();
+  /*
+   * Example:
+   * ```
+   * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
+   *    <calculated error>
+   *    if not H is None:
+   *        <calculate the Jacobian>
+   *        H[0] = J1 # 2-d numpy array for a Jacobian block
+   *        H[1] = J2
+   *        ...
+   *    return error # 1-d numpy array
+   *
+   * cf = CustomFactor(noise_model, keys, error_func)
+   * ```
+   */
+  CustomFactor(const gtsam::SharedNoiseModel& noiseModel,
+               const gtsam::KeyVector& keys,
+               const gtsam::CustomErrorFunction& errorFunction);
+
+  void print(string s = "",
+             gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
+};
+
+}
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@ -99,11 +99,11 @@ class NonlinearFactorGraph {
  string dot(
      const gtsam::Values& values,
      const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
-      const GraphvizFormatting& writer = GraphvizFormatting());
+      const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting());
  void saveGraph(
      const string& s, const gtsam::Values& values,
      const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
-      const GraphvizFormatting& writer = GraphvizFormatting()) const;
+      const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting()) const;

  // enabling serialization functionality
  void serialize() const;
@ -135,37 +135,6 @@ virtual class NoiseModelFactor : gtsam::NonlinearFactor {
  Vector whitenedError(const gtsam::Values& x) const;
 };

-#include <gtsam/nonlinear/CustomFactor.h>
-virtual class CustomFactor : gtsam::NoiseModelFactor {
-  /*
-   * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting
-   * machinery there. This is achieved by adding `gtsam::CustomFactor` to the
-   * ignore list in `matlab/CMakeLists.txt`.
-   */
-  CustomFactor();
-  /*
-   * Example:
-   * ```
-   * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
-   *    <calculated error>
-   *    if not H is None:
-   *        <calculate the Jacobian>
-   *        H[0] = J1 # 2-d numpy array for a Jacobian block
-   *        H[1] = J2
-   *        ...
-   *    return error # 1-d numpy array
-   *
-   * cf = CustomFactor(noise_model, keys, error_func)
-   * ```
-   */
-  CustomFactor(const gtsam::SharedNoiseModel& noiseModel,
-               const gtsam::KeyVector& keys,
-               const gtsam::CustomErrorFunction& errorFunction);
-
-  void print(string s = "",
-             gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
-};
-
 #include <gtsam/nonlinear/Values.h>
 class Values {
  Values();
@ -544,12 +513,34 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams {
 };

 #include <gtsam/nonlinear/GncParams.h>
+enum GncLossType {
+  GM /*Geman McClure*/,
+  TLS /*Truncated least squares*/
+};
+
 template<PARAMS>
 virtual class GncParams {
  GncParams(const PARAMS& baseOptimizerParams);
  GncParams();
-  void setVerbosityGNC(const This::Verbosity value);
-  void print(const string& str) const;
+  BaseOptimizerParameters baseOptimizerParams;
+  gtsam::GncLossType lossType;
+  size_t maxIterations;
+  double muStep;
+  double relativeCostTol;
+  double weightsTol;
+  Verbosity verbosity;
+  gtsam::KeyVector knownInliers;
+  gtsam::KeyVector knownOutliers;
+
+  void setLossType(const gtsam::GncLossType type);
+  void setMaxIterations(const size_t maxIter);
+  void setMuStep(const double step);
+  void setRelativeCostTol(double value);
+  void setWeightsTol(double value);
+  void setVerbosityGNC(const gtsam::This::Verbosity value);
+  void setKnownInliers(const gtsam::KeyVector& knownIn);
+  void setKnownOutliers(const gtsam::KeyVector& knownOut);
+  void print(const string& str = "GncParams: ") const;
  
  enum Verbosity {
    SILENT,
@ -597,6 +588,11 @@ virtual class GncOptimizer {
  GncOptimizer(const gtsam::NonlinearFactorGraph& graph,
               const gtsam::Values& initialValues,
               const PARAMS& params);
+  void setInlierCostThresholds(const double inth);
+  const Vector& getInlierCostThresholds();
+  void setInlierCostThresholdsAtProbability(const double alpha);
+  void setWeights(const Vector w);
+  const Vector& getWeights();
  gtsam::Values optimize();
 };

@ -705,7 +701,7 @@ class ISAM2Result {
  /** Getters and Setters for all properties */
  size_t getVariablesRelinearized() const;
  size_t getVariablesReeliminated() const;
-  FactorIndices getNewFactorsIndices() const;
+  gtsam::FactorIndices getNewFactorsIndices() const;
  size_t getCliques() const;
  double getErrorBefore() const;
  double getErrorAfter() const;
@ -873,7 +869,7 @@ template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
               gtsam::PinholeCamera<gtsam::Cal3Unified>,
               gtsam::imuBias::ConstantBias}>
 virtual class NonlinearEquality2 : gtsam::NoiseModelFactor {
-  NonlinearEquality2(Key key1, Key key2, double mu = 1e4);
+  NonlinearEquality2(gtsam::Key key1, gtsam::Key key2, double mu = 1e4);
  gtsam::Vector evaluateError(const T& x1, const T& x2);
 };

--- a/gtsam/nonlinear/tests/testExpression.cpp
+++ b/gtsam/nonlinear/tests/testExpression.cpp
@ -122,7 +122,7 @@ class Class : public Point3 {
  enum {dimension = 3};
  using Point3::Point3;
  const Vector3& vector() const { return *this; }
-  inline static Class identity() { return Class(0,0,0); }
+  inline static Class Identity() { return Class(0,0,0); }
  double norm(OptionalJacobian<1,3> H = boost::none) const {
    return norm3(*this, H);
  }
@ -285,7 +285,7 @@ TEST(Expression, compose2) {
 // Test compose with one arguments referring to constant rotation.
 TEST(Expression, compose3) {
  // Create expression
-  Rot3_ R1(Rot3::identity()), R2(3);
+  Rot3_ R1(Rot3::Identity()), R2(3);
  Rot3_ R3 = R1 * R2;

  // Check keys
--- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp
+++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp
@ -217,7 +217,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) {

  // Setup prior factors
  // Note: If x0 is too far away from the origin (e.g. x=100) this test can fail.
-  Pose3 x0(Rot3::identity(), Vector3(10, -1, 1));
+  Pose3 x0(Rot3::Identity(), Vector3(10, -1, 1));
  auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
  graph.addPrior<Pose3>(X(0), x0, x0_noise);

@ -241,7 +241,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) {
  // Initial values
  // Just offset the initial pose by 1m. This is what we are trying to optimize.
  Values initialEstimate;
-  Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1,0,0)));
+  Pose3 x0_initial = x0.compose(Pose3(Rot3::Identity(), Vector3(1,0,0)));
  initialEstimate.insert(P(1), p1);
  initialEstimate.insert(P(2), p2);
  initialEstimate.insert(X(0), x0_initial);
--- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp
@ -69,7 +69,7 @@ SmartProjectionParams params(
 TEST(SmartProjectionRigFactor, Constructor) {
  using namespace vanillaRig;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
  SmartRigFactor::shared_ptr factor1(
      new SmartRigFactor(model, cameraRig, params));
 }
@ -89,7 +89,7 @@ TEST(SmartProjectionRigFactor, Constructor2) {
 TEST(SmartProjectionRigFactor, Constructor3) {
  using namespace vanillaRig;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
  SmartRigFactor::shared_ptr factor1(
      new SmartRigFactor(model, cameraRig, params));
  factor1->add(measurement1, x1, cameraId1);
@ -99,7 +99,7 @@ TEST(SmartProjectionRigFactor, Constructor3) {
 TEST(SmartProjectionRigFactor, Constructor4) {
  using namespace vanillaRig;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
  SmartProjectionParams params2(
      gtsam::HESSIAN,
      gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
@ -112,7 +112,7 @@ TEST(SmartProjectionRigFactor, Constructor4) {
 TEST(SmartProjectionRigFactor, Equals) {
  using namespace vanillaRig;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));

  SmartRigFactor::shared_ptr factor1(
      new SmartRigFactor(model, cameraRig, params));
@ -140,7 +140,7 @@ TEST(SmartProjectionRigFactor, noiseless) {
  Point2 level_uv_right = level_camera_right.project(landmark1);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));

  SmartRigFactor factor(model, cameraRig, params);
  factor.add(level_uv, x1);  // both taken from the same camera
@ -197,7 +197,7 @@ TEST(SmartProjectionRigFactor, noisy) {
  using namespace vanillaRig;

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));

  // Project two landmarks into two cameras
  Point2 pixelError(0.2, 0.2);
@ -323,7 +323,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
      Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));
  Pose3 body_T_sensor2 =
      Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1));
-  Pose3 body_T_sensor3 = Pose3::identity();
+  Pose3 body_T_sensor3 = Pose3();

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(body_T_sensor1, sharedK));
@ -407,7 +407,7 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) {
  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK2));
+  cameraRig->push_back(Camera(Pose3(), sharedK2));

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -495,7 +495,7 @@ TEST(SmartProjectionRigFactor, Factors) {
  FastVector<size_t> cameraIds{0, 0};

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));

  SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>(
      model, cameraRig, params);
@ -578,7 +578,7 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) {

  // create smart factor
  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
  FastVector<size_t> cameraIds{0, 0, 0};
  SmartRigFactor::shared_ptr smartFactor1(
      new SmartRigFactor(model, cameraRig, params));
@ -646,7 +646,7 @@ TEST(SmartProjectionRigFactor, landmarkDistance) {
  params.setEnableEPI(false);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
  FastVector<size_t> cameraIds{0, 0, 0};

  SmartRigFactor::shared_ptr smartFactor1(
@ -717,7 +717,7 @@ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) {
  params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
  FastVector<size_t> cameraIds{0, 0, 0};

  SmartRigFactor::shared_ptr smartFactor1(
@ -783,7 +783,7 @@ TEST(SmartProjectionRigFactor, CheckHessian) {
  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
  FastVector<size_t> cameraIds{0, 0, 0};

  SmartRigFactor::shared_ptr smartFactor1(
@ -859,7 +859,7 @@ TEST(SmartProjectionRigFactor, Hessian) {
  measurements_cam1.push_back(cam2_uv1);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK2));
+  cameraRig->push_back(Camera(Pose3(), sharedK2));
  FastVector<size_t> cameraIds{0, 0};

  SmartProjectionParams params(
@ -889,7 +889,7 @@ TEST(SmartProjectionRigFactor, Hessian) {
 TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
  using namespace bundlerPose;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK));
+  cameraRig->push_back(Camera(Pose3(), sharedBundlerK));

  SmartProjectionParams params;
  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
@ -917,7 +917,7 @@ TEST(SmartProjectionRigFactor, Cal3Bundler) {
  KeyVector views{x1, x2, x3};

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK));
+  cameraRig->push_back(Camera(Pose3(), sharedBundlerK));
  FastVector<size_t> cameraIds{0, 0, 0};

  SmartRigFactor::shared_ptr smartFactor1(
@ -988,7 +988,7 @@ TEST(SmartProjectionRigFactor,
  KeyVector keys{x1, x2, x3, x1};

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
  FastVector<size_t> cameraIds{0, 0, 0, 0};

  SmartRigFactor::shared_ptr smartFactor1(
@ -1116,7 +1116,7 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) {
  // create inputs
  KeyVector keys{x1, x2, x3};
  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
  FastVector<size_t> cameraIds{0, 0, 0};
  FastVector<size_t> cameraIdsRedundant{0, 0, 0, 0};

@ -1195,11 +1195,11 @@ TEST(SmartProjectionRigFactor, timing) {
  // Default cameras for simple derivatives
  static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));

-  Rot3 R = Rot3::identity();
+  Rot3 R = Rot3::Identity();
  Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
  Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
  Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
-  Pose3 body_P_sensorId = Pose3::identity();
+  Pose3 body_P_sensorId = Pose3();

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));
@ -1267,7 +1267,7 @@ TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
  keys.push_back(x3);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), emptyK));
+  cameraRig->push_back(Camera(Pose3(), emptyK));

  SmartProjectionParams params(
      gtsam::HESSIAN,
@ -1330,10 +1330,10 @@ TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
 /* *************************************************************************/
 TEST(SmartProjectionFactorP, timing_sphericalCamera) {
  // create common data
-  Rot3 R = Rot3::identity();
+  Rot3 R = Rot3::Identity();
  Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
  Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
-  Pose3 body_P_sensorId = Pose3::identity();
+  Pose3 body_P_sensorId = Pose3();
  Point3 landmark1(0, 0, 10);

  // create spherical data
@ -1423,7 +1423,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {

    boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
        new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
-    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), sharedK));
+    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), sharedK));

    SmartRigFactor::shared_ptr smartFactor1(
        new SmartRigFactor(model, cameraRig, params));
@ -1461,7 +1461,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {

    boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
            new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
-    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
+    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), canonicalK));

    SmartRigFactor::shared_ptr smartFactor1(
        new SmartRigFactor(model, cameraRig, params));
@ -1498,7 +1498,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {

    boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
        new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
-    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
+    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), canonicalK));

    SmartRigFactor::shared_ptr smartFactor1(
        new SmartRigFactor(model, cameraRig, params));
@ -1537,7 +1537,7 @@ TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) {

  boost::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
          new CameraSet<SphericalCamera>());  // single camera in the rig
-  cameraRig->push_back(SphericalCamera(Pose3::identity(), emptyK));
+  cameraRig->push_back(SphericalCamera(Pose3(), emptyK));

  // TRIANGULATION TEST WITH DEFAULT RANK TOL
  {  // rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a
--- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
+++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2;
 const double dt = 1.0;

 PoseRTV origin,
-        pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)),
+        pose1(Point3(0.5, 0.0, 0.0), Rot3(), Velocity3(1.0, 0.0, 0.0)),
        pose1a(Point3(0.5, 0.0, 0.0)),
-        pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0));
+        pose2(Point3(1.5, 0.0, 0.0), Rot3(), Velocity3(1.0, 0.0, 0.0));

 /* ************************************************************************* */
 TEST( testVelocityConstraint, trapezoidal ) {
--- a/gtsam_unstable/examples/GncPoseAveragingExample.cpp
+++ b/gtsam_unstable/examples/GncPoseAveragingExample.cpp
@ -53,7 +53,7 @@ int main(int argc, char** argv){
  normal_distribution<double> normalInliers(0.0, 0.05);

  Values initial;
-  initial.insert(0, Pose3::identity()); // identity pose as initialization
+  initial.insert(0, Pose3()); // identity pose as initialization

  // create ground truth pose
  Vector6 poseGtVector;
--- a/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp
+++ b/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp
@ -129,8 +129,8 @@ int main(int argc, char* argv[]) {
  // Pose prior - at identity
  auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
-  graph.addPrior(X(1), Pose3::identity(), priorPoseNoise);
-  initialEstimate.insert(X(0), Pose3::identity());
+  graph.addPrior(X(1), Pose3::Identity(), priorPoseNoise);
+  initialEstimate.insert(X(0), Pose3::Identity());

  // Bias prior
  graph.addPrior(B(1), imu.priorImuBias,
@ -157,7 +157,7 @@ int main(int argc, char* argv[]) {
    if (frame != lastFrame || in.eof()) {
      cout << "Running iSAM for frame: " << lastFrame << "\n";

-      initialEstimate.insert(X(lastFrame), Pose3::identity());
+      initialEstimate.insert(X(lastFrame), Pose3::Identity());
      initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0));
      initialEstimate.insert(B(lastFrame), imu.prevBias);

--- a/gtsam_unstable/geometry/Pose3Upright.h
+++ b/gtsam_unstable/geometry/Pose3Upright.h
@ -95,7 +95,7 @@ public:
  /// @{

  /// identity for group operation
-  static Pose3Upright identity() { return Pose3Upright(); }
+  static Pose3Upright Identity() { return Pose3Upright(); }

  /// inverse transformation with derivatives
  Pose3Upright inverse(boost::optional<Matrix&> H1=boost::none) const;
--- a/gtsam_unstable/gtsam_unstable.i
+++ b/gtsam_unstable/gtsam_unstable.i
@ -130,7 +130,7 @@ class Pose3Upright {
  gtsam::Pose3Upright retract(Vector v) const;
  Vector localCoordinates(const gtsam::Pose3Upright& p2) const;

-  static gtsam::Pose3Upright identity();
+  static gtsam::Pose3Upright Identity();
  gtsam::Pose3Upright inverse() const;
  gtsam::Pose3Upright compose(const gtsam::Pose3Upright& p2) const;
  gtsam::Pose3Upright between(const gtsam::Pose3Upright& p2) const;
--- a/gtsam_unstable/partition/tests/CMakeLists.txt
+++ b/gtsam_unstable/partition/tests/CMakeLists.txt
@ -1,2 +1,2 @@
 set(ignore_test "testNestedDissection.cpp")
-gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis-gtsam")
+gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis-gtsam-if")
--- a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
+++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
@ -44,8 +44,8 @@ TEST(LocalOrientedPlane3Factor, lm_translation_error) {

  // Init pose and prior.  Pose Prior is needed since a single plane measurement
  // does not fully constrain the pose
-  Pose3 init_pose = Pose3::identity();
-  Pose3 anchor_pose = Pose3::identity();
+  Pose3 init_pose = Pose3::Identity();
+  Pose3 anchor_pose = Pose3::Identity();
  graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001));
  graph.addPrior(X(1), anchor_pose, noiseModel::Isotropic::Sigma(6, 0.001));

@ -89,7 +89,7 @@ TEST (LocalOrientedPlane3Factor, lm_rotation_error) {

  // Init pose and prior.  Pose Prior is needed since a single plane measurement
  // does not fully constrain the pose
-  Pose3 init_pose = Pose3::identity();
+  Pose3 init_pose = Pose3::Identity();
  graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001));

  // Add two landmark measurements, differing in angle
@ -180,8 +180,8 @@ TEST(LocalOrientedPlane3Factor, Issue561Simplified) {
  NonlinearFactorGraph graph;

  // Setup prior factors
-  Pose3 x0(Rot3::identity(), Vector3(100, 30, 10));  // the "sensor pose"
-  Pose3 x1(Rot3::identity(), Vector3(90, 40,  5) );  // the "anchor pose"
+  Pose3 x0(Rot3::Identity(), Vector3(100, 30, 10));  // the "sensor pose"
+  Pose3 x1(Rot3::Identity(), Vector3(90, 40,  5) );  // the "anchor pose"

  auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
  auto x1_noise = noiseModel::Isotropic::Sigma(6, 0.01);
@ -213,7 +213,7 @@ TEST(LocalOrientedPlane3Factor, Issue561Simplified) {
  // Initial values
  // Just offset the initial pose by 1m. This is what we are trying to optimize.
  Values initialEstimate;
-  Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1, 0, 0)));
+  Pose3 x0_initial = x0.compose(Pose3(Rot3::Identity(), Vector3(1, 0, 0)));
  initialEstimate.insert(P(1), p1_in_x1);
  initialEstimate.insert(P(2), p2_in_x1);
  initialEstimate.insert(X(0), x0_initial);
--- a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
+++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
@ -173,7 +173,7 @@ TEST(PartialPriorFactor, Constructors3) {
 /* ************************************************************************* */
 TEST(PartialPriorFactor, JacobianAtIdentity3) {
  Key poseKey(1);
-  Pose3 measurement = Pose3::identity();
+  Pose3 measurement = Pose3::Identity();
  SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25);

  TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model);
--- a/gtsam_unstable/slam/tests/testPoseToPointFactor.cpp
+++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.cpp
@ -16,7 +16,7 @@ using namespace gtsam::noiseModel;
 /* ************************************************************************* */
 // Verify zero error when there is no noise
 TEST(PoseToPointFactor, errorNoiseless_2D) {
-  Pose2 pose = Pose2::identity();
+  Pose2 pose = Pose2::Identity();
  Point2 point(1.0, 2.0);
  Point2 noise(0.0, 0.0);
  Point2 measured = point + noise;
@ -33,7 +33,7 @@ TEST(PoseToPointFactor, errorNoiseless_2D) {
 /* ************************************************************************* */
 // Verify expected error in test scenario
 TEST(PoseToPointFactor, errorNoise_2D) {
-  Pose2 pose = Pose2::identity();
+  Pose2 pose = Pose2::Identity();
  Point2 point(1.0, 2.0);
  Point2 noise(-1.0, 0.5);
  Point2 measured = point + noise;
@ -86,7 +86,7 @@ TEST(PoseToPointFactor, jacobian_2D) {
 /* ************************************************************************* */
 // Verify zero error when there is no noise
 TEST(PoseToPointFactor, errorNoiseless_3D) {
-  Pose3 pose = Pose3::identity();
+  Pose3 pose = Pose3::Identity();
  Point3 point(1.0, 2.0, 3.0);
  Point3 noise(0.0, 0.0, 0.0);
  Point3 measured = point + noise;
@ -103,7 +103,7 @@ TEST(PoseToPointFactor, errorNoiseless_3D) {
 /* ************************************************************************* */
 // Verify expected error in test scenario
 TEST(PoseToPointFactor, errorNoise_3D) {
-  Pose3 pose = Pose3::identity();
+  Pose3 pose = Pose3::Identity();
  Point3 point(1.0, 2.0, 3.0);
  Point3 noise(-1.0, 0.5, 0.3);
  Point3 measured = point + noise;
--- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
+++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
@ -88,7 +88,7 @@ typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
 TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
  using namespace vanillaPoseRS;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
  SmartFactorRS::shared_ptr factor1(
      new SmartFactorRS(model, cameraRig, params));
 }
@ -97,7 +97,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
 TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
  using namespace vanillaPoseRS;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
  params.setRankTolerance(rankTol);
  SmartFactorRS factor1(model, cameraRig, params);
 }
@ -106,7 +106,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
 TEST(SmartProjectionPoseFactorRollingShutter, add) {
  using namespace vanillaPoseRS;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
  SmartFactorRS::shared_ptr factor1(
      new SmartFactorRS(model, cameraRig, params));
  factor1->add(measurement1, x1, x2, interp_factor);
@ -230,7 +230,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
  // Project two landmarks into two cameras
  Point2 level_uv = cam1.project(landmark1);
  Point2 level_uv_right = cam2.project(landmark1);
-  Pose3 body_P_sensorId = Pose3::identity();
+  Pose3 body_P_sensorId = Pose3::Identity();

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(body_P_sensorId, sharedK));
@ -405,7 +405,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
  interp_factors.push_back(interp_factor3);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));

  SmartFactorRS::shared_ptr smartFactor1(
      new SmartFactorRS(model, cameraRig, params));
@ -480,7 +480,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) {

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(body_P_sensor, sharedK));
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));

  SmartFactorRS::shared_ptr smartFactor1(
      new SmartFactorRS(model, cameraRig, params));
@ -633,11 +633,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
  // Default cameras for simple derivatives
  static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));

-  Rot3 R = Rot3::identity();
+  Rot3 R = Rot3::Identity();
  Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
  Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
  Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
-  Pose3 body_P_sensorId = Pose3::identity();
+  Pose3 body_P_sensorId = Pose3::Identity();

  // one landmarks 1m in front of camera
  Point3 landmark1(0, 0, 10);
@ -747,7 +747,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
  params.setEnableEPI(true);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));

  SmartFactorRS smartFactor1(model, cameraRig, params);
  smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
@ -816,7 +816,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
  params.setEnableEPI(false);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));

  SmartFactorRS smartFactor1(model, cameraRig, params);
  smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
@ -894,7 +894,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
  params.setEnableEPI(false);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));

  SmartFactorRS::shared_ptr smartFactor1(
      new SmartFactorRS(model, cameraRig, params));
@ -961,7 +961,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
  interp_factors.push_back(interp_factor3);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));

  SmartFactorRS::shared_ptr smartFactor1(
      new SmartFactorRS(model, cameraRig, params));
@ -1102,7 +1102,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
  interp_factors.push_back(interp_factor1);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));

  SmartFactorRS::shared_ptr smartFactor1(
      new SmartFactorRS(model, cameraRig, params));
@ -1261,7 +1261,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
      interp_factors.at(0));  // we readd the first interp factor

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));

  SmartFactorRS::shared_ptr smartFactor1(
      new SmartFactorRS(model, cameraRig, params));
@ -1331,11 +1331,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
      gtsam::HESSIAN,
      gtsam::ZERO_ON_DEGENERACY);  // only config that works with RS factors

-  Rot3 R = Rot3::identity();
+  Rot3 R = Rot3::Identity();
  Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
  Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
  Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
-  Pose3 body_P_sensorId = Pose3::identity();
+  Pose3 body_P_sensorId = Pose3::Identity();

  // one landmarks 1m in front of camera
  Point3 landmark1(0, 0, 10);
@ -1431,7 +1431,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
  params.setRankTolerance(0.1);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), emptyK));
+  cameraRig->push_back(Camera(Pose3::Identity(), emptyK));

  SmartFactorRS_spherical::shared_ptr smartFactor1(
      new SmartFactorRS_spherical(model, cameraRig, params));
--- a/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp
+++ b/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp
@ -198,10 +198,10 @@ TEST(testISAM2SmartFactor, Stereo_Batch) {

    // prior, for the first keyframe:
    if (kf_id == 0) {
-      batch_graph.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise);
+      batch_graph.addPrior(X(kf_id), Pose3::Identity(), priorPoseNoise);
    }

-    batch_values.insert(X(kf_id), Pose3::identity());
+    batch_values.insert(X(kf_id), Pose3::Identity());
  }

  LevenbergMarquardtParams parameters;
@ -267,7 +267,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) {
  // Storage of smart factors:
  std::map<lm_id_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;

-  Pose3 lastKeyframePose = Pose3::identity();
+  Pose3 lastKeyframePose = Pose3::Identity();

  // Run one timestep at once:
  for (const auto &entries : dataset) {
@ -307,7 +307,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) {

    // prior, for the first keyframe:
    if (kf_id == 0) {
-      newFactors.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise);
+      newFactors.addPrior(X(kf_id), Pose3::Identity(), priorPoseNoise);
    }

    // 2) Run iSAM2:
--- a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
+++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
@ -181,8 +181,8 @@ TEST_UNSAFE( SmartStereoProjectionFactorPP, noiseless_error_identityExtrinsics )
  Values values;
  values.insert(x1, w_Pose_cam1);
  values.insert(x2, w_Pose_cam2);
-  values.insert(body_P_cam1_key, Pose3::identity());
-  values.insert(body_P_cam2_key, Pose3::identity());
+  values.insert(body_P_cam1_key, Pose3::Identity());
+  values.insert(body_P_cam2_key, Pose3::Identity());

  SmartStereoProjectionFactorPP factor1(model);
  factor1.add(cam1_uv, x1, body_P_cam1_key, K2);
@ -426,7 +426,7 @@ TEST( SmartStereoProjectionFactorPP, 3poses_optimization_multipleExtrinsics ) {
  // Values
  Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
  Pose3 body_Pose_cam2 = Pose3(Rot3::Ypr(-M_PI / 4, 0.1, 1.0),Point3(1, 1, 1));
-  Pose3 body_Pose_cam3 = Pose3::identity();
+  Pose3 body_Pose_cam3 = Pose3::Identity();
  Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse());
  Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam2.inverse());
  Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam3.inverse());
@ -1147,7 +1147,7 @@ TEST( SmartStereoProjectionFactorPP, landmarkDistance ) {
  graph.push_back(smartFactor3);
  graph.addPrior(x1, pose1, noisePrior);
  graph.addPrior(x2, pose2, noisePrior);
-  graph.addPrior(body_P_cam_key, Pose3::identity(), noisePrior);
+  graph.addPrior(body_P_cam_key, Pose3::Identity(), noisePrior);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -1156,7 +1156,7 @@ TEST( SmartStereoProjectionFactorPP, landmarkDistance ) {
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  values.insert(x3, pose3 * noise_pose);
-  values.insert(body_P_cam_key, Pose3::identity());
+  values.insert(body_P_cam_key, Pose3::Identity());

  // All smart factors are disabled and pose should remain where it is
  Values result;
@ -1245,7 +1245,7 @@ TEST( SmartStereoProjectionFactorPP, dynamicOutlierRejection ) {
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  values.insert(x3, pose3);
-  values.insert(body_P_cam_key, Pose3::identity());
+  values.insert(body_P_cam_key, Pose3::Identity());

  EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9);
  EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9);
@ -1267,7 +1267,7 @@ TEST( SmartStereoProjectionFactorPP, dynamicOutlierRejection ) {
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
  result = optimizer.optimize();
-  EXPECT(assert_equal(Pose3::identity(), result.at<Pose3>(body_P_cam_key)));
+  EXPECT(assert_equal(Pose3::Identity(), result.at<Pose3>(body_P_cam_key)));
 }

 /* ************************************************************************* */
--- a/matlab/CMakeLists.txt
+++ b/matlab/CMakeLists.txt
@ -73,6 +73,7 @@ set(interface_files
    ${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i
    ${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i
    ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
+    ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/custom.i
    ${GTSAM_SOURCE_DIR}/gtsam/symbolic/symbolic.i
    ${GTSAM_SOURCE_DIR}/gtsam/sam/sam.i
    ${GTSAM_SOURCE_DIR}/gtsam/slam/slam.i
@ -98,7 +99,6 @@ endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)

 # Record the root dir for gtsam - needed during external builds, e.g., ROS
 set(GTSAM_SOURCE_ROOT_DIR ${GTSAM_SOURCE_DIR})
-message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]")

 # Tests message(STATUS "Installing Matlab Toolbox")
 install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig")
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@ -61,6 +61,7 @@ set(interface_headers
    ${PROJECT_SOURCE_DIR}/gtsam/geometry/geometry.i
    ${PROJECT_SOURCE_DIR}/gtsam/linear/linear.i
    ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
+    ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/custom.i
    ${PROJECT_SOURCE_DIR}/gtsam/symbolic/symbolic.i
    ${PROJECT_SOURCE_DIR}/gtsam/sam/sam.i
    ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i
--- a/python/gtsam/preamble/custom.h
+++ b/python/gtsam/preamble/custom.h
@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */
--- a/python/gtsam/preamble/nonlinear.h
+++ b/python/gtsam/preamble/nonlinear.h
@ -9,4 +9,4 @@
 * automatic STL binding, such that the raw objects can be accessed in Python.
 * Without this they will be automatically converted to a Python object, and all
 * mutations on Python side will not be reflected on C++.
- */
+ */
--- a/python/gtsam/specializations/custom.h
+++ b/python/gtsam/specializations/custom.h
@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like
+ * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
+ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
+ * and saves one copy operation.
+ */
--- a/python/gtsam/specializations/nonlinear.h
+++ b/python/gtsam/specializations/nonlinear.h
@ -9,4 +9,4 @@
 * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
 * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
 * and saves one copy operation.
- */
+ */
--- a/python/gtsam/tests/test_NonlinearOptimizer.py
+++ b/python/gtsam/tests/test_NonlinearOptimizer.py
@ -15,10 +15,12 @@ from __future__ import print_function
 import unittest

 import gtsam
-from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters,
-                   GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLMOptimizer,
-                   LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph,
-                   Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values)
+from gtsam import (
+    DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, GaussNewtonOptimizer,
+    GaussNewtonParams, GncLMParams, GncLossType, GncLMOptimizer, LevenbergMarquardtOptimizer,
+    LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2,
+    PriorFactorPoint2, Values
+)
 from gtsam.utils.test_case import GtsamTestCase

 KEY1 = 1
@ -27,7 +29,6 @@ KEY2 = 2

 class TestScenario(GtsamTestCase):
    """Do trivial test with three optimizer variants."""
-
    def setUp(self):
        """Set up the optimization problem and ordering"""
        # create graph
@ -83,16 +84,83 @@ class TestScenario(GtsamTestCase):
        actual = GncLMOptimizer(self.fg, self.initial_values, gncParams).optimize()
        self.assertAlmostEqual(0, self.fg.error(actual))

+    def test_gnc_params(self):
+        base_params = LevenbergMarquardtParams()
+        # Test base params
+        for base_max_iters in (50, 100):
+            base_params.setMaxIterations(base_max_iters)
+            params = GncLMParams(base_params)
+            self.assertEqual(params.baseOptimizerParams.getMaxIterations(), base_max_iters)
+
+        # Test printing
+        params_str = str(params)
+        for s in (
+            "lossType",
+            "maxIterations",
+            "muStep",
+            "relativeCostTol",
+            "weightsTol",
+            "verbosity",
+        ):
+            self.assertTrue(s in params_str)
+
+        # Test each parameter
+        for loss_type in (GncLossType.TLS, GncLossType.GM):
+            params.setLossType(loss_type)  # Default is TLS
+            self.assertEqual(params.lossType, loss_type)
+        for max_iter in (1, 10, 100):
+            params.setMaxIterations(max_iter)
+            self.assertEqual(params.maxIterations, max_iter)
+        for mu_step in (1.1, 1.2, 1.5):
+            params.setMuStep(mu_step)
+            self.assertEqual(params.muStep, mu_step)
+        for rel_cost_tol in (1e-5, 1e-6, 1e-7):
+            params.setRelativeCostTol(rel_cost_tol)
+            self.assertEqual(params.relativeCostTol, rel_cost_tol)
+        for weights_tol in (1e-4, 1e-3, 1e-2):
+            params.setWeightsTol(weights_tol)
+            self.assertEqual(params.weightsTol, weights_tol)
+        for i in (0, 1, 2):
+            verb = GncLMParams.Verbosity(i)
+            params.setVerbosityGNC(verb)
+            self.assertEqual(params.verbosity, verb)
+        for inl in ([], [10], [0, 100]):
+            params.setKnownInliers(inl)
+            self.assertEqual(params.knownInliers, inl)
+            params.knownInliers = []
+        for out in ([], [1], [0, 10]):
+            params.setKnownInliers(out)
+            self.assertEqual(params.knownInliers, out)
+            params.knownInliers = []
+
+        # Test optimizer params
+        optimizer = GncLMOptimizer(self.fg, self.initial_values, params)
+        for ict_factor in (0.9, 1.1):
+            new_ict = ict_factor * optimizer.getInlierCostThresholds()
+            optimizer.setInlierCostThresholds(new_ict)
+            self.assertAlmostEqual(optimizer.getInlierCostThresholds(), new_ict)
+        for w_factor in (0.8, 0.9):
+            new_weights = w_factor * optimizer.getWeights()
+            optimizer.setWeights(new_weights)
+            self.assertAlmostEqual(optimizer.getWeights(), new_weights)
+        optimizer.setInlierCostThresholdsAtProbability(0.9)
+        w1 = optimizer.getInlierCostThresholds()
+        optimizer.setInlierCostThresholdsAtProbability(0.8)
+        w2 = optimizer.getInlierCostThresholds()
+        self.assertLess(w2, w1)
+
    def test_iteration_hook(self):
        # set up iteration hook to track some testable values
        iteration_count = 0
        final_error = 0
        final_values = None
+
        def iteration_hook(iter, error_before, error_after):
            nonlocal iteration_count, final_error, final_values
            iteration_count = iter
            final_error = error_after
            final_values = optimizer.values()
+
        # optimize
        params = LevenbergMarquardtParams.CeresDefaults()
        params.setOrdering(self.ordering)
@ -104,5 +172,6 @@ class TestScenario(GtsamTestCase):
        self.assertEqual(self.fg.error(actual), final_error)
        self.assertEqual(optimizer.iterations(), iteration_count)

+
 if __name__ == "__main__":
    unittest.main()
--- a/tests/testExpressionFactor.cpp
+++ b/tests/testExpressionFactor.cpp
@ -382,7 +382,7 @@ TEST(ExpressionFactor, compose2) {
 TEST(ExpressionFactor, compose3) {

  // Create expression
-  Rot3_ R1(Rot3::identity()), R2(3);
+  Rot3_ R1(Rot3::Identity()), R2(3);
  Rot3_ R3 = R1 * R2;

  // Create factor
--- a/tests/testGncOptimizer.cpp
+++ b/tests/testGncOptimizer.cpp
@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
  Values initial;
  initial.insert(X(1), p0);

-  std::vector<size_t> knownInliers;
+  GncParams<GaussNewtonParams>::IndexVector knownInliers;
  knownInliers.push_back(0);
  knownInliers.push_back(1);
  knownInliers.push_back(2);
@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) {
  Values initial;
  initial.insert(X(1), p0);

-  std::vector<size_t> knownInliers;
+  GncParams<GaussNewtonParams>::IndexVector knownInliers;
  knownInliers.push_back(0);
  knownInliers.push_back(1);
  knownInliers.push_back(2);
@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) {
  Values initial;
  initial.insert(X(1), p0);

-  std::vector<size_t> knownInliers;
+  GncParams<GaussNewtonParams>::IndexVector knownInliers;
  knownInliers.push_back(0);
  knownInliers.push_back(1);
  knownInliers.push_back(2);
@ -763,7 +763,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) {
  // GNC
  // Note: in difficult instances, we set the odometry measurements to be
  // inliers, but this problem is simple enought to succeed even without that
-  // assumption std::vector<size_t> knownInliers;
+  // assumption GncParams<GaussNewtonParams>::IndexVector knownInliers;
  GncParams<GaussNewtonParams> gncParams;
  auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(*graph, *initial,
                                                        gncParams);
@ -784,12 +784,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
  // nonconvexity with known inliers and known outliers (check early stopping
  // when all measurements are known to be inliers or outliers)
  {
-    std::vector<size_t> knownInliers;
+    GncParams<GaussNewtonParams>::IndexVector knownInliers;
    knownInliers.push_back(0);
    knownInliers.push_back(1);
    knownInliers.push_back(2);

-    std::vector<size_t> knownOutliers;
+    GncParams<GaussNewtonParams>::IndexVector knownOutliers;
    knownOutliers.push_back(3);

    GncParams<GaussNewtonParams> gncParams;
@ -813,11 +813,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) {

  // nonconvexity with known inliers and known outliers
  {
-    std::vector<size_t> knownInliers;
+    GncParams<GaussNewtonParams>::IndexVector knownInliers;
    knownInliers.push_back(2);
    knownInliers.push_back(0);

-    std::vector<size_t> knownOutliers;
+    GncParams<GaussNewtonParams>::IndexVector knownOutliers;
    knownOutliers.push_back(3);

    GncParams<GaussNewtonParams> gncParams;
@ -841,7 +841,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) {

  // only known outliers
  {
-    std::vector<size_t> knownOutliers;
+    GncParams<GaussNewtonParams>::IndexVector knownOutliers;
    knownOutliers.push_back(3);

    GncParams<GaussNewtonParams> gncParams;
@ -916,11 +916,11 @@ TEST(GncOptimizer, setWeights) {
  // initialize weights and also set known inliers/outliers
  {
    GncParams<GaussNewtonParams> gncParams;
-    std::vector<size_t> knownInliers;
+    GncParams<GaussNewtonParams>::IndexVector knownInliers;
    knownInliers.push_back(2);
    knownInliers.push_back(0);

-    std::vector<size_t> knownOutliers;
+    GncParams<GaussNewtonParams>::IndexVector knownOutliers;
    knownOutliers.push_back(3);
    gncParams.setKnownInliers(knownInliers);
    gncParams.setKnownOutliers(knownOutliers);
--- a/tests/testManifold.cpp
+++ b/tests/testManifold.cpp
@ -139,7 +139,7 @@ TEST(Manifold, DefaultChart) {

  Vector v3(3);
  v3 << 1, 1, 1;
-  Rot3 I = Rot3::identity();
+  Rot3 I = Rot3::Identity();
  Rot3 R = I.retract(v3);
  //DefaultChart<Rot3> chart5;
  EXPECT(assert_equal(v3, traits<Rot3>::Local(I, R)));
--- a/timing/timeShonanFactor.cpp
+++ b/timing/timeShonanFactor.cpp
@ -62,9 +62,9 @@ int main(int argc, char* argv[]) {

  // Build graph
  NonlinearFactorGraph graph;
-  // graph.add(NonlinearEquality<SOn>(0, SOn::identity(4)));
+  // graph.add(NonlinearEquality<SOn>(0, SOn::Identity(4)));
  auto priorModel = noiseModel::Isotropic::Sigma(6, 10000);
-  graph.add(PriorFactor<SOn>(0, SOn::identity(4), priorModel));
+  graph.add(PriorFactor<SOn>(0, SOn::Identity(4), priorModel));
  auto G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(4));
  for (const auto &m : measurements) {
    const auto &keys = m.keys();
@ -92,7 +92,7 @@ int main(int argc, char* argv[]) {
  for (size_t i = 0; i < 100; i++) {
    gttic_(optimize);
    Values initial;
-    initial.insert(0, SOn::identity(4));
+    initial.insert(0, SOn::Identity(4));
    for (size_t j = 1; j < poses.size(); j++) {
      initial.insert(j, SOn::Random(rng, 4));
    }