Merge branch 'develop' into fix/planeFactor

CMakeLists.txt

@@ -34,7 +34,7 @@ include(GtsamTesting)
 include(GtsamPrinting)
 
 # guard against in-source builds
-if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
+if(${GTSAM_SOURCE_DIR} STREQUAL ${GTSAM_BINARY_DIR})
   message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ")
 endif()
 
@@ -62,6 +62,11 @@ add_subdirectory(CppUnitLite)
 
 # This is the new wrapper
 if(GTSAM_BUILD_PYTHON)
+    # Need to set this for the wrap package so we don't use the default value.
+    set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION}
+        CACHE STRING "The Python version to use for wrapping")
+
+    add_subdirectory(wrap)
     list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")
     add_subdirectory(python)
 endif()

INSTALL.md

@@ -70,7 +70,34 @@ execute commands as follows for an out-of-source build:
   This will build the library and unit tests, run all of the unit tests,
   and then install the library itself.
 
-## CMake Configuration Options and Details
+# Windows Installation
+
+This section details how to build a GTSAM `.sln` file using Visual Studio.
+
+### Prerequisites
+
+- Visual Studio with C++ CMake tools for Windows
+- All the other pre-requisites listed above.
+
+### Steps
+
+1. Open Visual Studio.
+2. Select `Open a local folder` and select the GTSAM source directory.
+3. Go to `Project -> CMake Settings`.
+  - (Optional) Set `Configuration name`.
+  - (Optional) Set `Configuration type`.
+  - Set the `Toolset` to `msvc_x64_x64`. If you know what toolset you require, then skip this step.
+  - Update the `Build root` to `${projectDir}\build\${name}`.
+  - You can optionally create a new configuration for a `Release` build.
+  - Set the necessary CMake variables for your use case.
+  - Click on `Show advanced settings`.
+  - For `CMake generator`, select a version which matches `Visual Studio <Version> <Year> Win64`, e.g. `Visual Studio 16 2019 Win64`.
+  - Save the settings (Ctrl + S).
+4. Click on `Project -> Generate Cache`. This will generate the CMake build files (as seen in the Output window).
+5. The last step will generate a `GTSAM.sln` file in the `build` directory. At this point, GTSAM can be used as a regular Visual Studio project.
+
+
+# CMake Configuration Options and Details
 
 GTSAM has a number of options that can be configured, which is best done with
 one of the following:
@@ -78,7 +105,7 @@ one of the following:
   - ccmake      the curses GUI for cmake
   - cmake-gui   a real GUI for cmake
 
-### Important Options:
+## Important Options:
 
 #### CMAKE_BUILD_TYPE
 We support several build configurations for GTSAM (case insensitive)

cmake/Config.cmake.in

@@ -6,7 +6,7 @@
 get_filename_component(OUR_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
 if(EXISTS "${OUR_CMAKE_DIR}/CMakeCache.txt")
   # In build tree
-  set(@PACKAGE_NAME@_INCLUDE_DIR @CMAKE_SOURCE_DIR@ CACHE PATH "@PACKAGE_NAME@ include directory")
+  set(@PACKAGE_NAME@_INCLUDE_DIR @GTSAM_SOURCE_DIR@ CACHE PATH "@PACKAGE_NAME@ include directory")
 else()
   # Find installed library
   set(@PACKAGE_NAME@_INCLUDE_DIR "${OUR_CMAKE_DIR}/@CONF_REL_INCLUDE_DIR@" CACHE PATH "@PACKAGE_NAME@ include directory")

cmake/GtsamBuildTypes.cmake

@@ -204,9 +204,9 @@ endif()
 
 # Make common binary output directory when on Windows
 if(WIN32)
-  set(RUNTIME_OUTPUT_PATH "${CMAKE_BINARY_DIR}/bin")
+  set(RUNTIME_OUTPUT_PATH "${GTSAM_BINARY_DIR}/bin")
-  set(EXECUTABLE_OUTPUT_PATH "${CMAKE_BINARY_DIR}/bin")
+  set(EXECUTABLE_OUTPUT_PATH "${GTSAM_BINARY_DIR}/bin")
-  set(LIBRARY_OUTPUT_PATH "${CMAKE_BINARY_DIR}/lib")
+  set(LIBRARY_OUTPUT_PATH "${GTSAM_BINARY_DIR}/lib")
 endif()
 
 # Set up build type list for cmake-gui

cmake/GtsamMatlabWrap.cmake

@@ -1,3 +1,19 @@
+# Check / set dependent variables for MATLAB wrapper
+if(GTSAM_INSTALL_MATLAB_TOOLBOX)
+    find_package(Matlab COMPONENTS MEX_COMPILER REQUIRED)
+    if(NOT Matlab_MEX_COMPILER)
+        message(FATAL_ERROR "Cannot find MEX compiler binary. Please check your Matlab installation and ensure MEX in installed as well.")
+        endif()
+
+    if(GTSAM_BUILD_TYPE_POSTFIXES)
+        set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
+    endif()
+
+    if(NOT BUILD_SHARED_LIBS)
+        message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries.  If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
+    endif()
+endif()
+
 # Set up cache options
 option(GTSAM_MEX_BUILD_STATIC_MODULE "Build MATLAB wrapper statically (increases build time)" OFF)
 set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation")
@@ -12,30 +28,27 @@ if(GTSAM_MEX_BUILD_STATIC_MODULE AND WIN32)
     message(FATAL_ERROR "GTSAM_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries. To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).")
 endif()
 
-# Try to automatically configure mex path
+set(MEX_COMMAND ${Matlab_MEX_COMPILER} CACHE PATH "Path to MATLAB MEX compiler")
-if(APPLE)
+set(MATLAB_ROOT ${Matlab_ROOT_DIR} CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
-	file(GLOB matlab_bin_directories "/Applications/MATLAB*/bin")
+
-	set(mex_program_name "mex")
+# Try to automatically configure mex path from provided custom `bin` path.
-elseif(WIN32)
+if(GTSAM_CUSTOM_MATLAB_PATH)
-	file(GLOB matlab_bin_directories "C:/Program Files*/MATLAB/*/bin")
+    set(matlab_bin_directory ${GTSAM_CUSTOM_MATLAB_PATH})
+
+    if(WIN32)
         set(mex_program_name "mex.bat")
     else()
-	file(GLOB matlab_bin_directories "/usr/local/MATLAB/*/bin")
         set(mex_program_name "mex")
     endif()
 
-if(GTSAM_CUSTOM_MATLAB_PATH)
-	set(matlab_bin_directories ${GTSAM_CUSTOM_MATLAB_PATH})
-endif()
-
     # Run find_program explicitly putting $PATH after our predefined program
     # directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents
     # finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is
     # on the system path.
-list(REVERSE matlab_bin_directories) # Reverse list so the highest version (sorted alphabetically) is preferred
     find_program(MEX_COMMAND ${mex_program_name}
-	PATHS ${matlab_bin_directories} ENV PATH
+       PATHS ${matlab_bin_directory} ENV PATH
        NO_DEFAULT_PATH)
+
     mark_as_advanced(FORCE MEX_COMMAND)
     # Now that we have mex, trace back to find the Matlab installation root
     get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
@@ -45,7 +58,7 @@ if(mex_path MATCHES ".*/win64$")
     else()
        get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
     endif()
-set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
+endif()
 
 
 # User-friendly wrapping function.  Builds a mex module from the provided
@@ -227,12 +240,16 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
 
 	set(_ignore gtsam::Point2
 			gtsam::Point3)
+
+    # set the matlab wrapping script variable
+    set(MATLAB_WRAP_SCRIPT "${GTSAM_SOURCE_DIR}/wrap/scripts/matlab_wrap.py")
+
     add_custom_command(
 		OUTPUT ${generated_cpp_file}
 		DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects}
         COMMAND
 			${PYTHON_EXECUTABLE}
-			${CMAKE_SOURCE_DIR}/wrap/matlab_wrapper.py
+			${MATLAB_WRAP_SCRIPT}
             --src ${interfaceHeader}
 			--module_name ${moduleName}
             --out ${generated_files_path}
@@ -418,4 +435,3 @@ function(install_matlab_scripts source_directory patterns)
 	endif()
 
 endfunction()
-

cmake/HandleEigen.cmake

@@ -42,7 +42,7 @@ else()
     set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/")
 
     # The actual include directory (for BUILD cmake target interface):
-    set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${CMAKE_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
+    set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${GTSAM_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
 endif()
 
 # Detect Eigen version:

cmake/HandleGeneralOptions.cmake

@@ -24,6 +24,7 @@ option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available"
 option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
 option(GTSAM_THROW_CHEIRALITY_EXCEPTION  "Throw exception when a triangulated point is behind a camera" ON)
 option(GTSAM_BUILD_PYTHON                "Enable/Disable building & installation of Python module with pybind11" OFF)
+option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab toolbox"  OFF)
 option(GTSAM_ALLOW_DEPRECATED_SINCE_V41  "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
 option(GTSAM_SUPPORT_NESTED_DISSECTION   "Support Metis-based nested dissection" ON)
 option(GTSAM_TANGENT_PREINTEGRATION      "Use new ImuFactor with integration on tangent space" ON)
@@ -40,16 +41,5 @@ elseif(GTSAM_ROT3_EXPMAP)
     set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE)
 endif()
 
-# Options relating to MATLAB wrapper
+# Set the default Python version. This is later updated in HandlePython.cmake.
-# TODO: Check for matlab mex binary before handling building of binaries
-option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab toolbox"  OFF)
 set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.")
-
-# Check / set dependent variables for MATLAB wrapper
-if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES)
-    set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
-endif()
-
-if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
-    message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries.  If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
-endif()

cmake/HandlePython.cmake

@@ -1,22 +1,48 @@
 # Set Python version if either Python or MATLAB wrapper is requested.
 if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
   if(${GTSAM_PYTHON_VERSION} STREQUAL "Default")
+
+    if(${CMAKE_VERSION} VERSION_LESS "3.12.0")
+      # Use older version of cmake's find_python
+      find_package(PythonInterp)
+
+      if(NOT ${PYTHONINTERP_FOUND})
+        message(
+          FATAL_ERROR
+            "Cannot find Python interpreter. Please install Python >= 3.6.")
+      endif()
+
+      find_package(PythonLibs ${PYTHON_VERSION_STRING})
+
+      set(Python_VERSION_MAJOR ${PYTHON_VERSION_MAJOR})
+      set(Python_VERSION_MINOR ${PYTHON_VERSION_MINOR})
+      set(Python_EXECUTABLE ${PYTHON_EXECUTABLE})
+
+    else()
       # Get info about the Python3 interpreter
       # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3
       find_package(Python3 COMPONENTS Interpreter Development)
 
       if(NOT ${Python3_FOUND})
-            message(FATAL_ERROR "Cannot find Python3 interpreter. Please install Python >= 3.6.")
+        message(
+          FATAL_ERROR
+            "Cannot find Python3 interpreter. Please install Python >= 3.6.")
       endif()
 
-        set(GTSAM_PYTHON_VERSION "${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}"
+      set(Python_VERSION_MAJOR ${Python3_VERSION_MAJOR})
-                CACHE
+      set(Python_VERSION_MINOR ${Python3_VERSION_MINOR})
-                STRING
+
-                "The version of Python to build the wrappers against."
+    endif()
+
+    set(GTSAM_PYTHON_VERSION
+        "${Python_VERSION_MAJOR}.${Python_VERSION_MINOR}"
+        CACHE STRING "The version of Python to build the wrappers against."
               FORCE)
+
   endif()
 endif()
 
+# Check for build of Unstable modules
 if(GTSAM_BUILD_PYTHON)
     if(GTSAM_UNSTABLE_BUILD_PYTHON)
         if (NOT GTSAM_BUILD_UNSTABLE)

examples/ImuFactorsExample.cpp

@@ -125,7 +125,7 @@ int main(int argc, char* argv[]) {
   output_filename = var_map["output_filename"].as<string>();
   use_isam = var_map["use_isam"].as<bool>();
 
-  ISAM2* isam2;
+  ISAM2* isam2 = 0;
   if (use_isam) {
     printf("Using ISAM2\n");
     ISAM2Params parameters;

examples/Pose3Localization.cpp

@@ -43,7 +43,7 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const Values::ConstKeyValuePair& key_value : *initial) {
+  for (const auto key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graph->addPrior(firstKey, Pose3(), priorModel);
@@ -74,7 +74,7 @@ int main(const int argc, const char* argv[]) {
 
   // Calculate and print marginal covariances for all variables
   Marginals marginals(*graph, result);
-  for (const auto& key_value : result) {
+  for (const auto key_value : result) {
     auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
     if (!p) continue;
     std::cout << marginals.marginalCovariance(key_value.key) << endl;

examples/Pose3SLAMExample_changeKeys.cpp

@@ -55,7 +55,7 @@ int main(const int argc, const char *argv[]) {
     std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
     // Additional: rewrite input with simplified keys 0,1,...
     Values simpleInitial;
-    for(const Values::ConstKeyValuePair& key_value: *initial) {
+    for(const auto key_value: *initial) {
       Key key;
       if(add)
         key = key_value.key + firstKey;

examples/Pose3SLAMExample_g2o.cpp

@@ -42,7 +42,7 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const Values::ConstKeyValuePair& key_value : *initial) {
+  for (const auto key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graph->addPrior(firstKey, Pose3(), priorModel);

examples/Pose3SLAMExample_initializePose3Chordal.cpp

@@ -42,7 +42,7 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const Values::ConstKeyValuePair& key_value : *initial) {
+  for (const auto key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graph->addPrior(firstKey, Pose3(), priorModel);

examples/Pose3SLAMExample_initializePose3Gradient.cpp

@@ -42,7 +42,7 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const Values::ConstKeyValuePair& key_value : *initial) {
+  for (const auto key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graph->addPrior(firstKey, Pose3(), priorModel);

examples/ShonanAveragingCLI.cpp

@@ -25,6 +25,9 @@
  * Read 3D dataset sphere25000.txt and output to shonan.g2o (default)
  * ./ShonanAveragingCLI -i spere2500.txt
  *
+ * If you prefer using a robust Huber loss, you can add the option "-h true",
+ * for instance
+ * ./ShonanAveragingCLI -i spere2500.txt -h true
  */
 
 #include <gtsam/base/timing.h>
@@ -43,7 +46,8 @@ int main(int argc, char* argv[]) {
   string datasetName;
   string inputFile;
   string outputFile;
-  int d, seed;
+  int d, seed, pMin;
+  bool useHuberLoss;
   po::options_description desc(
       "Shonan Rotation Averaging CLI reads a *pose* graph, extracts the "
       "rotation constraints, and runs the Shonan algorithm.");
@@ -58,6 +62,10 @@ int main(int argc, char* argv[]) {
       "Write solution to the specified file")(
       "dimension,d", po::value<int>(&d)->default_value(3),
       "Optimize over 2D or 3D rotations")(
+      "useHuberLoss,h", po::value<bool>(&useHuberLoss)->default_value(false),
+      "set True to use Huber loss")("pMin,p",
+                                    po::value<int>(&pMin)->default_value(3),
+                                    "set to use desired rank pMin")(
       "seed,s", po::value<int>(&seed)->default_value(42),
       "Random seed for initial estimate");
   po::variables_map vm;
@@ -85,11 +93,14 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph::shared_ptr inputGraph;
   Values::shared_ptr posesInFile;
   Values poses;
+  auto lmParams = LevenbergMarquardtParams::CeresDefaults();
   if (d == 2) {
     cout << "Running Shonan averaging for SO(2) on " << inputFile << endl;
-    ShonanAveraging2 shonan(inputFile);
+    ShonanAveraging2::Parameters parameters(lmParams);
+    parameters.setUseHuber(useHuberLoss);
+    ShonanAveraging2 shonan(inputFile, parameters);
     auto initial = shonan.initializeRandomly(rng);
-    auto result = shonan.run(initial);
+    auto result = shonan.run(initial, pMin);
 
     // Parse file again to set up translation problem, adding a prior
     boost::tie(inputGraph, posesInFile) = load2D(inputFile);
@@ -101,9 +112,11 @@ int main(int argc, char* argv[]) {
     poses = initialize::computePoses<Pose2>(result.first, &poseGraph);
   } else if (d == 3) {
     cout << "Running Shonan averaging for SO(3) on " << inputFile << endl;
-    ShonanAveraging3 shonan(inputFile);
+    ShonanAveraging3::Parameters parameters(lmParams);
+    parameters.setUseHuber(useHuberLoss);
+    ShonanAveraging3 shonan(inputFile, parameters);
     auto initial = shonan.initializeRandomly(rng);
-    auto result = shonan.run(initial);
+    auto result = shonan.run(initial, pMin);
 
     // Parse file again to set up translation problem, adding a prior
     boost::tie(inputGraph, posesInFile) = load3D(inputFile);
@@ -118,7 +131,7 @@ int main(int argc, char* argv[]) {
     return 1;
   }
   cout << "Writing result to " << outputFile << endl;
-  writeG2o(NonlinearFactorGraph(), poses, outputFile);
+  writeG2o(*inputGraph, poses, outputFile);
   return 0;
 }
 

examples/SolverComparer.cpp

@@ -559,7 +559,7 @@ void runPerturb()
 
   // Perturb values
   VectorValues noise;
-  for(const Values::KeyValuePair& key_val: initial)
+  for(const Values::KeyValuePair key_val: initial)
   {
     Vector noisev(key_val.value.dim());
     for(Vector::Index i = 0; i < noisev.size(); ++i)

gtsam/3rdparty/metis/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8)
+cmake_minimum_required(VERSION 3.0)
 project(METIS)
 
 # Add flags for currect directory and below

gtsam/3rdparty/metis/libmetis/CMakeLists.txt

@@ -12,6 +12,7 @@ endif()
 if(WIN32)
 		set_target_properties(metis-gtsam PROPERTIES
 			PREFIX ""
+			COMPILE_FLAGS /w
 			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin")
 endif()
 

gtsam/CMakeLists.txt

@@ -134,12 +134,12 @@ endif()
 # of any previously installed GTSAM headers.
 target_include_directories(gtsam BEFORE PUBLIC
   # main gtsam includes:
-  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
+  $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}>
   $<INSTALL_INTERFACE:include/>
   # config.h
-  $<BUILD_INTERFACE:${CMAKE_BINARY_DIR}>
+  $<BUILD_INTERFACE:${GTSAM_BINARY_DIR}>
   # unit tests:
-  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/CppUnitLite>
+  $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/CppUnitLite>
 )
 # 3rdparty libraries: use the "system" flag so they are included via "-isystem"
 # and warnings (and warnings-considered-errors) in those headers are not
@@ -156,9 +156,9 @@ target_include_directories(gtsam SYSTEM BEFORE PUBLIC
 )
 if(GTSAM_SUPPORT_NESTED_DISSECTION)
   target_include_directories(gtsam BEFORE PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/include>
+    $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/include>
-    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
+    $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
-    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
+    $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
     $<INSTALL_INTERFACE:include/gtsam/3rdparty/metis/>
   )
 endif()
@@ -199,7 +199,7 @@ if(WIN32)
 else()
   if("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
     # Suppress all warnings from 3rd party sources.
-    set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w")
+    set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w -Wno-everything")
   else()
     set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-Wno-error")
   endif()

gtsam/base/Matrix.cpp

@@ -153,7 +153,7 @@ const Eigen::IOFormat& matlabFormat() {
 /* ************************************************************************* */
 //3 argument call
 void print(const Matrix& A, const string &s, ostream& stream) {
-  cout << s << A.format(matlabFormat()) << endl;
+  stream << s << A.format(matlabFormat()) << endl;
 }
 
 /* ************************************************************************* */

gtsam/base/Matrix.h

@@ -90,7 +90,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBas
 
   for(size_t i=0; i<m1; i++)
     for(size_t j=0; j<n1; j++) {
-      if(!fpEqual(A(i,j), B(i,j), tol)) {
+      if(!fpEqual(A(i,j), B(i,j), tol, false)) {
         return false;
       }
     }

gtsam/base/Testable.h

@@ -33,9 +33,9 @@
 
 #pragma once
 
-#include <boost/shared_ptr.hpp>
 #include <boost/concept_check.hpp>
-#include <stdio.h>
+#include <boost/shared_ptr.hpp>
+#include <iostream>
 #include <string>
 
 #define GTSAM_PRINT(x)((x).print(#x))
@@ -72,10 +72,10 @@ namespace gtsam {
   }; // \ Testable
 
   inline void print(float v, const std::string& s = "") {
-    printf("%s%f\n",s.c_str(),v);
+    std::cout << (s == "" ? s : s + " ") << v << std::endl;
   }
   inline void print(double v, const std::string& s = "") {
-    printf("%s%lf\n",s.c_str(),v);
+    std::cout << (s == "" ? s : s + " ") << v << std::endl;
   }
 
   /** Call equal on the object */

gtsam/base/Vector.cpp

@@ -39,7 +39,7 @@ namespace gtsam {
  * 1. https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
  * 2. https://floating-point-gui.de/errors/comparison/
  * ************************************************************************* */
-bool fpEqual(double a, double b, double tol) {
+bool fpEqual(double a, double b, double tol, bool check_relative_also) {
   using std::abs;
   using std::isnan;
   using std::isinf;
@@ -48,7 +48,7 @@ bool fpEqual(double a, double b, double tol) {
   double larger = (abs(b) > abs(a)) ? abs(b) : abs(a);
 
   // handle NaNs
-  if(std::isnan(a) || isnan(b)) {
+  if(isnan(a) || isnan(b)) {
     return isnan(a) && isnan(b);
   }
   // handle inf
@@ -60,13 +60,15 @@ bool fpEqual(double a, double b, double tol) {
   else if(a == 0 || b == 0 || (abs(a) + abs(b)) < DOUBLE_MIN_NORMAL) {
     return abs(a-b) <= tol * DOUBLE_MIN_NORMAL;
   }
-  // Check if the numbers are really close
+  // Check if the numbers are really close.
-  // Needed when comparing numbers near zero or tol is in vicinity
+  // Needed when comparing numbers near zero or tol is in vicinity.
   else if (abs(a - b) <= tol) {
     return true;
   }
-  // Use relative error
+  // Check for relative error
-  else if(abs(a-b) <= tol * min(larger, std::numeric_limits<double>::max())) {
+  else if (abs(a - b) <=
+               tol * min(larger, std::numeric_limits<double>::max()) &&
+           check_relative_also) {
     return true;
   }
 

gtsam/base/Vector.h

@@ -85,9 +85,15 @@ static_assert(
  * respectively for the comparison to be true.
  * If one is NaN/Inf and the other is not, returns false.
  *
+ * @param check_relative_also is a flag which toggles additional checking for
+ * relative error. This means that if either the absolute error or the relative
+ * error is within the tolerance, the result will be true.
+ * By default, the flag is true.
+ *
  * Return true if two numbers are close wrt tol.
  */
-GTSAM_EXPORT bool fpEqual(double a, double b, double tol);
+GTSAM_EXPORT bool fpEqual(double a, double b, double tol,
+                          bool check_relative_also = true);
 
 /**
  * print without optional string, must specify cout yourself

gtsam/base/tests/testMatrix.cpp

@@ -1163,6 +1163,19 @@ TEST(Matrix , IsVectorSpace) {
   BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>));
 }
 
+TEST(Matrix, AbsoluteError) {
+  double a = 2000, b = 1997, tol = 1e-1;
+  bool isEqual;
+
+  // Test only absolute error
+  isEqual = fpEqual(a, b, tol, false);
+  EXPECT(!isEqual);
+
+  // Test relative error as well
+  isEqual = fpEqual(a, b, tol);
+  EXPECT(isEqual);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/geometry/Cal3.cpp

@@ -0,0 +1,75 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   Cal3.cpp
+ * @brief  Common code for all calibration models.
+ * @author Frank Dellaert
+ */
+
+#include <gtsam/geometry/Cal3.h>
+
+#include <cmath>
+#include <fstream>
+#include <iostream>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+Cal3::Cal3(double fov, int w, int h)
+    : s_(0), u0_((double)w / 2.0), v0_((double)h / 2.0) {
+  double a = fov * M_PI / 360.0;  // fov/2 in radians
+  fx_ = double(w) / (2.0 * tan(a));
+  fy_ = fx_;
+}
+
+/* ************************************************************************* */
+Cal3::Cal3(const std::string& path) {
+  const auto buffer = path + std::string("/calibration_info.txt");
+  std::ifstream infile(buffer, std::ios::in);
+
+  if (infile && !infile.eof()) {
+    infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
+  } else {
+    throw std::runtime_error("Cal3: Unable to load the calibration");
+  }
+
+  infile.close();
+}
+
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3& cal) {
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py();
+  return os;
+}
+
+/* ************************************************************************* */
+void Cal3::print(const std::string& s) const { gtsam::print((Matrix)K(), s); }
+
+/* ************************************************************************* */
+bool Cal3::equals(const Cal3& K, double tol) const {
+  return (std::fabs(fx_ - K.fx_) < tol && std::fabs(fy_ - K.fy_) < tol &&
+          std::fabs(s_ - K.s_) < tol && std::fabs(u0_ - K.u0_) < tol &&
+          std::fabs(v0_ - K.v0_) < tol);
+}
+
+Matrix3 Cal3::inverse() const {
+  const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
+  Matrix3 K_inverse;
+  K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, 1.0 / fy_,
+      -v0_ / fy_, 0.0, 0.0, 1.0;
+  return K_inverse;
+}
+
+/* ************************************************************************* */
+
+}  // \ namespace gtsam

gtsam/geometry/Cal3.h

@@ -0,0 +1,206 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   Cal3.h
+ * @brief  Common code for all Calibration models.
+ * @author Varun Agrawal
+ */
+
+/**
+ * @addtogroup geometry
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Point2.h>
+
+namespace gtsam {
+
+/**
+ * Function which makes use of the Implicit Function Theorem to compute the
+ * Jacobians of `calibrate` using `uncalibrate`.
+ * This is useful when there are iterative operations in the `calibrate`
+ * function which make computing jacobians difficult.
+ *
+ * Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can
+ * easily compute the Jacobians:
+ * df/pi = -I (pn and pi are independent args)
+ * Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
+ * Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
+ *
+ * @tparam Cal Calibration model.
+ * @tparam Dim The number of parameters in the calibration model.
+ * @param p Calibrated point.
+ * @param Dcal optional 2*p Jacobian wrpt `p` Cal3DS2 parameters.
+ * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates.
+ */
+template <typename Cal, size_t Dim>
+void calibrateJacobians(const Cal& calibration, const Point2& pn,
+                        OptionalJacobian<2, Dim> Dcal = boost::none,
+                        OptionalJacobian<2, 2> Dp = boost::none) {
+  if (Dcal || Dp) {
+    Eigen::Matrix<double, 2, Dim> H_uncal_K;
+    Matrix22 H_uncal_pn, H_uncal_pn_inv;
+
+    // Compute uncalibrate Jacobians
+    calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
+
+    H_uncal_pn_inv = H_uncal_pn.inverse();
+
+    if (Dp) *Dp = H_uncal_pn_inv;
+    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
+  }
+}
+
+/**
+ * @brief Common base class for all calibration models.
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT Cal3 {
+ protected:
+  double fx_ = 1.0f, fy_ = 1.0f;  ///< focal length
+  double s_ = 0.0f;               ///< skew
+  double u0_ = 0.0f, v0_ = 0.0f;  ///< principal point
+
+ public:
+  enum { dimension = 5 };
+  ///< shared pointer to calibration object
+  using shared_ptr = boost::shared_ptr<Cal3>;
+
+  /// @name Standard Constructors
+  /// @{
+
+  /// Create a default calibration that leaves coordinates unchanged
+  Cal3() = default;
+
+  /// constructor from doubles
+  Cal3(double fx, double fy, double s, double u0, double v0)
+      : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {}
+
+  /// constructor from vector
+  Cal3(const Vector5& d)
+      : fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {}
+
+  /**
+   * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
+   * @param fov field of view in degrees
+   * @param w image width
+   * @param h image height
+   */
+  Cal3(double fov, int w, int h);
+
+  /// Virtual destructor
+  virtual ~Cal3() {}
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+
+  /**
+   * Load calibration parameters from `calibration_info.txt` file located in
+   * `path` directory.
+   *
+   * The contents of calibration file should be the 5 parameters in order:
+   * `fx, fy, s, u0, v0`
+   *
+   * @param path path to directory containing `calibration_info.txt`.
+   */
+  Cal3(const std::string& path);
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3& cal);
+
+  /// print with optional string
+  virtual void print(const std::string& s = "") const;
+
+  /// Check if equal up to specified tolerance
+  bool equals(const Cal3& K, double tol = 10e-9) const;
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /// focal length x
+  inline double fx() const { return fx_; }
+
+  /// focal length y
+  inline double fy() const { return fy_; }
+
+  /// aspect ratio
+  inline double aspectRatio() const { return fx_ / fy_; }
+
+  /// skew
+  inline double skew() const { return s_; }
+
+  /// image center in x
+  inline double px() const { return u0_; }
+
+  /// image center in y
+  inline double py() const { return v0_; }
+
+  /// return the principal point
+  Point2 principalPoint() const { return Point2(u0_, v0_); }
+
+  /// vectorized form (column-wise)
+  Vector5 vector() const {
+    Vector5 v;
+    v << fx_, fy_, s_, u0_, v0_;
+    return v;
+  }
+
+  /// return calibration matrix K
+  virtual Matrix3 K() const {
+    Matrix3 K;
+    K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
+    return K;
+  }
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
+  /** @deprecated The following function has been deprecated, use K above */
+  Matrix3 matrix() const { return K(); }
+#endif
+
+  /// Return inverted calibration matrix inv(K)
+  Matrix3 inverse() const;
+
+  /// return DOF, dimensionality of tangent space
+  inline virtual size_t dim() const { return Dim(); }
+
+  /// return DOF, dimensionality of tangent space
+  inline static size_t Dim() { return dimension; }
+
+  /// @}
+  /// @name Advanced Interface
+  /// @{
+
+ private:
+  /// Serialization function
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_NVP(fx_);
+    ar& BOOST_SERIALIZATION_NVP(fy_);
+    ar& BOOST_SERIALIZATION_NVP(s_);
+    ar& BOOST_SERIALIZATION_NVP(u0_);
+    ar& BOOST_SERIALIZATION_NVP(v0_);
+  }
+
+  /// @}
+};
+
+}  // \ namespace gtsam

gtsam/geometry/Cal3Bundler.cpp

@@ -15,28 +15,19 @@
  * @author ydjian
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3Bundler.h>
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3Bundler::Cal3Bundler() :
-    f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) {
-}
-
-/* ************************************************************************* */
-Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0,
-                         double tol)
-    : f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {}
-
 /* ************************************************************************* */
 Matrix3 Cal3Bundler::K() const {
+  // This function is needed to ensure skew = 0;
   Matrix3 K;
-  K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1;
+  K << fx_, 0, u0_, 0, fy_, v0_, 0, 0, 1.0;
   return K;
 }
 
@@ -48,35 +39,42 @@ Vector4 Cal3Bundler::k() const {
 }
 
 /* ************************************************************************* */
-Vector3 Cal3Bundler::vector() const {
+Vector3 Cal3Bundler::vector() const { return Vector3(fx_, k1_, k2_); }
-  return Vector3(f_, k1_, k2_);
+
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
+  os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()
+     << ", px: " << cal.px() << ", py: " << cal.py();
+  return os;
 }
 
 /* ************************************************************************* */
 void Cal3Bundler::print(const std::string& s) const {
-  gtsam::print((Vector)(Vector(5) << f_, k1_, k2_, u0_, v0_).finished(), s + ".K");
+  gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(),
+               s + ".K");
 }
 
 /* ************************************************************************* */
 bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
-  if (std::abs(f_ - K.f_) > tol || std::abs(k1_ - K.k1_) > tol
+  const Cal3* base = dynamic_cast<const Cal3*>(&K);
-      || std::abs(k2_ - K.k2_) > tol || std::abs(u0_ - K.u0_) > tol
+  return (Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
-      || std::abs(v0_ - K.v0_) > tol)
+          std::fabs(k2_ - K.k2_) < tol && std::fabs(u0_ - K.u0_) < tol &&
-    return false;
+          std::fabs(v0_ - K.v0_) < tol);
-  return true;
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::uncalibrate(const Point2& p, //
+Point2 Cal3Bundler::uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal,
-    OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const {
+                                OptionalJacobian<2, 2> Dp) const {
-  //  r = x^2 + y^2;
+  //  r = x² + y²;
-  //  g = (1 + k(1)*r + k(2)*r^2);
+  //  g = (1 + k(1)*r + k(2)*r²);
   //  pi(:,i) = g * pn(:,i)
   const double x = p.x(), y = p.y();
   const double r = x * x + y * y;
   const double g = 1. + (k1_ + k2_ * r) * r;
   const double u = g * x, v = g * y;
 
+  const double f_ = fx_;
+
   // Derivatives make use of intermediate variables above
   if (Dcal) {
     double rx = r * x, ry = r * y;
@@ -94,23 +92,22 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::calibrate(const Point2& pi,
+Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
-                              OptionalJacobian<2, 3> Dcal,
                               OptionalJacobian<2, 2> Dp) const {
-  // Copied from Cal3DS2 :-(
+  // Copied from Cal3DS2
   // but specialized with k1, k2 non-zero only and fx=fy and s=0
-  double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_;
+  double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
   const Point2 invKPi(x, y);
 
-  // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
+  // initialize by ignoring the distortion at all, might be problematic for
+  // pixels around boundary
   Point2 pn(x, y);
 
   // iterate until the uncalibrate is close to the actual pixel coordinate
   const int maxIterations = 10;
   int iteration;
   for (iteration = 0; iteration < maxIterations; ++iteration) {
-    if (distance2(uncalibrate(pn), pi) <= tol_)
+    if (distance2(uncalibrate(pn), pi) <= tol_) break;
-      break;
     const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
     const double rr = xx + yy;
     const double g = (1 + k1_ * rr + k2_ * rr * rr);
@@ -119,26 +116,10 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
 
   if (iteration >= maxIterations)
     throw std::runtime_error(
-        "Cal3Bundler::calibrate fails to converge. need a better initialization");
+        "Cal3Bundler::calibrate fails to converge. need a better "
+        "initialization");
 
-  // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
+  calibrateJacobians<Cal3Bundler, dimension>(*this, pn, Dcal, Dp);
-  // Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians
-  // df/pi = -I (pn and pi are independent args)
-  // Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
-  // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
-  Matrix23 H_uncal_K;
-  Matrix22 H_uncal_pn, H_uncal_pn_inv;
-
-  if (Dcal || Dp) {
-    // Compute uncalibrate Jacobians
-    uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
-
-    H_uncal_pn_inv = H_uncal_pn.inverse();
-
-    if (Dp) *Dp = H_uncal_pn_inv;
-    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
-
-  }
 
   return pn;
 }
@@ -167,14 +148,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
   return H;
 }
 
-/* ************************************************************************* */
+}  // \ namespace gtsam
-Cal3Bundler Cal3Bundler::retract(const Vector& d) const {
-  return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
-}
-
-/* ************************************************************************* */
-Vector3 Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const {
-  return T2.vector() - vector();
-}
-
-}

gtsam/geometry/Cal3Bundler.h

@@ -14,10 +14,12 @@
  * @brief Calibration used by Bundler
  * @date Sep 25, 2010
  * @author Yong Dian Jian
+ * @author Varun Agrawal
  */
 
 #pragma once
 
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -27,23 +29,23 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Cal3Bundler {
+class GTSAM_EXPORT Cal3Bundler : public Cal3 {
-
  private:
-  double f_; ///< focal length
+  double k1_ = 0.0f, k2_ = 0.0f;  ///< radial distortion
-  double k1_, k2_; ///< radial distortion
+  double tol_ = 1e-5;             ///< tolerance value when calibrating
-  double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
+
-  double tol_; ///< tolerance value when calibrating
+  // NOTE: We use the base class fx to represent the common focal length.
+  // Also, image center parameters (u0, v0) are not optimized
+  // but are treated as constants.
 
  public:
-
   enum { dimension = 3 };
 
   /// @name Standard Constructors
   /// @{
 
   /// Default constructor
-  Cal3Bundler();
+  Cal3Bundler() = default;
 
   /**
    *  Constructor
@@ -55,7 +57,8 @@ public:
    *  @param tol optional calibration tolerance value
    */
   Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
-              double tol = 1e-5);
+              double tol = 1e-5)
+      : Cal3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
 
   virtual ~Cal3Bundler() {}
 
@@ -63,8 +66,12 @@ public:
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3Bundler& cal);
+
   /// print with optional string
-  void print(const std::string& s = "") const;
+  void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
@@ -73,59 +80,36 @@ public:
   /// @name Standard Interface
   /// @{
 
-  Matrix3 K() const; ///< Standard 3*3 calibration matrix
+  /// distorsion parameter k1
+  inline double k1() const { return k1_; }
+
+  /// distorsion parameter k2
+  inline double k2() const { return k2_; }
+
+  /// image center in x
+  inline double px() const { return u0_; }
+
+  /// image center in y
+  inline double py() const { return v0_; }
+
+  Matrix3 K() const override;  ///< Standard 3*3 calibration matrix
   Vector4 k() const;  ///< Radial distortion parameters (4 of them, 2 0)
 
   Vector3 vector() const;
 
-  /// focal length x
-  inline double fx() const {
-    return f_;
-  }
-
-  /// focal length y
-  inline double fy() const {
-    return f_;
-  }
-
-  /// distorsion parameter k1
-  inline double k1() const {
-    return k1_;
-  }
-
-  /// distorsion parameter k2
-  inline double k2() const {
-    return k2_;
-  }
-
-  /// image center in x
-  inline double px() const {
-    return u0_;
-  }
-
-  /// image center in y
-  inline double py() const {
-    return v0_;
-  }
-
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
   /// get parameter u0
-  inline double u0() const {
+  inline double u0() const { return u0_; }
-    return u0_;
-  }
 
   /// get parameter v0
-  inline double v0() const {
+  inline double v0() const { return v0_; }
-    return v0_;
-  }
 #endif
 
-
   /**
    * @brief: convert intrinsic coordinates xy to image coordinates uv
    * Version of uncalibrate with derivatives
    * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
+   * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in image coordinates
    */
@@ -140,8 +124,7 @@ public:
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in intrinsic coordinates
    */
-  Point2 calibrate(const Point2& pi,
+  Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none,
-                   OptionalJacobian<2, 3> Dcal = boost::none,
                    OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
@@ -157,24 +140,23 @@ public:
   /// @name Manifold
   /// @{
 
+  /// return DOF, dimensionality of tangent space
+  virtual size_t dim() const override { return Dim(); }
+
+  /// return DOF, dimensionality of tangent space
+  inline static size_t Dim() { return dimension; }
+
   /// Update calibration with tangent space delta
-  Cal3Bundler retract(const Vector& d) const;
+  inline Cal3Bundler retract(const Vector& d) const {
-
+    return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
-  /// Calculate local coordinates to another calibration
-  Vector3 localCoordinates(const Cal3Bundler& T2) const;
-
-  /// dimensionality
-  virtual size_t dim() const {
-    return 3;
   }
 
-  /// dimensionality
+  /// Calculate local coordinates to another calibration
-  static size_t Dim() {
+  Vector3 localCoordinates(const Cal3Bundler& T2) const {
-    return 3;
+    return T2.vector() - vector();
   }
 
  private:
-
   /// @}
   /// @name Advanced Interface
   /// @{
@@ -183,16 +165,14 @@ private:
   friend class boost::serialization::access;
   template <class Archive>
   void serialize(Archive& ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_NVP(f_);
+    ar& boost::serialization::make_nvp(
+        "Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
     ar& BOOST_SERIALIZATION_NVP(k1_);
     ar& BOOST_SERIALIZATION_NVP(k2_);
-    ar & BOOST_SERIALIZATION_NVP(u0_);
-    ar & BOOST_SERIALIZATION_NVP(v0_);
     ar& BOOST_SERIALIZATION_NVP(tol_);
   }
 
   /// @}
-
 };
 
 template <>

gtsam/geometry/Cal3DS2.cpp

@@ -13,28 +13,30 @@
  * @file Cal3DS2.cpp
  * @date Feb 28, 2010
  * @author ydjian
+ * @author Varun Agrawal
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3DS2.h>
 
 namespace gtsam {
 
 /* ************************************************************************* */
-void Cal3DS2::print(const std::string& s_) const {
+std::ostream& operator<<(std::ostream& os, const Cal3DS2& cal) {
-  Base::print(s_);
+  os << (Cal3DS2_Base&)cal;
+  return os;
 }
 
+/* ************************************************************************* */
+void Cal3DS2::print(const std::string& s_) const { Base::print(s_); }
+
 /* ************************************************************************* */
 bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
+  const Cal3DS2_Base* base = dynamic_cast<const Cal3DS2_Base*>(&K);
-      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
+  return Cal3DS2_Base::equals(*base, tol);
-      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol)
-    return false;
-  return true;
 }
 
 /* ************************************************************************* */
@@ -46,8 +48,5 @@ Cal3DS2 Cal3DS2::retract(const Vector& d) const {
 Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const {
   return T2.vector() - vector();
 }
-
 }
 /* ************************************************************************* */
-
-

gtsam/geometry/Cal3DS2.h

@@ -11,9 +11,11 @@
 
 /**
  * @file Cal3DS2.h
- * @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base
+ * @brief Calibration of a camera with radial distortion, calculations in base
+ * class Cal3DS2_Base
  * @date Feb 28, 2010
  * @author ydjian
+ * @autho Varun Agrawal
  */
 
 #pragma once
@@ -30,22 +32,20 @@ namespace gtsam {
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
-
+  using Base = Cal3DS2_Base;
-  typedef Cal3DS2_Base Base;
 
  public:
-
   enum { dimension = 9 };
 
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3DS2() : Base() {}
+  Cal3DS2() = default;
 
-  Cal3DS2(double fx, double fy, double s, double u0, double v0,
+  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
+          double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
-        Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {}
+      : Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
 
   virtual ~Cal3DS2() {}
 
@@ -53,12 +53,16 @@ public:
   /// @name Advanced Constructors
   /// @{
 
-  Cal3DS2(const Vector &v) : Base(v) {}
+  Cal3DS2(const Vector9& v) : Base(v) {}
 
   /// @}
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3DS2& cal);
+
   /// print with optional string
   void print(const std::string& s = "") const override;
 
@@ -76,10 +80,10 @@ public:
   Vector localCoordinates(const Cal3DS2& T2) const;
 
   /// Return dimensions of calibration manifold object
-  virtual size_t dim() const { return dimension ; }
+  virtual size_t dim() const override { return Dim(); }
 
   /// Return dimensions of calibration manifold object
-  static size_t Dim() { return dimension; }
+  inline static size_t Dim() { return dimension; }
 
   /// @}
   /// @name Clone
@@ -92,23 +96,19 @@ public:
 
   /// @}
 
-
  private:
-
   /// @name Advanced Interface
   /// @{
 
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/)
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-  {
+    ar& boost::serialization::make_nvp(
-    ar & boost::serialization::make_nvp("Cal3DS2",
+        "Cal3DS2", boost::serialization::base_object<Cal3DS2_Base>(*this));
-        boost::serialization::base_object<Cal3DS2_Base>(*this));
   }
 
   /// @}
-
 };
 
 template <>
@@ -116,6 +116,4 @@ struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
 
 template <>
 struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
-
 }
-

gtsam/geometry/Cal3DS2_Base.cpp

@@ -13,27 +13,17 @@
  * @file Cal3DS2_Base.cpp
  * @date Feb 28, 2010
  * @author ydjian
+ * @author Varun Agrawal
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3DS2_Base.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3DS2_Base.h>
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
-    fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
-
-/* ************************************************************************* */
-Matrix3 Cal3DS2_Base::K() const {
-    Matrix3 K;
-    K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
-    return K;
-}
-
 /* ************************************************************************* */
 Vector9 Cal3DS2_Base::vector() const {
   Vector9 v;
@@ -41,6 +31,14 @@ Vector9 Cal3DS2_Base::vector() const {
   return v;
 }
 
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3DS2_Base& cal) {
+  os << (Cal3&)cal;
+  os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", p1: " << cal.p1()
+     << ", p2: " << cal.p2();
+  return os;
+}
+
 /* ************************************************************************* */
 void Cal3DS2_Base::print(const std::string& s_) const {
   gtsam::print((Matrix)K(), s_ + ".K");
@@ -49,17 +47,16 @@ void Cal3DS2_Base::print(const std::string& s_) const {
 
 /* ************************************************************************* */
 bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
+  const Cal3* base = dynamic_cast<const Cal3*>(&K);
-      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
+  return Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
-      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol)
+         std::fabs(k2_ - K.k2_) < tol && std::fabs(p1_ - K.p1_) < tol &&
-    return false;
+         std::fabs(p2_ - K.p2_) < tol;
-  return true;
 }
 
 /* ************************************************************************* */
-static Matrix29 D2dcalibration(double x, double y, double xx,
+static Matrix29 D2dcalibration(double x, double y, double xx, double yy,
-    double yy, double xy, double rr, double r4, double pnx, double pny,
+                               double xy, double rr, double r4, double pnx,
-    const Matrix2& DK) {
+                               double pny, const Matrix2& DK) {
   Matrix25 DR1;
   DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
   Matrix24 DR2;
@@ -71,8 +68,8 @@ static Matrix29 D2dcalibration(double x, double y, double xx,
 }
 
 /* ************************************************************************* */
-static Matrix2 D2dintrinsic(double x, double y, double rr,
+static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1,
-    double g, double k1, double k2, double p1, double p2,
+                            double k2, double p1, double p2,
                             const Matrix2& DK) {
   const double drdx = 2. * x;
   const double drdy = 2. * y;
@@ -94,12 +91,11 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
 }
 
 /* ************************************************************************* */
-Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
+Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal,
-    OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const {
+                                 OptionalJacobian<2, 2> Dp) const {
-
+  //  r² = x² + y²;
-  //  rr = x^2 + y^2;
+  //  g = (1 + k(1)*r² + k(2)*r⁴);
-  //  g = (1 + k(1)*rr + k(2)*rr^2);
+  //  dp = [2*k(3)*x*y + k(4)*(r² + 2*x²); 2*k(4)*x*y + k(3)*(r² + 2*y²)];
-  //  dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
   //  pi(:,i) = g * pn(:,i) + dp;
   const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
   const double rr = xx + yy;
@@ -115,37 +111,44 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
   const double pny = g * y + dy;
 
   Matrix2 DK;
-  if (H1 || H2) DK << fx_, s_, 0.0, fy_;
+  if (Dcal || Dp) {
+    DK << fx_, s_, 0.0, fy_;
+  }
 
   // Derivative for calibration
-  if (H1)
+  if (Dcal) {
-    *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
+    *Dcal = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
+  }
 
   // Derivative for points
-  if (H2)
+  if (Dp) {
-    *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
+    *Dp = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
+  }
 
   // Regular uncalibrate after distortion
   return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
 }
 
 /* ************************************************************************* */
-Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
+Point2 Cal3DS2_Base::calibrate(const Point2& pi, OptionalJacobian<2, 9> Dcal,
+                               OptionalJacobian<2, 2> Dp) const {
   // Use the following fixed point iteration to invert the radial distortion.
   // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
 
   const Point2 invKPi((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
                       (1 / fy_) * (pi.y() - v0_));
 
-  // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
+  // initialize by ignoring the distortion at all, might be problematic for
+  // pixels around boundary
   Point2 pn = invKPi;
 
   // iterate until the uncalibrate is close to the actual pixel coordinate
   const int maxIterations = 10;
   int iteration;
   for (iteration = 0; iteration < maxIterations; ++iteration) {
-    if (distance2(uncalibrate(pn), pi) <= tol) break;
+    if (distance2(uncalibrate(pn), pi) <= tol_) break;
-    const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
+    const double px = pn.x(), py = pn.y(), xy = px * py, xx = px * px,
+                 yy = py * py;
     const double rr = xx + yy;
     const double g = (1 + k1_ * rr + k2_ * rr * rr);
     const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
@@ -154,7 +157,10 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
   }
 
   if (iteration >= maxIterations)
-    throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
+    throw std::runtime_error(
+        "Cal3DS2::calibrate fails to converge. need a better initialization");
+
+  calibrateJacobians<Cal3DS2_Base, dimension>(*this, pn, Dcal, Dp);
 
   return pn;
 }
@@ -184,8 +190,5 @@ Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const {
   DK << fx_, s_, 0.0, fy_;
   return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
 }
-
 }
 /* ************************************************************************* */
-
-

gtsam/geometry/Cal3DS2_Base.h

@@ -14,10 +14,12 @@
  * @brief Calibration of a camera with radial distortion
  * @date Feb 28, 2010
  * @author ydjian
+ * @author Varun Agrawal
  */
 
 #pragma once
 
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -31,30 +33,34 @@ namespace gtsam {
  * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
  * but using only k1,k2,p1, and p2 coefficients.
  * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
- * rr = Pn.x^2 + Pn.y^2
+ * r² = P.x² + P.y²
- * \hat{Pn} = (1 + k1*rr + k2*rr^2 ) Pn + [ 2*p1 Pn.x Pn.y + p2 (rr + 2 Pn.x^2) ;
+ * P̂ = (1 + k1*r² + k2*r⁴) P + [ (2*p1 P.x P.y) + p2 (r² + 2 Pn.x²)
- *                      p1 (rr + 2 Pn.y^2) + 2*p2 Pn.x Pn.y  ]
+ *                            p1 (r² + 2 Pn.y²) + (2*p2 Pn.x Pn.y) ]
- * pi = K*Pn
+ * pi = K*P̂
  */
-class GTSAM_EXPORT Cal3DS2_Base {
+class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
-
  protected:
-
+  double k1_ = 0.0f, k2_ = 0.0f;  ///< radial 2nd-order and 4th-order
-  double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
+  double p1_ = 0.0f, p2_ = 0.0f;  ///< tangential distortion
-  double k1_, k2_ ; // radial 2nd-order and 4th-order
+  double tol_ = 1e-5;             ///< tolerance value when calibrating
-  double p1_, p2_ ; // tangential distortion
 
  public:
+  enum { dimension = 9 };
 
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3DS2_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
+  Cal3DS2_Base() = default;
 
-  Cal3DS2_Base(double fx, double fy, double s, double u0, double v0,
+  Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
+               double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
-  fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
+      : Cal3(fx, fy, s, u0, v0),
+        k1_(k1),
+        k2_(k2),
+        p1_(p1),
+        p2_(p2),
+        tol_(tol) {}
 
   virtual ~Cal3DS2_Base() {}
 
@@ -62,37 +68,31 @@ public:
   /// @name Advanced Constructors
   /// @{
 
-  Cal3DS2_Base(const Vector &v) ;
+  Cal3DS2_Base(const Vector9& v)
+      : Cal3(v(0), v(1), v(2), v(3), v(4)),
+        k1_(v(5)),
+        k2_(v(6)),
+        p1_(v(7)),
+        p2_(v(8)) {}
 
   /// @}
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3DS2_Base& cal);
+
   /// print with optional string
-  virtual void print(const std::string& s = "") const;
+  void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
-  bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
+  bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const;
 
   /// @}
   /// @name Standard Interface
   /// @{
 
-  /// focal length x
-  inline double fx() const { return fx_;}
-
-  /// focal length x
-  inline double fy() const { return fy_;}
-
-  /// skew
-  inline double skew() const { return s_;}
-
-  /// image center in x
-  inline double px() const { return u0_;}
-
-  /// image center in y
-  inline double py() const { return v0_;}
-
   /// First distortion coefficient
   inline double k1() const { return k1_; }
 
@@ -105,9 +105,6 @@ public:
   /// Second tangential distortion coefficient
   inline double p2() const { return p2_; }
 
-  /// return calibration matrix -- not really applicable
-  Matrix3 K() const;
-
   /// return distortion parameter vector
   Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
 
@@ -121,12 +118,12 @@ public:
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in (distorted) image coordinates
    */
-  Point2 uncalibrate(const Point2& p,
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
-       OptionalJacobian<2,9> Dcal = boost::none,
                      OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
-  Point2 calibrate(const Point2& p, const double tol=1e-5) const;
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Derivative of uncalibrate wrpt intrinsic coordinates
   Matrix2 D2d_intrinsic(const Point2& p) const;
@@ -134,6 +131,12 @@ public:
   /// Derivative of uncalibrate wrpt the calibration parameters
   Matrix29 D2d_calibration(const Point2& p) const;
 
+  /// return DOF, dimensionality of tangent space
+  virtual size_t dim() const override { return Dim(); }
+
+  /// return DOF, dimensionality of tangent space
+  inline static size_t Dim() { return dimension; }
+
   /// @}
   /// @name Clone
   /// @{
@@ -146,29 +149,22 @@ public:
   /// @}
 
  private:
-
   /// @name Advanced Interface
   /// @{
 
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/)
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-  {
+    ar& boost::serialization::make_nvp(
-    ar & BOOST_SERIALIZATION_NVP(fx_);
+        "Cal3DS2_Base", boost::serialization::base_object<Cal3>(*this));
-    ar & BOOST_SERIALIZATION_NVP(fy_);
-    ar & BOOST_SERIALIZATION_NVP(s_);
-    ar & BOOST_SERIALIZATION_NVP(u0_);
-    ar & BOOST_SERIALIZATION_NVP(v0_);
     ar& BOOST_SERIALIZATION_NVP(k1_);
     ar& BOOST_SERIALIZATION_NVP(k2_);
     ar& BOOST_SERIALIZATION_NVP(p1_);
     ar& BOOST_SERIALIZATION_NVP(p2_);
+    ar& BOOST_SERIALIZATION_NVP(tol_);
   }
 
   /// @}
-
 };
-
 }
-

gtsam/geometry/Cal3Fisheye.cpp

@@ -13,6 +13,7 @@
  * @file Cal3Fisheye.cpp
  * @date Apr 8, 2020
  * @author ghaggin
+ * @author Varun Agrawal
  */
 
 #include <gtsam/base/Matrix.h>
@@ -23,18 +24,6 @@
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3Fisheye::Cal3Fisheye(const Vector9& v)
-    : fx_(v[0]),
-      fy_(v[1]),
-      s_(v[2]),
-      u0_(v[3]),
-      v0_(v[4]),
-      k1_(v[5]),
-      k2_(v[6]),
-      k3_(v[7]),
-      k4_(v[8]) {}
-
 /* ************************************************************************* */
 Vector9 Cal3Fisheye::vector() const {
   Vector9 v;
@@ -42,13 +31,6 @@ Vector9 Cal3Fisheye::vector() const {
   return v;
 }
 
-/* ************************************************************************* */
-Matrix3 Cal3Fisheye::K() const {
-  Matrix3 K;
-  K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
-  return K;
-}
-
 /* ************************************************************************* */
 double Cal3Fisheye::Scaling(double r) {
   static constexpr double threshold = 1e-8;
@@ -122,14 +104,15 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
 }
 
 /* ************************************************************************* */
-Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
+Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
+                              OptionalJacobian<2, 2> Dp) const {
   // initial gues just inverts the pinhole model
   const double u = uv.x(), v = uv.y();
   const double yd = (v - v0_) / fy_;
   const double xd = (u - s_ * yd - u0_) / fx_;
   Point2 pi(xd, yd);
 
-  // Perform newtons method, break when solution converges past tol,
+  // Perform newtons method, break when solution converges past tol_,
   // throw exception if max iterations are reached
   const int maxIterations = 10;
   int iteration;
@@ -140,7 +123,7 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
     const Point2 uv_hat = uncalibrate(pi, boost::none, jac);
 
     // Test convergence
-    if ((uv_hat - uv).norm() < tol) break;
+    if ((uv_hat - uv).norm() < tol_) break;
 
     // Newton's method update step
     pi = pi - jac.inverse() * (uv_hat - uv);
@@ -151,9 +134,19 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
         "Cal3Fisheye::calibrate fails to converge. need a better "
         "initialization");
 
+  calibrateJacobians<Cal3Fisheye, dimension>(*this, pi, Dcal, Dp);
+
   return pi;
 }
 
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3Fisheye& cal) {
+  os << (Cal3&)cal;
+  os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", k3: " << cal.k3()
+     << ", k4: " << cal.k4();
+  return os;
+}
+
 /* ************************************************************************* */
 void Cal3Fisheye::print(const std::string& s_) const {
   gtsam::print((Matrix)K(), s_ + ".K");
@@ -162,24 +155,12 @@ void Cal3Fisheye::print(const std::string& s_) const {
 
 /* ************************************************************************* */
 bool Cal3Fisheye::equals(const Cal3Fisheye& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol ||
+  const Cal3* base = dynamic_cast<const Cal3*>(&K);
-      std::abs(s_ - K.s_) > tol || std::abs(u0_ - K.u0_) > tol ||
+  return Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
-      std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
+         std::fabs(k2_ - K.k2_) < tol && std::fabs(k3_ - K.k3_) < tol &&
-      std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol ||
+         std::fabs(k4_ - K.k4_) < tol;
-      std::abs(k4_ - K.k4_) > tol)
-    return false;
-  return true;
 }
 
 /* ************************************************************************* */
-Cal3Fisheye Cal3Fisheye::retract(const Vector& d) const {
-  return Cal3Fisheye(vector() + d);
-}
 
-/* ************************************************************************* */
+}  // \ namespace gtsam
-Vector Cal3Fisheye::localCoordinates(const Cal3Fisheye& T2) const {
-  return T2.vector() - vector();
-}
-
-}  // namespace gtsam
-/* ************************************************************************* */

gtsam/geometry/Cal3Fisheye.h

@@ -14,10 +14,12 @@
  * @brief Calibration of a fisheye camera
  * @date Apr 8, 2020
  * @author ghaggin
+ * @author Varun Agrawal
  */
 
 #pragma once
 
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 #include <string>
@@ -36,43 +38,40 @@ namespace gtsam {
  * Intrinsic coordinates:
  *   [x_i;y_i] = [x/z; y/z]
  * Distorted coordinates:
- *   r^2 = (x_i)^2 + (y_i)^2
+ *   r² = (x_i)² + (y_i)²
  *   th = atan(r)
- *   th_d = th(1 + k1*th^2 + k2*th^4 + k3*th^6 + k4*th^8)
+ *   th_d = th(1 + k1*th² + k2*th⁴ + k3*th⁶ + k4*th⁸)
  *   [x_d; y_d] = (th_d / r)*[x_i; y_i]
  * Pixel coordinates:
  *   K = [fx s u0; 0 fy v0 ;0 0 1]
  *   [u; v; 1] = K*[x_d; y_d; 1]
  */
-class GTSAM_EXPORT Cal3Fisheye {
+class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
  private:
-  double fx_, fy_, s_, u0_, v0_;  // focal length, skew and principal point
+  double k1_ = 0.0f, k2_ = 0.0f;  ///< fisheye distortion coefficients
-  double k1_, k2_, k3_, k4_;      // fisheye distortion coefficients
+  double k3_ = 0.0f, k4_ = 0.0f;  ///< fisheye distortion coefficients
+  double tol_ = 1e-5;             ///< tolerance value when calibrating
 
  public:
   enum { dimension = 9 };
-  typedef boost::shared_ptr<Cal3Fisheye>
+  ///< shared pointer to fisheye calibration object
-      shared_ptr;  ///< shared pointer to fisheye calibration object
+  using shared_ptr = boost::shared_ptr<Cal3Fisheye>;
 
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3Fisheye()
+  Cal3Fisheye() = default;
-      : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {}
 
   Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
               const double v0, const double k1, const double k2,
-              const double k3, const double k4)
+              const double k3, const double k4, double tol = 1e-5)
-      : fx_(fx),
+      : Cal3(fx, fy, s, u0, v0),
-        fy_(fy),
-        s_(s),
-        u0_(u0),
-        v0_(v0),
         k1_(k1),
         k2_(k2),
         k3_(k3),
-        k4_(k4) {}
+        k4_(k4),
+        tol_(tol) {}
 
   virtual ~Cal3Fisheye() {}
 
@@ -80,27 +79,17 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// @name Advanced Constructors
   /// @{
 
-  explicit Cal3Fisheye(const Vector9& v);
+  explicit Cal3Fisheye(const Vector9& v)
+      : Cal3(v(0), v(1), v(2), v(3), v(4)),
+        k1_(v(5)),
+        k2_(v(6)),
+        k3_(v(7)),
+        k4_(v(8)) {}
 
   /// @}
   /// @name Standard Interface
   /// @{
 
-  /// focal length x
-  inline double fx() const { return fx_; }
-
-  /// focal length x
-  inline double fy() const { return fy_; }
-
-  /// skew
-  inline double skew() const { return s_; }
-
-  /// image center in x
-  inline double px() const { return u0_; }
-
-  /// image center in y
-  inline double py() const { return v0_; }
-
   /// First distortion coefficient
   inline double k1() const { return k1_; }
 
@@ -113,9 +102,6 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// Second tangential distortion coefficient
   inline double k4() const { return k4_; }
 
-  /// return calibration matrix
-  Matrix3 K() const;
-
   /// return distortion parameter vector
   Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
 
@@ -129,24 +115,34 @@ class GTSAM_EXPORT Cal3Fisheye {
    * @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image
    * coordinates [u; v]
    * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters (fx, fy, ...,
+   * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters
-   * k4)
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
    * @return point in (distorted) image coordinates
    */
   Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
                      OptionalJacobian<2, 2> Dp = boost::none) const;
 
-  /// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
+  /**
-  /// y_i]
+   * Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
-  Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
+   * y_i]
+   * @param p point in image coordinates
+   * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
+   * @return point in intrinsic coordinates
+   */
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// @}
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3Fisheye& cal);
+
   /// print with optional string
-  virtual void print(const std::string& s = "") const;
+  virtual void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3Fisheye& K, double tol = 10e-9) const;
@@ -155,17 +151,21 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// @name Manifold
   /// @{
 
+  /// Return dimensions of calibration manifold object
+  virtual size_t dim() const override { return Dim(); }
+
+  /// Return dimensions of calibration manifold object
+  inline static size_t Dim() { return dimension; }
+
   /// Given delta vector, update calibration
-  Cal3Fisheye retract(const Vector& d) const;
+  inline Cal3Fisheye retract(const Vector& d) const {
+    return Cal3Fisheye(vector() + d);
+  }
 
   /// Given a different calibration, calculate update to obtain it
-  Vector localCoordinates(const Cal3Fisheye& T2) const;
+  Vector localCoordinates(const Cal3Fisheye& T2) const {
-
+    return T2.vector() - vector();
-  /// Return dimensions of calibration manifold object
+  }
-  virtual size_t dim() const { return 9; }
-
-  /// Return dimensions of calibration manifold object
-  static size_t Dim() { return 9; }
 
   /// @}
   /// @name Clone
@@ -186,11 +186,8 @@ class GTSAM_EXPORT Cal3Fisheye {
   friend class boost::serialization::access;
   template <class Archive>
   void serialize(Archive& ar, const unsigned int /*version*/) {
-    ar& BOOST_SERIALIZATION_NVP(fx_);
+    ar& boost::serialization::make_nvp(
-    ar& BOOST_SERIALIZATION_NVP(fy_);
+        "Cal3Fisheye", boost::serialization::base_object<Cal3>(*this));
-    ar& BOOST_SERIALIZATION_NVP(s_);
-    ar& BOOST_SERIALIZATION_NVP(u0_);
-    ar& BOOST_SERIALIZATION_NVP(v0_);
     ar& BOOST_SERIALIZATION_NVP(k1_);
     ar& BOOST_SERIALIZATION_NVP(k2_);
     ar& BOOST_SERIALIZATION_NVP(k3_);

gtsam/geometry/Cal3Unified.cpp

@@ -13,21 +13,18 @@
  * @file Cal3Unified.cpp
  * @date Mar 8, 2014
  * @author Jing Dong
+ * @author Varun Agrawal
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
-#include <gtsam/geometry/Point2.h>
+#include <gtsam/base/Vector.h>
 #include <gtsam/geometry/Cal3Unified.h>
+#include <gtsam/geometry/Point2.h>
 
 #include <cmath>
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3Unified::Cal3Unified(const Vector &v):
-    Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {}
-
 /* ************************************************************************* */
 Vector10 Cal3Unified::vector() const {
   Vector10 v;
@@ -35,6 +32,13 @@ Vector10 Cal3Unified::vector() const {
   return v;
 }
 
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3Unified& cal) {
+  os << (Cal3DS2_Base&)cal;
+  os << ", xi: " << cal.xi();
+  return os;
+}
+
 /* ************************************************************************* */
 void Cal3Unified::print(const std::string& s) const {
   Base::print(s);
@@ -43,20 +47,14 @@ void Cal3Unified::print(const std::string& s) const {
 
 /* ************************************************************************* */
 bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
+  const Cal3DS2_Base* base = dynamic_cast<const Cal3DS2_Base*>(&K);
-      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
+  return Cal3DS2_Base::equals(*base, tol) && std::fabs(xi_ - K.xi_) < tol;
-      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol ||
-      std::abs(xi_ - K.xi_) > tol)
-    return false;
-  return true;
 }
 
 /* ************************************************************************* */
 // todo: make a fixed sized jacobian version of this
-Point2 Cal3Unified::uncalibrate(const Point2& p,
+Point2 Cal3Unified::uncalibrate(const Point2& p, OptionalJacobian<2, 10> Dcal,
-       OptionalJacobian<2,10> H1,
+                                OptionalJacobian<2, 2> Dp) const {
-       OptionalJacobian<2,2> H2) const {
-
   // this part of code is modified from Cal3DS2,
   // since the second part of this model (after project to normalized plane)
   // is same as Cal3DS2
@@ -78,16 +76,16 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
   Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
 
   // Inlined derivative for calibration
-  if (H1) {
+  if (Dcal) {
     // part1
     Vector2 DU;
     DU << -xs * sqrt_nx * xi_sqrt_nx2,  //
         -ys * sqrt_nx * xi_sqrt_nx2;
-    *H1 << H1base, H2base * DU;
+    *Dcal << H1base, H2base * DU;
   }
 
   // Inlined derivative for points
-  if (H2) {
+  if (Dp) {
     // part1
     const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
     const double mid = -(xi * xs * ys) * denom;
@@ -95,24 +93,27 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
     DU << (sqrt_nx + xi * (ys * ys + 1)) * denom, mid,  //
         mid, (sqrt_nx + xi * (xs * xs + 1)) * denom;
 
-    *H2 << H2base * DU;
+    *Dp << H2base * DU;
   }
 
   return puncalib;
 }
 
 /* ************************************************************************* */
-Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const {
+Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal,
-
+                              OptionalJacobian<2, 2> Dp) const {
   // calibrate point to Nplane use base class::calibrate()
-  Point2 pnplane = Base::calibrate(pi, tol);
+  Point2 pnplane = Base::calibrate(pi);
 
   // call nplane to space
-  return this->nPlaneToSpace(pnplane);
+  Point2 pn = this->nPlaneToSpace(pnplane);
+
+  calibrateJacobians<Cal3Unified, dimension>(*this, pn, Dcal, Dp);
+
+  return pn;
 }
 /* ************************************************************************* */
 Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
-
   const double x = p.x(), y = p.y();
   const double xy2 = x * x + y * y;
   const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1);
@@ -122,7 +123,6 @@ Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
 
 /* ************************************************************************* */
 Point2 Cal3Unified::spaceToNPlane(const Point2& p) const {
-
   const double x = p.x(), y = p.y();
   const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1);
 
@@ -135,11 +135,10 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const {
 }
 
 /* ************************************************************************* */
-Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
+Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
   return T2.vector() - vector();
 }
 
-}
 /* ************************************************************************* */
 
-
+}  // \ namespace gtsam

gtsam/geometry/Cal3Unified.h

@@ -14,6 +14,7 @@
  * @brief Unified Calibration Model, see Mei07icra for details
  * @date Mar 8, 2014
  * @author Jing Dong
+ * @author Varun Agrawal
  */
 
 /**
@@ -27,40 +28,39 @@
 namespace gtsam {
 
 /**
- * @brief Calibration of a omni-directional camera with mirror + lens radial distortion
+ * @brief Calibration of a omni-directional camera with mirror + lens radial
+ * distortion
  * @addtogroup geometry
  * \nosubgrouping
  *
  * Similar to Cal3DS2, does distortion but has additional mirror parameter xi
  * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
- * Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
+ * Pn = [ P.x / (1 + xi * \sqrt{P.x² + P.y² + 1}), P.y / (1 + xi * \sqrt{P.x² +
- * rr = Pn.x^2 + Pn.y^2
+ * P.y² + 1})]
- * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
+ * r² = Pn.x² + Pn.y²
- *                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
+ * \hat{pn} = (1 + k1*r² + k2*r⁴ ) pn + [ 2*k3 pn.x pn.y + k4 (r² + 2 Pn.x²) ;
+ *                      k3 (rr + 2 Pn.y²) + 2*k4 pn.x pn.y  ]
  * pi = K*pn
  */
 class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
-
+  using This = Cal3Unified;
-  typedef Cal3Unified This;
+  using Base = Cal3DS2_Base;
-  typedef Cal3DS2_Base Base;
 
  private:
-
+  double xi_ = 0.0f;  ///< mirror parameter
-  double xi_;  // mirror parameter
 
  public:
-
   enum { dimension = 10 };
 
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3Unified() : Base(), xi_(0) {}
+  Cal3Unified() = default;
 
-  Cal3Unified(double fx, double fy, double s, double u0, double v0,
+  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) :
+              double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0)
-        Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
+      : Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
 
   virtual ~Cal3Unified() {}
 
@@ -68,12 +68,17 @@ public:
   /// @name Advanced Constructors
   /// @{
 
-  Cal3Unified(const Vector &v) ;
+  Cal3Unified(const Vector10& v)
+      : Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {}
 
   /// @}
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3Unified& cal);
+
   /// print with optional string
   void print(const std::string& s = "") const override;
 
@@ -87,6 +92,9 @@ public:
   /// mirror parameter
   inline double xi() const { return xi_; }
 
+  /// Return all parameters as a vector
+  Vector10 vector() const;
+
   /**
    * convert intrinsic coordinates xy to image coordinates uv
    * @param p point in intrinsic coordinates
@@ -99,7 +107,8 @@ public:
                      OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Conver a pixel coordinate to ideal coordinate
-  Point2 calibrate(const Point2& p, const double tol=1e-5) const;
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Convert a 3D point to normalized unit plane
   Point2 spaceToNPlane(const Point2& p) const;
@@ -115,31 +124,25 @@ public:
   Cal3Unified retract(const Vector& d) const;
 
   /// Given a different calibration, calculate update to obtain it
-  Vector10 localCoordinates(const Cal3Unified& T2) const ;
+  Vector localCoordinates(const Cal3Unified& T2) const;
 
   /// Return dimensions of calibration manifold object
-  virtual size_t dim() const { return dimension ; }
+  virtual size_t dim() const override { return Dim(); }
 
   /// Return dimensions of calibration manifold object
-  static size_t Dim() { return dimension; }
+  inline static size_t Dim() { return dimension; }
-
-  /// Return all parameters as a vector
-  Vector10 vector() const ;
 
   /// @}
 
  private:
-
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/)
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-  {
+    ar& boost::serialization::make_nvp(
-    ar & boost::serialization::make_nvp("Cal3Unified",
+        "Cal3Unified", boost::serialization::base_object<Cal3DS2_Base>(*this));
-        boost::serialization::base_object<Cal3DS2_Base>(*this));
     ar& BOOST_SERIALIZATION_NVP(xi_);
   }
-
 };
 
 template <>
@@ -147,6 +150,4 @@ struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
 
 template <>
 struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
-
 }
-

gtsam/geometry/Cal3_S2.cpp

@@ -22,69 +22,30 @@
 #include <iostream>
 
 namespace gtsam {
-using namespace std;
 
 /* ************************************************************************* */
-Cal3_S2::Cal3_S2(double fov, int w, int h) :
+std::ostream& operator<<(std::ostream& os, const Cal3_S2& cal) {
-    s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) {
+  // Use the base class version since it is identical.
-  double a = fov * M_PI / 360.0; // fov/2 in radians
+  os << (Cal3&)cal;
-  fx_ = (double) w / (2.0 * tan(a)); //    old formula: fx_ = (double) w * tan(a);
-  fy_ = fx_;
-}
-
-/* ************************************************************************* */
-Cal3_S2::Cal3_S2(const std::string &path) :
-    fx_(320), fy_(320), s_(0), u0_(320), v0_(140) {
-
-  char buffer[200];
-  buffer[0] = 0;
-  sprintf(buffer, "%s/calibration_info.txt", path.c_str());
-  std::ifstream infile(buffer, std::ios::in);
-
-  if (infile)
-    infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
-  else {
-    throw std::runtime_error("Cal3_S2: Unable to load the calibration");
-  }
-
-  infile.close();
-}
-
-/* ************************************************************************* */
-ostream& operator<<(ostream& os, const Cal3_S2& cal) {
-  os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px()
-     << ", py:" << cal.py() << "}";
   return os;
 }
 
 /* ************************************************************************* */
 void Cal3_S2::print(const std::string& s) const {
-  gtsam::print((Matrix)matrix(), s);
+  gtsam::print((Matrix)K(), s);
 }
 
 /* ************************************************************************* */
 bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol)
+  return Cal3::equals(K, tol);
-    return false;
-  if (std::abs(fy_ - K.fy_) > tol)
-    return false;
-  if (std::abs(s_ - K.s_) > tol)
-    return false;
-  if (std::abs(u0_ - K.u0_) > tol)
-    return false;
-  if (std::abs(v0_ - K.v0_) > tol)
-    return false;
-  return true;
 }
 
 /* ************************************************************************* */
 Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
                             OptionalJacobian<2, 2> Dp) const {
   const double x = p.x(), y = p.y();
-  if (Dcal)
+  if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
-    *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
+  if (Dp) *Dp << fx_, s_, 0.0, fy_;
-  if (Dp)
-    *Dp << fx_, s_, 0.0, fy_;
   return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
 }
 
@@ -94,22 +55,21 @@ Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal,
   const double u = p.x(), v = p.y();
   double delta_u = u - u0_, delta_v = v - v0_;
   double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
-    double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
+  double inv_fy_delta_v = inv_fy * delta_v;
-    Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v),
+  double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
-                  inv_fy_delta_v);
+
-    if(Dcal)
+  Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
-       *Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,  -inv_fx * point.y(),
+  if (Dcal) {
-            -inv_fx,  inv_fx_s_inv_fy,
+    *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
-            0, -inv_fy * point.y(), 0,  0, -inv_fy;
+        -inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, -inv_fy * point.y(),
-    if(Dp)
+        0, 0, -inv_fy;
-        *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
+  }
+  if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
   return point;
 }
 
 /* ************************************************************************* */
-Vector3 Cal3_S2::calibrate(const Vector3& p) const {
+Vector3 Cal3_S2::calibrate(const Vector3& p) const { return inverse() * p; }
-  return matrix_inverse() * p;
-}
 
 /* ************************************************************************* */
 

gtsam/geometry/Cal3_S2.h

@@ -21,6 +21,7 @@
 
 #pragma once
 
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -30,31 +31,25 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Cal3_S2 {
+class GTSAM_EXPORT Cal3_S2 : public Cal3 {
-private:
-  double fx_, fy_, s_, u0_, v0_;
-
  public:
   enum { dimension = 5 };
-  typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
+
+  ///< shared pointer to calibration object
+  using shared_ptr = boost::shared_ptr<Cal3_S2>;
 
   /// @name Standard Constructors
   /// @{
 
   /// Create a default calibration that leaves coordinates unchanged
-  Cal3_S2() :
+  Cal3_S2() = default;
-      fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {
-  }
 
   /// constructor from doubles
-  Cal3_S2(double fx, double fy, double s, double u0, double v0) :
+  Cal3_S2(double fx, double fy, double s, double u0, double v0)
-      fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
+      : Cal3(fx, fy, s, u0, v0) {}
-  }
 
   /// constructor from vector
-  Cal3_S2(const Vector &d) :
+  Cal3_S2(const Vector5& d) : Cal3(d) {}
-      fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {
-  }
 
   /**
    * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
@@ -62,99 +57,12 @@ public:
    * @param w image width
    * @param h image height
    */
-  Cal3_S2(double fov, int w, int h);
+  Cal3_S2(double fov, int w, int h) : Cal3(fov, w, h) {}
-
-  /// @}
-  /// @name Advanced Constructors
-  /// @{
-
-  /// load calibration from location (default name is calibration_info.txt)
-  Cal3_S2(const std::string &path);
-
-  /// @}
-  /// @name Testable
-  /// @{
-
-  /// Output stream operator
-  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Cal3_S2& cal);
-
-  /// print with optional string
-  void print(const std::string& s = "Cal3_S2") const;
-
-  /// Check if equal up to specified tolerance
-  bool equals(const Cal3_S2& K, double tol = 10e-9) const;
-
-  /// @}
-  /// @name Standard Interface
-  /// @{
-
-  /// focal length x
-  inline double fx() const {
-    return fx_;
-  }
-
-  /// focal length y
-  inline double fy() const {
-    return fy_;
-  }
-
-  /// aspect ratio
-  inline double aspectRatio() const {
-    return fx_/fy_;
-  }
-
-  /// skew
-  inline double skew() const {
-    return s_;
-  }
-
-  /// image center in x
-  inline double px() const {
-    return u0_;
-  }
-
-  /// image center in y
-  inline double py() const {
-    return v0_;
-  }
-
-  /// return the principal point
-  Point2 principalPoint() const {
-    return Point2(u0_, v0_);
-  }
-
-  /// vectorized form (column-wise)
-  Vector5 vector() const {
-    Vector5 v;
-    v << fx_, fy_, s_, u0_, v0_;
-    return v;
-  }
-
-  /// return calibration matrix K
-  Matrix3 K() const {
-    Matrix3 K;
-    K <<  fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
-    return K;
-  }
-
-  /** @deprecated The following function has been deprecated, use K above */
-  Matrix3 matrix() const {
-    return K();
-  }
-
-  /// return inverted calibration matrix inv(K)
-  Matrix3 matrix_inverse() const {
-    const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
-    Matrix3 K_inverse;
-    K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
-        1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0;
-    return K_inverse;
-  }
 
   /**
-   * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
+   * Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
    * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
+   * @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in image coordinates
    */
@@ -162,9 +70,9 @@ public:
                      OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /**
-   * convert image coordinates uv to intrinsic coordinates xy
+   * Convert image coordinates uv to intrinsic coordinates xy
    * @param p point in image coordinates
-   * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
+   * @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in intrinsic coordinates
    */
@@ -172,31 +80,42 @@ public:
                    OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /**
-   * convert homogeneous image coordinates to intrinsic coordinates
+   * Convert homogeneous image coordinates to intrinsic coordinates
    * @param p point in image coordinates
    * @return point in intrinsic coordinates
    */
   Vector3 calibrate(const Vector3& p) const;
 
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3_S2& cal);
+
+  /// print with optional string
+  void print(const std::string& s = "Cal3_S2") const override;
+
+  /// Check if equal up to specified tolerance
+  bool equals(const Cal3_S2& K, double tol = 10e-9) const;
+
   /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
   inline Cal3_S2 between(const Cal3_S2& q,
                          OptionalJacobian<5, 5> H1 = boost::none,
                          OptionalJacobian<5, 5> H2 = boost::none) const {
     if (H1) *H1 = -I_5x5;
     if (H2) *H2 = I_5x5;
-    return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
+    return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_,
+                   q.v0_ - v0_);
   }
 
-
   /// @}
   /// @name Manifold
   /// @{
 
   /// return DOF, dimensionality of tangent space
-  inline size_t dim() const { return dimension; }
+  inline static size_t Dim() { return dimension; }
-
-  /// return DOF, dimensionality of tangent space
-  static size_t Dim() { return dimension; }
 
   /// Given 5-dim tangent vector, create new calibration
   inline Cal3_S2 retract(const Vector& d) const {
@@ -213,20 +132,15 @@ public:
   /// @{
 
  private:
-
   /// Serialization function
   friend class boost::serialization::access;
   template <class Archive>
   void serialize(Archive& ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_NVP(fx_);
+    ar& boost::serialization::make_nvp(
-    ar & BOOST_SERIALIZATION_NVP(fy_);
+        "Cal3_S2", boost::serialization::base_object<Cal3>(*this));
-    ar & BOOST_SERIALIZATION_NVP(s_);
-    ar & BOOST_SERIALIZATION_NVP(u0_);
-    ar & BOOST_SERIALIZATION_NVP(v0_);
   }
 
   /// @}
-
 };
 
 template <>

gtsam/geometry/Cal3_S2Stereo.cpp

@@ -20,18 +20,54 @@
 #include <iostream>
 
 namespace gtsam {
-using namespace std;
+
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3_S2Stereo& cal) {
+  os << (Cal3_S2&)cal;
+  os << ", b: " << cal.baseline();
+  return os;
+}
 
 /* ************************************************************************* */
 void Cal3_S2Stereo::print(const std::string& s) const {
-     K_.print(s+"K: ");
+  std::cout << s << (s != "" ? " " : "");
-     std::cout << s << "Baseline: " << b_ << std::endl;
+  std::cout << "K: " << (Matrix)K() << std::endl;
+  std::cout << "Baseline: " << b_ << std::endl;
 }
 
 /* ************************************************************************* */
 bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
-  if (std::abs(b_ - other.b_) > tol) return false;
+  const Cal3_S2* base = dynamic_cast<const Cal3_S2*>(&other);
-  return K_.equals(other.K_,tol);
+  return (Cal3_S2::equals(*base, tol) &&
+          std::fabs(b_ - other.baseline()) < tol);
+}
+
+/* ************************************************************************* */
+Point2 Cal3_S2Stereo::uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal,
+                            OptionalJacobian<2, 2> Dp) const {
+  const double x = p.x(), y = p.y();
+  if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, 0.0, y, 0.0, 0.0, 1.0, 0.0;
+  if (Dp) *Dp << fx_, s_, 0.0, fy_;
+  return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
+}
+
+/* ************************************************************************* */
+Point2 Cal3_S2Stereo::calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal,
+                          OptionalJacobian<2, 2> Dp) const {
+  const double u = p.x(), v = p.y();
+  double delta_u = u - u0_, delta_v = v - v0_;
+  double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
+  double inv_fy_delta_v = inv_fy * delta_v;
+  double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
+
+  Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
+  if (Dcal) {
+    *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
+        -inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, 0,
+        -inv_fy * point.y(), 0, 0, -inv_fy, 0;
+  }
+  if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
+  return point;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Cal3_S2Stereo.h

@@ -27,43 +27,71 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-  class GTSAM_EXPORT Cal3_S2Stereo {
+class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
  private:
-
+  double b_ = 1.0f;  ///< Stereo baseline.
-    Cal3_S2 K_;
-    double b_;
 
  public:
-
   enum { dimension = 6 };
-    typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr;  ///< shared pointer to stereo calibration object
+
+  ///< shared pointer to stereo calibration object
+  using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
 
   /// @name Standard Constructors
   /// @
 
   /// default calibration leaves coordinates unchanged
-    Cal3_S2Stereo() :
+  Cal3_S2Stereo() = default;
-      K_(1, 1, 0, 0, 0), b_(1.0) {
-    }
 
   /// constructor from doubles
-    Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) :
+  Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
-      K_(fx, fy, s, u0, v0), b_(b) {
+      : Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
-    }
 
   /// constructor from vector
-    Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){}
+  Cal3_S2Stereo(const Vector6& d)
+      : Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
 
   /// easy constructor; field-of-view in degrees, assumes zero skew
-    Cal3_S2Stereo(double fov, int w, int h, double b) :
+  Cal3_S2Stereo(double fov, int w, int h, double b)
-      K_(fov, w, h), b_(b) {
+      : Cal3_S2(fov, w, h), b_(b) {}
-    }
+
+  /**
+   * Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
+   * @param p point in intrinsic coordinates
+   * @param Dcal optional 2*6 Jacobian wrpt Cal3_S2Stereo parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in image coordinates
+   */
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
+
+  /**
+   * Convert image coordinates uv to intrinsic coordinates xy
+   * @param p point in image coordinates
+   * @param Dcal optional 2*6 Jacobian wrpt Cal3_S2Stereo parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in intrinsic coordinates
+   */
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
+
+  /**
+   * Convert homogeneous image coordinates to intrinsic coordinates
+   * @param p point in image coordinates
+   * @return point in intrinsic coordinates
+   */
+  Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); }
 
   /// @}
   /// @name Testable
   /// @{
 
-    void print(const std::string& s = "") const;
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3_S2Stereo& cal);
+
+  /// print with optional string
+  void print(const std::string& s = "") const override;
 
   /// Check if equal up to specified tolerance
   bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
@@ -73,28 +101,10 @@ namespace gtsam {
   /// @{
 
   /// return calibration, same for left and right
-    const Cal3_S2& calibration() const { return K_;}
+  const Cal3_S2& calibration() const { return *this; }
 
   /// return calibration matrix K, same for left and right
-    Matrix matrix() const { return K_.matrix();}
+  Matrix3 K() const override { return Cal3_S2::K(); }
-
-    /// focal length x
-    inline double fx() const { return K_.fx();}
-
-    /// focal length x
-    inline double fy() const { return K_.fy();}
-
-    /// skew
-    inline double skew() const { return K_.skew();}
-
-    /// image center in x
-    inline double px() const { return K_.px();}
-
-    /// image center in y
-    inline double py() const { return K_.py();}
-
-    /// return the principal point
-    Point2 principalPoint() const { return K_.principalPoint();}
 
   /// return baseline
   inline double baseline() const { return b_; }
@@ -102,7 +112,7 @@ namespace gtsam {
   /// vectorized form (column-wise)
   Vector6 vector() const {
     Vector6 v;
-      v << K_.vector(), b_;
+    v << Cal3_S2::vector(), b_;
     return v;
   }
 
@@ -111,14 +121,15 @@ namespace gtsam {
   /// @{
 
   /// return DOF, dimensionality of tangent space
-    inline size_t dim() const { return dimension; }
+  inline size_t dim() const override { return Dim(); }
 
   /// return DOF, dimensionality of tangent space
-    static size_t Dim() { return dimension; }
+  inline static size_t Dim() { return dimension; }
 
   /// Given 6-dim tangent vector, create new calibration
   inline Cal3_S2Stereo retract(const Vector& d) const {
-      return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5));
+    return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3),
+                         py() + d(4), b_ + d(5));
   }
 
   /// Unretraction for the calibration
@@ -126,7 +137,6 @@ namespace gtsam {
     return T2.vector() - vector();
   }
 
-
   /// @}
   /// @name Advanced Interface
   /// @{
@@ -135,19 +145,17 @@ namespace gtsam {
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
-    void serialize(Archive & ar, const unsigned int /*version*/)
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-    {
+    ar& boost::serialization::make_nvp(
-      ar & BOOST_SERIALIZATION_NVP(K_);
+        "Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
     ar& BOOST_SERIALIZATION_NVP(b_);
   }
   /// @}
-
 };
 
 // Define GTSAM traits
 template <>
-  struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
+struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
-  };
 
 template <>
 struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {

gtsam/geometry/Line3.h

@@ -102,6 +102,27 @@ class Line3 {
    */
   Point3 point(double distance = 0) const;
 
+  /**
+   * Return the rotation of the line.
+   */
+  inline Rot3 R() const {
+    return R_;
+  }
+
+  /**
+   * Return the x-coordinate of the intersection of the line with the xy plane.
+   */
+  inline double a() const {
+    return a_;
+  }
+
+  /**
+   * Return the y-coordinate of the intersection of the line with the xy plane.
+   */
+  inline double b() const {
+    return b_;
+  }
+
   /**
    * Transform a line from world to camera frame
    * @param wTc - Pose3 of camera in world frame

gtsam/geometry/Point3.h

@@ -61,7 +61,7 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
 
 /// mean
 template <class CONTAINER>
-GTSAM_EXPORT Point3 mean(const CONTAINER& points) {
+Point3 mean(const CONTAINER& points) {
   if (points.size() == 0) throw std::invalid_argument("Point3::mean input container is empty");
   Point3 sum(0, 0, 0);
   sum = std::accumulate(points.begin(), points.end(), sum);

gtsam/geometry/Pose2.cpp

@@ -48,7 +48,13 @@ Matrix3 Pose2::matrix() const {
 
 /* ************************************************************************* */
 void Pose2::print(const string& s) const {
-  cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;
+  cout << s << this << endl;
+}
+
+/* ************************************************************************* */
+std::ostream &operator<<(std::ostream &os, const Pose2& pose) {
+  os << "(" << pose.x() << ", " << pose.y() << ", " << pose.theta() << ")";
+  return os;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Pose2.h

@@ -287,6 +287,10 @@ public:
    */
   static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); }
 
+  /// Output stream operator
+  GTSAM_EXPORT
+  friend std::ostream &operator<<(std::ostream &os, const Pose2& p);
+
   /// @}
 
  private:

gtsam/geometry/Pose3.cpp

@@ -106,8 +106,8 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
 }
 
 /* ************************************************************************* */
-void Pose3::print(const string& s) const {
+void Pose3::print(const std::string& s) const {
-  cout << (s.empty() ? s : s + " ") << *this << endl;
+  std::cout << (s.empty() ? s : s + " ") << *this << std::endl;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Pose3.h

@@ -112,6 +112,25 @@ public:
     return Pose3(R_ * T.R_, t_ + R_ * T.t_);
   }
 
+  /**
+   * Interpolate between two poses via individual rotation and translation
+   * interpolation.
+   *
+   * The default "interpolate" method defined in Lie.h minimizes the geodesic
+   * distance on the manifold, leading to a screw motion interpolation in
+   * Cartesian space, which might not be what is expected.
+   * In contrast, this method executes a straight line interpolation for the
+   * translation, while still using interpolate (aka "slerp") for the rotational
+   * component. This might be more intuitive in many applications.
+   *
+   * @param T End point of interpolation.
+   * @param t A value in [0, 1].
+   */
+  Pose3 interpolateRt(const Pose3& T, double t) const {
+    return Pose3(interpolate<Rot3>(R_, T.R_, t),
+                 interpolate<Point3>(t_, T.t_, t));
+  }
+
   /// @}
   /// @name Lie Group
   /// @{

gtsam/geometry/SOn-inl.h

@@ -22,8 +22,6 @@
 
 #include <iostream>
 
-using namespace std;
-
 namespace gtsam {
 
 // Implementation for N>=5 just uses dynamic version
@@ -108,7 +106,7 @@ typename SO<N>::VectorN2 SO<N>::vec(
 
 template <int N>
 void SO<N>::print(const std::string& s) const {
-    cout << s << matrix_ << endl;
+    std::cout << s << matrix_ << std::endl;
 }
 
 }  // namespace gtsam

gtsam/geometry/SimpleCamera.cpp

@@ -21,6 +21,7 @@
 
 namespace gtsam {
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
   SimpleCamera simpleCamera(const Matrix34& P) {
 
     // P = [A|a] = s K cRw [I|-T], with s the unknown scale
@@ -45,5 +46,6 @@ namespace gtsam {
     return SimpleCamera(Pose3(wRc, T),
         Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
   }
+#endif
 
 }

gtsam/geometry/SimpleCamera.h

@@ -19,14 +19,22 @@
 #pragma once
 
 #include <gtsam/geometry/BearingRange.h>
-#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/PinholeCamera.h>
 
 namespace gtsam {
 
-  /// A simple camera class with a Cal3_S2 calibration
+  /// Convenient aliases for Pinhole camera classes with different calibrations.
-typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
+  /// Also needed as forward declarations in the wrapper.
+  using PinholeCameraCal3_S2 = gtsam::PinholeCamera<gtsam::Cal3_S2>;
+  using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
+  using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
+  using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
 /**
  * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x
  * Use PinholeCameraCal3_S2 instead
@@ -140,4 +148,6 @@ struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {};
 template <typename T>
 struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
 
+#endif
+
 }  // namespace gtsam

gtsam/geometry/tests/testCal3Bundler.cpp

@@ -11,11 +11,12 @@
 
 /**
  * @file  testCal3Bundler.cpp
- * @brief Unit tests for transform derivatives
+ * @brief Unit tests for Bundler calibration model.
  */
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 
@@ -28,8 +29,7 @@ static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
 static Point2 p(2, 3);
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, vector)
+TEST(Cal3Bundler, vector) {
-{
   Cal3Bundler K;
   Vector expected(3);
   expected << 1, 0, 0;
@@ -37,8 +37,7 @@ TEST( Cal3Bundler, vector)
 }
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, uncalibrate)
+TEST(Cal3Bundler, uncalibrate) {
-{
   Vector v = K.vector();
   double r = p.x() * p.x() + p.y() * p.y();
   double g = v[0] * (1 + v[1] * r + v[2] * r * r);
@@ -47,8 +46,7 @@ TEST( Cal3Bundler, uncalibrate)
   CHECK(assert_equal(expected, actual));
 }
 
-TEST( Cal3Bundler, calibrate )
+TEST(Cal3Bundler, calibrate) {
-{
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
   Point2 pn_hat = K.calibrate(pi);
@@ -56,13 +54,16 @@ TEST( Cal3Bundler, calibrate )
 }
 
 /* ************************************************************************* */
-Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
+Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
 
-Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
+Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
+  return k.calibrate(pt);
+}
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, Duncalibrate)
+TEST(Cal3Bundler, Duncalibrate) {
-{
   Matrix Dcal, Dp;
   Point2 actual = K.uncalibrate(p, Dcal, Dp);
   Point2 expected(2182, 3773);
@@ -74,8 +75,7 @@ TEST( Cal3Bundler, Duncalibrate)
 }
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, Dcalibrate)
+TEST(Cal3Bundler, Dcalibrate) {
-{
   Matrix Dcal, Dp;
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
@@ -88,16 +88,17 @@ TEST( Cal3Bundler, Dcalibrate)
 }
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, assert_equal)
+TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); }
-{
-  CHECK(assert_equal(K,K,1e-7));
-}
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, retract)
+TEST(Cal3Bundler, retract) {
-{
   Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000);
-  Vector d(3);
+  EXPECT_LONGS_EQUAL(3, expected.dim());
+
+  EXPECT_LONGS_EQUAL(Cal3Bundler::Dim(), 3);
+  EXPECT_LONGS_EQUAL(expected.dim(), 3);
+
+  Vector3 d;
   d << 10, 1e-3, 1e-3;
   Cal3Bundler actual = K.retract(d);
   CHECK(assert_equal(expected, actual, 1e-7));
@@ -105,5 +106,18 @@ TEST( Cal3Bundler, retract)
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+TEST(Cal3_S2, Print) {
+  Cal3Bundler cal(1, 2, 3, 4, 5);
+  std::stringstream os;
+  os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()
+     << ", px: " << cal.px() << ", py: " << cal.py();
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

gtsam/geometry/tests/testCal3DFisheye.cpp

@@ -10,12 +10,13 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file  testCal3DFisheye.cpp
+ * @file  testCal3Fisheye.cpp
  * @brief Unit tests for fisheye calibration class
  * @author ghaggin
  */
 
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Point3.h>
@@ -41,7 +42,11 @@ TEST(Cal3Fisheye, retract) {
   Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4,
                        K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8,
                        K.k4() + 9);
-  Vector d(9);
+
+  EXPECT_LONGS_EQUAL(Cal3Fisheye::Dim(), 9);
+  EXPECT_LONGS_EQUAL(expected.dim(), 9);
+
+  Vector9 d;
   d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
   Cal3Fisheye actual = K.retract(d);
   CHECK(assert_equal(expected, actual, 1e-7));
@@ -181,6 +186,33 @@ TEST(Cal3Fisheye, calibrate3) {
   CHECK(assert_equal(xi_hat, xi));
 }
 
+Point2 calibrate_(const Cal3Fisheye& k, const Point2& pt) {
+  return k.calibrate(pt);
+}
+
+/* ************************************************************************* */
+TEST(Cal3Fisheye, Dcalibrate) {
+  Point2 p(0.5, 0.5);
+  Point2 pi = K.uncalibrate(p);
+  Matrix Dcal, Dp;
+  K.calibrate(pi, Dcal, Dp);
+  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
+  CHECK(assert_equal(numerical1, Dcal, 1e-5));
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
+  CHECK(assert_equal(numerical2, Dp, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(Cal3Fisheye, Print) {
+  Cal3Fisheye cal(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  std::stringstream os;
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
+     << ", k2: " << cal.k2() << ", k3: " << cal.k3() << ", k4: " << cal.k4();
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/geometry/tests/testCal3DS2.cpp

@@ -11,12 +11,12 @@
 
 /**
  * @file  testCal3DS2.cpp
- * @brief Unit tests for transform derivatives
+ * @brief Unit tests for Cal3DS2 calibration model.
  */
 
-
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3DS2.h>
 
@@ -25,12 +25,12 @@ using namespace gtsam;
 GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2)
 GTSAM_CONCEPT_MANIFOLD_INST(Cal3DS2)
 
-static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
+static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3,
+                 4.0 * 1e-3);
 static Point2 p(2, 3);
 
 /* ************************************************************************* */
-TEST( Cal3DS2, uncalibrate)
+TEST(Cal3DS2, Uncalibrate) {
-{
   Vector k = K.k();
   double r = p.x() * p.x() + p.y() * p.y();
   double g = 1 + k[0] * r + k[1] * r * r;
@@ -43,19 +43,19 @@ TEST( Cal3DS2, uncalibrate)
   CHECK(assert_equal(q, p_i));
 }
 
-TEST( Cal3DS2, calibrate )
+TEST(Cal3DS2, Calibrate) {
-{
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
   Point2 pn_hat = K.calibrate(pi);
   CHECK(traits<Point2>::Equals(pn, pn_hat, 1e-5));
 }
 
-Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); }
+Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
 
 /* ************************************************************************* */
-TEST( Cal3DS2, Duncalibrate1)
+TEST(Cal3DS2, Duncalibrate1) {
-{
   Matrix computed;
   K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
@@ -65,27 +65,43 @@ TEST( Cal3DS2, Duncalibrate1)
 }
 
 /* ************************************************************************* */
-TEST( Cal3DS2, Duncalibrate2)
+TEST(Cal3DS2, Duncalibrate2) {
-{
+  Matrix computed;
-  Matrix computed; K.uncalibrate(p, boost::none, computed);
+  K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
   CHECK(assert_equal(numerical, computed, 1e-5));
   Matrix separate = K.D2d_intrinsic(p);
   CHECK(assert_equal(numerical, separate, 1e-5));
 }
 
-/* ************************************************************************* */
+Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
-TEST( Cal3DS2, assert_equal)
+  return k.calibrate(pt);
-{
-  CHECK(assert_equal(K,K,1e-5));
 }
 
 /* ************************************************************************* */
-TEST( Cal3DS2, retract)
+TEST(Cal3DS2, Dcalibrate) {
-{
+  Point2 pn(0.5, 0.5);
+  Point2 pi = K.uncalibrate(pn);
+  Matrix Dcal, Dp;
+  K.calibrate(pi, Dcal, Dp);
+  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi, 1e-7);
+  CHECK(assert_equal(numerical1, Dcal, 1e-5));
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi, 1e-7);
+  CHECK(assert_equal(numerical2, Dp, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(Cal3DS2, Equal) { CHECK(assert_equal(K, K, 1e-5)); }
+
+/* ************************************************************************* */
+TEST(Cal3DS2, Retract) {
   Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6,
                    2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9);
-  Vector d(9);
+
+  EXPECT_LONGS_EQUAL(Cal3DS2::Dim(), 9);
+  EXPECT_LONGS_EQUAL(expected.dim(), 9);
+
+  Vector9 d;
   d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
   Cal3DS2 actual = K.retract(d);
   CHECK(assert_equal(expected, actual, 1e-7));
@@ -93,5 +109,19 @@ TEST( Cal3DS2, retract)
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+TEST(Cal3DS2, Print) {
+  Cal3DS2 cal(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  std::stringstream os;
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
+     << ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2();
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

gtsam/geometry/tests/testCal3Unified.cpp

@@ -10,17 +10,18 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file  testCal3Unify.cpp
+ * @file  testCal3Unified.cpp
- * @brief Unit tests for transform derivatives
+ * @brief Unit tests for Cal3Unified calibration model.
  */
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Unified.h>
 
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/inference/Key.h>
+#include <gtsam/nonlinear/Values.h>
 
 using namespace gtsam;
 
@@ -35,38 +36,37 @@ V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240];
 matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox
 */
 
-static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1);
+static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3,
+                     4.0 * 1e-3, 0.1);
 static Point2 p(0.5, 0.7);
 
 /* ************************************************************************* */
-TEST( Cal3Unified, uncalibrate)
+TEST(Cal3Unified, Uncalibrate) {
-{
   Point2 p_i(364.7791831734982, 305.6677211952602);
   Point2 q = K.uncalibrate(p);
   CHECK(assert_equal(q, p_i));
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, spaceNplane)
+TEST(Cal3Unified, SpaceNplane) {
-{
   Point2 q = K.spaceToNPlane(p);
   CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
   CHECK(assert_equal(p, K.nPlaneToSpace(q)));
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, calibrate)
+TEST(Cal3Unified, Calibrate) {
-{
   Point2 pi = K.uncalibrate(p);
   Point2 pn_hat = K.calibrate(pi);
   CHECK(traits<Point2>::Equals(p, pn_hat, 1e-8));
 }
 
-Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); }
+Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
 
 /* ************************************************************************* */
-TEST( Cal3Unified, Duncalibrate1)
+TEST(Cal3Unified, Duncalibrate1) {
-{
   Matrix computed;
   K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
@@ -74,26 +74,41 @@ TEST( Cal3Unified, Duncalibrate1)
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, Duncalibrate2)
+TEST(Cal3Unified, Duncalibrate2) {
-{
   Matrix computed;
   K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
   CHECK(assert_equal(numerical, computed, 1e-6));
 }
 
-/* ************************************************************************* */
+Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
-TEST( Cal3Unified, assert_equal)
+  return k.calibrate(pt);
-{
-  CHECK(assert_equal(K,K,1e-9));
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, retract)
+TEST(Cal3Unified, Dcalibrate) {
-{
+  Point2 pi = K.uncalibrate(p);
-  Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
+  Matrix Dcal, Dp;
-      1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1);
+  K.calibrate(pi, Dcal, Dp);
-  Vector d(10);
+  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
+  CHECK(assert_equal(numerical1, Dcal, 1e-5));
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
+  CHECK(assert_equal(numerical2, Dp, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(Cal3Unified, Equal) { CHECK(assert_equal(K, K, 1e-9)); }
+
+/* ************************************************************************* */
+TEST(Cal3Unified, Retract) {
+  Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7,
+                       2.0 * 1e-3 + 8, 3.0 * 1e-3 + 9, 4.0 * 1e-3 + 10,
+                       0.1 + 1);
+
+  EXPECT_LONGS_EQUAL(Cal3Unified::Dim(), 10);
+  EXPECT_LONGS_EQUAL(expected.dim(), 10);
+
+  Vector10 d;
   d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1;
   Cal3Unified actual = K.retract(d);
   CHECK(assert_equal(expected, actual, 1e-9));
@@ -101,8 +116,7 @@ TEST( Cal3Unified, retract)
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, DerivedValue)
+TEST(Cal3Unified, DerivedValue) {
-{
   Values values;
   Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
   Key key = 1;
@@ -114,5 +128,20 @@ TEST( Cal3Unified, DerivedValue)
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+TEST(Cal3Unified, Print) {
+  Cal3Unified cal(0, 1, 2, 3, 4, 5, 6, 7, 8, 9);
+  std::stringstream os;
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
+     << ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2()
+     << ", xi: " << cal.xi();
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

gtsam/geometry/tests/testCal3_S2.cpp

@@ -11,7 +11,7 @@
 
 /**
  * @file  testCal3_S2.cpp
- * @brief Unit tests for transform derivatives
+ * @brief Unit tests for basic Cal3_S2 calibration model.
  */
 
 #include <CppUnitLite/TestHarness.h>
@@ -31,8 +31,7 @@ static Point2 p_uv(1320.3, 1740);
 static Point2 p_xy(2, 3);
 
 /* ************************************************************************* */
-TEST( Cal3_S2, easy_constructor)
+TEST(Cal3_S2, Constructor) {
-{
   Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2);
 
   double fov = 60;  // degrees
@@ -43,8 +42,7 @@ TEST( Cal3_S2, easy_constructor)
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, calibrate)
+TEST(Cal3_S2, Calibrate) {
-{
   Point2 intrinsic(2, 3);
   Point2 expectedimage(1320.3, 1740);
   Point2 imagecoordinates = K.uncalibrate(intrinsic);
@@ -53,33 +51,38 @@ TEST( Cal3_S2, calibrate)
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, calibrate_homogeneous) {
+TEST(Cal3_S2, CalibrateHomogeneous) {
   Vector3 intrinsic(2, 3, 1);
   Vector3 image(1320.3, 1740, 1);
   CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image)));
 }
 
 /* ************************************************************************* */
-Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); }
+Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) {
-TEST( Cal3_S2, Duncalibrate1)
+  return k.uncalibrate(pt);
-{
+}
-  Matrix25 computed; K.uncalibrate(p, computed, boost::none);
+
+TEST(Cal3_S2, Duncalibrate1) {
+  Matrix25 computed;
+  K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
   CHECK(assert_equal(numerical, computed, 1e-8));
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, Duncalibrate2)
+TEST(Cal3_S2, Duncalibrate2) {
-{
+  Matrix computed;
-  Matrix computed; K.uncalibrate(p, boost::none, computed);
+  K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
   CHECK(assert_equal(numerical, computed, 1e-9));
 }
 
-Point2 calibrate_(const Cal3_S2& k,  const Point2& pt) {return k.calibrate(pt); }
+Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {
+  return k.calibrate(pt);
+}
+
 /* ************************************************************************* */
-TEST(Cal3_S2, Dcalibrate1)
+TEST(Cal3_S2, Dcalibrate1) {
-{
   Matrix computed;
   Point2 expected = K.calibrate(p_uv, computed, boost::none);
   Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
@@ -88,8 +91,7 @@ TEST(Cal3_S2, Dcalibrate1)
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2, Dcalibrate2)
+TEST(Cal3_S2, Dcalibrate2) {
-{
   Matrix computed;
   Point2 expected = K.calibrate(p_uv, boost::none, computed);
   Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
@@ -98,8 +100,7 @@ TEST(Cal3_S2, Dcalibrate2)
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, assert_equal)
+TEST(Cal3_S2, Equal) {
-{
   CHECK(assert_equal(K, K, 1e-9));
 
   Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2);
@@ -107,10 +108,13 @@ TEST( Cal3_S2, assert_equal)
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, retract)
+TEST(Cal3_S2, Retract) {
-{
   Cal3_S2 expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5);
-  Vector d(5);
+
+  EXPECT_LONGS_EQUAL(Cal3_S2::Dim(), 5);
+  EXPECT_LONGS_EQUAL(expected.dim(), 5);
+
+  Vector5 d;
   d << 1, 2, 3, 4, 5;
   Cal3_S2 actual = K.retract(d);
   CHECK(assert_equal(expected, actual, 1e-7));
@@ -125,15 +129,14 @@ TEST(Cal3_S2, between) {
   EXPECT(assert_equal(Cal3_S2(0, 1, 2, 3, 4), k1.between(k2, H1, H2)));
   EXPECT(assert_equal(-I_5x5, H1));
   EXPECT(assert_equal(I_5x5, H2));
-
 }
 
 /* ************************************************************************* */
 TEST(Cal3_S2, Print) {
   Cal3_S2 cal(5, 5, 5, 5, 5);
   std::stringstream os;
-  os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px()
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
-     << ", py:" << cal.py() << "}";
+     << ", px: " << cal.px() << ", py: " << cal.py();
 
   EXPECT(assert_stdout_equal(os.str(), cal));
 }
@@ -144,4 +147,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-

gtsam/geometry/tests/testCal3_S2Stereo.cpp

@@ -0,0 +1,129 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  testCal3_S2Stereo.cpp
+ * @brief Unit tests for stereo-rig calibration model.
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+
+using namespace gtsam;
+
+GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2Stereo)
+GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2Stereo)
+
+static Cal3_S2Stereo K(500, 500, 0.1, 640 / 2, 480 / 2, 1);
+static Point2 p(1, -2);
+static Point2 p_uv(1320.3, 1740);
+static Point2 p_xy(2, 3);
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, Constructor) {
+  Cal3_S2Stereo expected(554.256, 554.256, 0, 640 / 2, 480 / 2, 3);
+
+  double fov = 60;  // degrees
+  size_t w = 640, h = 480;
+  Cal3_S2Stereo actual(fov, w, h, 3);
+
+  CHECK(assert_equal(expected, actual, 1e-3));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, Calibrate) {
+  Point2 intrinsic(2, 3);
+  Point2 expectedimage(1320.3, 1740);
+  Point2 imagecoordinates = K.uncalibrate(intrinsic);
+  CHECK(assert_equal(expectedimage, imagecoordinates));
+  CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates)));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, CalibrateHomogeneous) {
+  Vector3 intrinsic(2, 3, 1);
+  Vector3 image(1320.3, 1740, 1);
+  CHECK(assert_equal(intrinsic, K.calibrate(image)));
+}
+
+/* ************************************************************************* */
+Point2 uncalibrate_(const Cal3_S2Stereo& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
+
+TEST(Cal3_S2Stereo, Duncalibrate) {
+  Matrix26 Dcal;
+  Matrix22 Dp;
+  K.uncalibrate(p, Dcal, Dp);
+
+  Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
+  CHECK(assert_equal(numerical1, Dcal, 1e-8));
+  Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
+  CHECK(assert_equal(numerical2, Dp, 1e-9));
+}
+
+Point2 calibrate_(const Cal3_S2Stereo& K, const Point2& pt) {
+  return K.calibrate(pt);
+}
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, Dcalibrate) {
+  Matrix26 Dcal;
+  Matrix22 Dp;
+  Point2 expected = K.calibrate(p_uv, Dcal, Dp);
+  CHECK(assert_equal(expected, p_xy, 1e-8));
+
+  Matrix numerical1 = numericalDerivative21(calibrate_, K, p_uv);
+  CHECK(assert_equal(numerical1, Dcal, 1e-8));
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, p_uv);
+  CHECK(assert_equal(numerical2, Dp, 1e-8));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, Equal) {
+  CHECK(assert_equal(K, K, 1e-9));
+
+  Cal3_S2Stereo K1(500, 500, 0.1, 640 / 2, 480 / 2, 1);
+  CHECK(assert_equal(K, K1, 1e-9));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, Retract) {
+  Cal3_S2Stereo expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5,
+                         7);
+  EXPECT_LONGS_EQUAL(Cal3_S2Stereo::Dim(), 6);
+  EXPECT_LONGS_EQUAL(expected.dim(), 6);
+
+  Vector6 d;
+  d << 1, 2, 3, 4, 5, 6;
+  Cal3_S2Stereo actual = K.retract(d);
+  CHECK(assert_equal(expected, actual, 1e-7));
+  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, Print) {
+  Cal3_S2Stereo cal(5, 5, 5, 5, 5, 2);
+  std::stringstream os;
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py()
+     << ", b: " << cal.baseline();
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/geometry/tests/testLine3.cpp

@@ -14,6 +14,16 @@ GTSAM_CONCEPT_MANIFOLD_INST(Line3)
 
 static const Line3 l(Rot3(), 1, 1);
 
+// Testing getters
+TEST(Line3, getMethods) {
+  const double a = 5, b = 10;
+  const Rot3 R = Rot3::Expmap(Vector3(0.1, 0.2, 0.3));
+  const Line3 line(R, a, b);
+  EXPECT_DOUBLES_EQUAL(a, line.a(), 1e-8);
+  EXPECT_DOUBLES_EQUAL(b, line.b(), 1e-8);
+  EXPECT(assert_equal(R, line.R(), 1e-8));
+}
+
 // Testing equals function of Line3
 TEST(Line3, equals) {
   Line3 l_same = l;

gtsam/geometry/tests/testPose3.cpp

@@ -1016,6 +1016,33 @@ TEST(Pose3, TransformCovariance6) {
 TEST(Pose3, interpolate) {
   EXPECT(assert_equal(T2, interpolate(T2,T3, 0.0)));
   EXPECT(assert_equal(T3, interpolate(T2,T3, 1.0)));
+
+  // Trivial example: start at origin and move to (1, 0, 0) while rotating pi/2
+  // about z-axis.
+  Pose3 start;
+  Pose3 end(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
+  // This interpolation is easy to calculate by hand.
+  double t = 0.5;
+  Pose3 expected0(Rot3::Rz(M_PI_4), Point3(0.5, 0, 0));
+  EXPECT(assert_equal(expected0, start.interpolateRt(end, t)));
+
+  // Example from Peter Corke
+  // https://robotacademy.net.au/lesson/interpolating-pose-in-3d/
+  t = 0.0759;  // corresponds to the 10th element when calling `ctraj` in
+               // the video
+  Pose3 O;
+  Pose3 F(Rot3::Roll(0.6).compose(Rot3::Pitch(0.8)).compose(Rot3::Yaw(1.4)),
+          Point3(1, 2, 3));
+
+  // The expected answer matches the result presented in the video.
+  Pose3 expected1(interpolate(O.rotation(), F.rotation(), t),
+                  interpolate(O.translation(), F.translation(), t));
+  EXPECT(assert_equal(expected1, O.interpolateRt(F, t)));
+
+  // Non-trivial interpolation, translation value taken from output.
+  Pose3 expected2(interpolate(T2.rotation(), T3.rotation(), t),
+                  interpolate(T2.translation(), T3.translation(), t));
+  EXPECT(assert_equal(expected2, T2.interpolateRt(T3, t)));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testRot3.cpp

@@ -807,15 +807,15 @@ TEST(Rot3, RQ_derivative) {
   test_xyz.push_back(VecAndErr{{0, 0, 0}, error});
   test_xyz.push_back(VecAndErr{{0, 0.5, -0.5}, error});
   test_xyz.push_back(VecAndErr{{0.3, 0, 0.2}, error});
-  test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, error});
+  test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, 1e-8});
   test_xyz.push_back(VecAndErr{{1.0, 0.7, 0.8}, error});
   test_xyz.push_back(VecAndErr{{3.0, 0.7, -0.6}, error});
   test_xyz.push_back(VecAndErr{{M_PI / 2, 0, 0}, error});
   test_xyz.push_back(VecAndErr{{0, 0, M_PI / 2}, error});
 
   // Test close to singularity
-  test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-8});
+  test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-7});
-  test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-8});
+  test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-7});
   test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1.1e-2, 0}, 1e-4});
   test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1.1e-2, 0}, 1e-4});
 
@@ -825,7 +825,7 @@ TEST(Rot3, RQ_derivative) {
     const auto R = Rot3::RzRyRx(xyz).matrix();
     const auto num = numericalDerivative11(RQ_proxy, R);
     Matrix39 calc;
-    RQ(R, calc).second;
+    auto dummy = RQ(R, calc).second;
 
     const auto err = vec_err.second;
     CHECK(assert_equal(num, calc, err));

gtsam/geometry/tests/testSimpleCamera.cpp

@@ -26,6 +26,8 @@
 using namespace std;
 using namespace gtsam;
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
+
 static const Cal3_S2 K(625, 625, 0, 0, 0);
 
 static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()),
@@ -149,6 +151,8 @@ TEST( SimpleCamera, simpleCamera)
   CHECK(assert_equal(expected, actual,1e-1));
 }
 
+#endif
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/geometry/triangulation.h

@@ -18,8 +18,10 @@
 
 #pragma once
 
-#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/CameraSet.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/slam/TriangulationFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -494,5 +496,9 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
     }
 }
 
+// Vector of Cameras - used by the Python/MATLAB wrapper
+using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
+using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
+
 } // \namespace gtsam
 

gtsam/gtsam.i

@@ -329,7 +329,7 @@ virtual class Value {
 };
 
 #include <gtsam/base/GenericValue.h>
-template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
 virtual class GenericValue : gtsam::Value {
   void serializable() const;
 };
@@ -852,8 +852,7 @@ class Cal3_S2 {
   gtsam::Point2 principalPoint() const;
   Vector vector() const;
   Matrix K() const;
-  Matrix matrix() const;
+  Matrix inverse() const;
-  Matrix matrix_inverse() const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -881,7 +880,7 @@ virtual class Cal3DS2_Base {
 
   // Action on Point2
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
-  gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -1059,56 +1058,24 @@ class PinholeCamera {
   void serialize() const;
 };
 
+// Forward declaration of PinholeCameraCalX is defined here.
 #include <gtsam/geometry/SimpleCamera.h>
-virtual class SimpleCamera {
-  // Standard Constructors and Named Constructors
-  SimpleCamera();
-  SimpleCamera(const gtsam::Pose3& pose);
-  SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K);
-  static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height);
-  static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
-  static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
-      const gtsam::Point3& upVector, const gtsam::Cal3_S2& K);
-  static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
-      const gtsam::Point3& upVector);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::SimpleCamera& camera, double tol) const;
-
-  // Standard Interface
-  gtsam::Pose3 pose() const;
-  gtsam::Cal3_S2 calibration() const;
-
-  // Manifold
-  gtsam::SimpleCamera retract(Vector d) const;
-  Vector localCoordinates(const gtsam::SimpleCamera& T2) const;
-  size_t dim() const;
-  static size_t Dim();
-
-  // Transformations and measurement functions
-  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
-  pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
-  gtsam::Point2 project(const gtsam::Point3& point);
-  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
-  double range(const gtsam::Point3& point);
-  double range(const gtsam::Pose3& pose);
-
-  // enabling serialization functionality
-  void serialize() const;
-
-};
-
-gtsam::SimpleCamera simpleCamera(const Matrix& P);
-
 // Some typedefs for common camera types
 // PinholeCameraCal3_S2 is the same as SimpleCamera above
 typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
-//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
+typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
-//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
+typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
-//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
 typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
 
+template<T>
+class CameraSet {
+  CameraSet();
+
+  // structure specific methods
+  T at(size_t i) const;
+  void push_back(const T& cam);
+};
+
 #include <gtsam/geometry/StereoCamera.h>
 class StereoCamera {
   // Standard Constructors and Named Constructors
@@ -1140,7 +1107,7 @@ class StereoCamera {
 
 #include <gtsam/geometry/triangulation.h>
 
-// Templates appear not yet supported for free functions
+// Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
     gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements,
     double rank_tol, bool optimize);
@@ -1150,6 +1117,12 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
     gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements,
     double rank_tol, bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
+    const gtsam::Point2Vector& measurements, double rank_tol,
+    bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
+    const gtsam::Point2Vector& measurements, double rank_tol,
+    bool optimize);
     
 //*************************************************************************
 // Symbolic
@@ -1963,6 +1936,21 @@ class KalmanFilter {
 //*************************************************************************
 
 #include <gtsam/inference/Symbol.h>
+
+class Symbol {
+  Symbol();
+  Symbol(char c, uint64_t j);
+  Symbol(size_t key);
+
+  size_t key() const;
+  void print(const string& s) const;
+  bool equals(const gtsam::Symbol& expected, double tol) const;
+
+  char chr() const;
+  uint64_t index() const;
+  string string() const;
+};
+
 size_t symbol(char chr, size_t index);
 char symbolChr(size_t key);
 size_t symbolIndex(size_t key);
@@ -2069,7 +2057,7 @@ class NonlinearFactorGraph {
   gtsam::KeySet keys() const;
   gtsam::KeyVector keyVector() const;
 
-  template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}>
+  template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}>
   void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
 
   // NonlinearFactorGraph
@@ -2083,6 +2071,7 @@ class NonlinearFactorGraph {
 
   // enabling serialization functionality
   void serialize() const;
+  void saveGraph(const string& s) const;
 };
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
@@ -2493,7 +2482,7 @@ class ISAM2 {
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
                      gtsam::Cal3Bundler, gtsam::EssentialMatrix,
-                     gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, 
+                     gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, 
                      Vector, Matrix}>
   VALUE calculateEstimate(size_t key) const;
   gtsam::Values calculateBestEstimate() const;
@@ -2527,12 +2516,11 @@ class NonlinearISAM {
 //*************************************************************************
 // Nonlinear factor types
 //*************************************************************************
-#include <gtsam/geometry/SimpleCamera.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/StereoPoint2.h>
 
 #include <gtsam/nonlinear/PriorFactor.h>
-template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
+template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
   T prior() const;
@@ -2556,7 +2544,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
 template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
                gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
                gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
-               gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2,
+               gtsam::PinholeCameraCal3_S2,
                gtsam::imuBias::ConstantBias}>
 virtual class NonlinearEquality : gtsam::NoiseModelFactor {
   // Constructor - forces exact evaluation
@@ -2611,6 +2599,7 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
 };
 
 typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
+typedef gtsam::BearingFactor<gtsam::Pose3, gtsam::Point3, gtsam::Unit3> BearingFactor3D;
 typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2> BearingFactorPose2;
 
 #include <gtsam/geometry/BearingRange.h>
@@ -2634,6 +2623,8 @@ virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
       const BEARING& measuredBearing, const RANGE& measuredRange,
       const gtsam::noiseModel::Base* noiseModel);
 
+  BearingRange<POSE, POINT, BEARING, RANGE> measured() const;
+
   // enabling serialization functionality
   void serialize() const;
 };
@@ -2675,11 +2666,10 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
   gtsam::Point2 measured() const;
 };
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
-//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
-//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
 
-template<CALIBRATION = {gtsam::Cal3_S2}>
+template<CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler}>
 virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
   GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey);
   gtsam::Point2 measured() const;
@@ -2780,6 +2770,12 @@ class SfmTrack {
   pair<size_t, gtsam::Point2> measurement(size_t idx) const;
   pair<size_t, size_t> siftIndex(size_t idx) const;
   void add_measurement(size_t idx, const gtsam::Point2& m);
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enabling function to compare objects
+  bool equals(const gtsam::SfmTrack& expected, double tol) const;
 };
 
 class SfmData {
@@ -2790,6 +2786,12 @@ class SfmData {
   gtsam::SfmTrack track(size_t idx) const;
   void add_track(const gtsam::SfmTrack& t) ;
   void add_camera(const gtsam::SfmCamera& cam);
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enabling function to compare objects
+  bool equals(const gtsam::SfmData& expected, double tol) const;
 };
 
 gtsam::SfmData readBal(string filename);
@@ -2933,9 +2935,14 @@ class ShonanAveragingParameters2 {
   void setAnchorWeight(double value);
   double getAnchorWeight() const;
   void setKarcherWeight(double value);
-  double getKarcherWeight();
+  double getKarcherWeight() const;
   void setGaugesWeight(double value);
-  double getGaugesWeight();
+  double getGaugesWeight() const;
+  void setUseHuber(bool value);
+  bool getUseHuber() const;
+  void setCertifyOptimality(bool value);
+  bool getCertifyOptimality() const;
+  void print() const;
 };
 
 class ShonanAveragingParameters3 {
@@ -2949,9 +2956,14 @@ class ShonanAveragingParameters3 {
   void setAnchorWeight(double value);
   double getAnchorWeight() const;
   void setKarcherWeight(double value);
-  double getKarcherWeight();
+  double getKarcherWeight() const;
   void setGaugesWeight(double value);
-  double getGaugesWeight();
+  double getGaugesWeight() const;
+  void setUseHuber(bool value);
+  bool getUseHuber() const;
+  void setCertifyOptimality(bool value);
+  bool getCertifyOptimality() const;
+  void print() const;
 };
 
 class ShonanAveraging2 {

gtsam/inference/Symbol.h

@@ -80,6 +80,9 @@ public:
   /** Create a string from the key */
   operator std::string() const;
 
+  /// Return string representation of the key
+  std::string string() const { return std::string(*this); };
+
   /** Comparison for use in maps */
   bool operator<(const Symbol& comp) const {
     return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_);
@@ -167,10 +170,11 @@ inline Key Z(std::uint64_t j) { return Symbol('z', j); }
 /** Generates symbol shorthands with alternative names different than the
  * one-letter predefined ones. */
 class SymbolGenerator {
-  const char c_;
+  const unsigned char c_;
 public:
-  SymbolGenerator(const char c) : c_(c) {}
+  constexpr SymbolGenerator(const unsigned char c) : c_(c) {}
   Symbol operator()(const std::uint64_t j) const { return Symbol(c_, j); }
+  constexpr unsigned char chr() const { return c_; }
 };
 
 /// traits

gtsam/inference/tests/testKey.cpp

@@ -59,6 +59,12 @@ TEST(Key, SymbolGenerator) {
   EXPECT(assert_equal(a1, ddz1));
 }
 
+/* ************************************************************************* */
+TEST(Key, SymbolGeneratorConstexpr) {
+  constexpr auto Z = gtsam::SymbolGenerator('x');
+  EXPECT(assert_equal(Z.chr(), 'x'));
+}
+
 /* ************************************************************************* */
 template<int KeySize>
 Key KeyTestValue();

gtsam/linear/AcceleratedPowerMethod.h

@@ -0,0 +1,176 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   AcceleratedPowerMethod.h
+ * @date   Sept 2020
+ * @author Jing Wu
+ * @brief  accelerated power method for fast eigenvalue and eigenvector
+ * computation
+ */
+
+#pragma once
+
+#include <gtsam/linear/PowerMethod.h>
+
+namespace gtsam {
+
+using Sparse = Eigen::SparseMatrix<double>;
+
+/**
+ * \brief Compute maximum Eigenpair with accelerated power method
+ *
+ * References :
+ * 1) G. Golub and C. V. Loan, Matrix Computations, 3rd ed. Baltimore, Johns
+ * Hopkins University Press, 1996, pp.405-411
+ * 2) Rosen, D. and Carlone, L., 2017, September. Computational
+ * enhancements for certifiably correct SLAM. In Proceedings of the
+ * International Conference on Intelligent Robots and Systems.
+ * 3) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How,
+ * 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv
+ * 4) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated
+ * stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp.
+ * 58–67
+ *
+ * It performs the following iteration: \f$ x_{k+1} = A * x_k - \beta *
+ * x_{k-1} \f$ where A is the aim matrix we want to get eigenpair of, x is the
+ * Ritz vector
+ *
+ * Template argument Operator just needs multiplication operator
+ *
+ */
+template <class Operator>
+class AcceleratedPowerMethod : public PowerMethod<Operator> {
+
+  double beta_ = 0;  // a Polyak momentum term
+
+  Vector previousVector_;  // store previous vector
+
+ public:
+  /**
+   * Constructor from aim matrix A (given as Matrix or Sparse), optional intial
+   * vector as ritzVector
+   */
+  explicit AcceleratedPowerMethod(
+      const Operator &A, const boost::optional<Vector> initial = boost::none,
+      double initialBeta = 0.0)
+      : PowerMethod<Operator>(A, initial) {
+    // initialize Ritz eigen vector and previous vector
+    this->ritzVector_ = initial ? initial.get() : Vector::Random(this->dim_);
+    this->ritzVector_.normalize();
+    previousVector_ = Vector::Zero(this->dim_);
+
+    // initialize beta_
+    beta_ = initialBeta;
+  }
+
+  /**
+   * Run accelerated power iteration to get ritzVector with beta and previous
+   * two ritzVector x0 and x00, and return y = (A * x0 - \beta * x00) / || A * x0
+   * - \beta * x00 ||
+   */
+  Vector acceleratedPowerIteration (const Vector &x1, const Vector &x0,
+                        const double beta) const {
+    Vector y = this->A_ * x1 - beta * x0;
+    y.normalize();
+    return y;
+  }
+
+  /**
+   * Run accelerated power iteration to get ritzVector with beta and previous
+   * two ritzVector x0 and x00, and return y = (A * x0 - \beta * x00) / || A * x0
+   * - \beta * x00 ||
+   */
+  Vector acceleratedPowerIteration () const {
+    Vector y = acceleratedPowerIteration(this->ritzVector_, previousVector_, beta_);
+    return y;
+  }
+
+  /**
+   * Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3), T
+   * is the iteration time to find beta with largest Rayleigh quotient
+   */
+  double estimateBeta(const size_t T = 10) const {
+    // set initial estimation of maxBeta
+    Vector initVector = this->ritzVector_;
+    const double up = initVector.dot( this->A_ * initVector );
+    const double down = initVector.dot(initVector);
+    const double mu = up / down;
+    double maxBeta = mu * mu / 4;
+    size_t maxIndex;
+    std::vector<double> betas;
+
+    Matrix R = Matrix::Zero(this->dim_, 10);
+    // run T times of iteration to find the beta that has the largest Rayleigh quotient
+    for (size_t t = 0; t < T; t++) {
+      // after each t iteration, reset the betas with the current maxBeta
+      betas = {2 / 3 * maxBeta, 0.99 * maxBeta, maxBeta, 1.01 * maxBeta,
+               1.5 * maxBeta};
+      // iterate through every beta value
+      for (size_t k = 0; k < betas.size(); ++k) {
+        // initialize x0 and x00 in each iteration of each beta
+        Vector x0 = initVector;
+        Vector x00 = Vector::Zero(this->dim_);
+        // run 10 steps of accelerated power iteration with this beta
+        for (size_t j = 1; j < 10; j++) {
+          if (j < 2) {
+            R.col(0) = acceleratedPowerIteration(x0, x00, betas[k]);
+            R.col(1) = acceleratedPowerIteration(R.col(0), x0, betas[k]);
+          } else {
+            R.col(j) = acceleratedPowerIteration(R.col(j - 1), R.col(j - 2),
+  betas[k]);
+          }
+        }
+        // compute the Rayleigh quotient for the randomly sampled vector after
+        // 10 steps of power accelerated iteration
+        const Vector x = R.col(9);
+        const double up = x.dot(this->A_ * x);
+        const double down = x.dot(x);
+        const double mu = up / down;
+        // store the momentum with largest Rayleigh quotient and its according index of beta_
+        if (mu * mu / 4 > maxBeta) {
+          // save the max beta index
+          maxIndex = k;
+          maxBeta = mu * mu / 4;
+        }
+      }
+    }
+    // set beta_ to momentum with largest Rayleigh quotient
+    return betas[maxIndex];
+  }
+
+  /**
+   * Start the accelerated iteration, after performing the
+   * accelerated iteration, calculate the ritz error, repeat this
+   * operation until the ritz error converge. If converged return true, else
+   * false.
+   */
+  bool compute(size_t maxIterations, double tol) {
+    // Starting
+    bool isConverged = false;
+
+    for (size_t i = 0; i < maxIterations && !isConverged; i++) {
+      ++(this->nrIterations_);
+      Vector tmp = this->ritzVector_;
+      // update the ritzVector after accelerated power iteration
+      this->ritzVector_ = acceleratedPowerIteration();
+      // update the previousVector with ritzVector
+      previousVector_ = tmp;
+      // update the ritzValue 
+      this->ritzValue_ = this->ritzVector_.dot(this->A_ * this->ritzVector_);
+      isConverged = this->converged(tol);
+    }
+
+    return isConverged;
+  }
+};
+
+}  // namespace gtsam

gtsam/linear/PowerMethod.h

@@ -0,0 +1,152 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   PowerMethod.h
+ * @date   Sept 2020
+ * @author Jing Wu
+ * @brief  Power method for fast eigenvalue and eigenvector
+ * computation
+ */
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+
+#include <Eigen/Core>
+#include <Eigen/Sparse>
+#include <random>
+#include <vector>
+
+namespace gtsam {
+
+using Sparse = Eigen::SparseMatrix<double>;
+
+/**
+ * \brief Compute maximum Eigenpair with power method
+ *
+ * References :
+ * 1) G. Golub and C. V. Loan, Matrix Computations, 3rd ed. Baltimore, Johns
+ * Hopkins University Press, 1996, pp.405-411
+ * 2) Rosen, D. and Carlone, L., 2017, September. Computational
+ * enhancements for certifiably correct SLAM. In Proceedings of the
+ * International Conference on Intelligent Robots and Systems.
+ * 3) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How,
+ * 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv
+ * 4) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated
+ * stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp.
+ * 58–67
+ *
+ * It performs the following iteration: \f$ x_{k+1} = A * x_k  \f$
+ * where A is the aim matrix we want to get eigenpair of, x is the
+ * Ritz vector
+ *
+ * Template argument Operator just needs multiplication operator
+ *
+ */
+template <class Operator>
+class PowerMethod {
+ protected:
+  /**
+   * Const reference to an externally-held matrix whose minimum-eigenvalue we
+   * want to compute
+   */
+  const Operator &A_;
+
+  const int dim_;  // dimension of Matrix A
+
+  size_t nrIterations_;  // number of iterations
+
+  double ritzValue_;   // Ritz eigenvalue
+  Vector ritzVector_;  // Ritz eigenvector
+
+ public:
+  /// @name Standard Constructors
+  /// @{
+
+  /// Construct from the aim matrix and intial ritz vector
+  explicit PowerMethod(const Operator &A,
+                       const boost::optional<Vector> initial = boost::none)
+      : A_(A), dim_(A.rows()), nrIterations_(0) {
+    Vector x0;
+    x0 = initial ? initial.get() : Vector::Random(dim_);
+    x0.normalize();
+
+    // initialize Ritz eigen value
+    ritzValue_ = 0.0;
+
+    // initialize Ritz eigen vector
+    ritzVector_ = powerIteration(x0);
+  }
+
+  /**
+   * Run power iteration to get ritzVector with previous ritzVector x, and
+   * return A * x / || A * x ||
+   */
+  Vector powerIteration(const Vector &x) const {
+    Vector y = A_ * x;
+    y.normalize();
+    return y;
+  }
+
+  /**
+   * Run power iteration to get ritzVector with previous ritzVector x, and
+   * return A * x / || A * x ||
+   */  
+  Vector powerIteration() const { return powerIteration(ritzVector_); }
+
+  /**
+   * After Perform power iteration on a single Ritz value, check if the Ritz
+   * residual for the current Ritz pair is less than the required convergence
+   * tol, return true if yes, else false
+   */
+  bool converged(double tol) const {
+    const Vector x = ritzVector_;
+    // store the Ritz eigen value
+    const double ritzValue = x.dot(A_ * x);
+    const double error = (A_ * x - ritzValue * x).norm();
+    return error < tol;
+  }
+
+  /// Return the number of iterations
+  size_t nrIterations() const { return nrIterations_; }
+
+  /**
+   * Start the power/accelerated iteration, after performing the
+   * power/accelerated iteration, calculate the ritz error, repeat this
+   * operation until the ritz error converge. If converged return true, else
+   * false.
+   */
+  bool compute(size_t maxIterations, double tol) {
+    // Starting
+    bool isConverged = false;
+
+    for (size_t i = 0; i < maxIterations && !isConverged; i++) {
+      ++nrIterations_;
+      // update the ritzVector after power iteration
+      ritzVector_ = powerIteration();
+      // update the ritzValue 
+      ritzValue_ = ritzVector_.dot(A_ * ritzVector_);
+      isConverged = converged(tol);
+    }
+
+    return isConverged;
+  }
+
+  /// Return the eigenvalue
+  double eigenvalue() const { return ritzValue_; }
+
+  /// Return the eigenvector
+  Vector eigenvector() const { return ritzVector_; }
+};
+
+}  // namespace gtsam

gtsam/linear/SparseEigen.h

@@ -0,0 +1,119 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file SparseEigen.h
+ *
+ * @brief Utilities for creating Eigen sparse matrices (gtsam::SparseEigen)
+ *
+ * @date Aug 2019
+ * @author Mandy Xie
+ * @author Fan Jiang
+ * @author Gerry Chen
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/VectorValues.h>
+
+#include <Eigen/Sparse>
+
+namespace gtsam {
+
+typedef Eigen::SparseMatrix<double> SparseEigen;
+
+/// Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph
+SparseEigen sparseJacobianEigen(
+    const GaussianFactorGraph &gfg, const Ordering &ordering) {
+  // TODO(gerry): eliminate copy/pasta by making GaussianFactorGraph version
+  // more general, or by creating an Eigen::Triplet compatible wrapper for
+  // boost::tuple return type
+
+  // First find dimensions of each variable
+  std::map<Key, size_t> dims;
+  for (const boost::shared_ptr<GaussianFactor> &factor : gfg) {
+    if (!static_cast<bool>(factor)) continue;
+
+    for (auto it = factor->begin(); it != factor->end(); ++it) {
+      dims[*it] = factor->getDim(it);
+    }
+  }
+
+  // Compute first scalar column of each variable
+  size_t currentColIndex = 0;
+  std::map<Key, size_t> columnIndices;
+  for (const auto key : ordering) {
+    columnIndices[key] = currentColIndex;
+    currentColIndex += dims[key];
+  }
+
+  // Iterate over all factors, adding sparse scalar entries
+  std::vector<Eigen::Triplet<double>> entries;
+  entries.reserve(60 * gfg.size());
+
+  size_t row = 0;
+  for (const boost::shared_ptr<GaussianFactor> &factor : gfg) {
+    if (!static_cast<bool>(factor)) continue;
+
+    // Convert to JacobianFactor if necessary
+    JacobianFactor::shared_ptr jacobianFactor(
+        boost::dynamic_pointer_cast<JacobianFactor>(factor));
+    if (!jacobianFactor) {
+      HessianFactor::shared_ptr hessian(
+          boost::dynamic_pointer_cast<HessianFactor>(factor));
+      if (hessian)
+        jacobianFactor.reset(new JacobianFactor(*hessian));
+      else
+        throw std::invalid_argument(
+            "GaussianFactorGraph contains a factor that is neither a "
+            "JacobianFactor nor a HessianFactor.");
+    }
+
+    // Whiten the factor and add entries for it
+    // iterate over all variables in the factor
+    const JacobianFactor whitened(jacobianFactor->whiten());
+    for (JacobianFactor::const_iterator key = whitened.begin();
+         key < whitened.end(); ++key) {
+      JacobianFactor::constABlock whitenedA = whitened.getA(key);
+      // find first column index for this key
+      size_t column_start = columnIndices[*key];
+      for (size_t i = 0; i < (size_t)whitenedA.rows(); i++)
+        for (size_t j = 0; j < (size_t)whitenedA.cols(); j++) {
+          double s = whitenedA(i, j);
+          if (std::abs(s) > 1e-12)
+            entries.emplace_back(row + i, column_start + j, s);
+        }
+    }
+
+    JacobianFactor::constBVector whitenedb(whitened.getb());
+    size_t bcolumn = currentColIndex;
+    for (size_t i = 0; i < (size_t)whitenedb.size(); i++) {
+      double s = whitenedb(i);
+      if (std::abs(s) > 1e-12) entries.emplace_back(row + i, bcolumn, s);
+    }
+
+    // Increment row index
+    row += jacobianFactor->rows();
+  }
+
+  // ...and make a sparse matrix with it.
+  SparseEigen Ab(row, currentColIndex + 1);
+  Ab.setFromTriplets(entries.begin(), entries.end());
+  return Ab;
+}
+
+SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg) {
+  return sparseJacobianEigen(gfg, Ordering(gfg.keys()));
+}
+
+}  // namespace gtsam

gtsam/linear/SubgraphBuilder.cpp

@@ -383,7 +383,7 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const {
   const vector<size_t> tree = buildTree(gfg, forward_ordering, weights);
   if (tree.size() != n - 1) {
     throw std::runtime_error(
-        "SubgraphBuilder::operator() failure: tree.size() != n-1");
+        "SubgraphBuilder::operator() failure: tree.size() != n-1, might be caused by disconnected graph");
   }
 
   // Downweight the tree edges to zero.

gtsam/linear/VectorValues.cpp

@@ -161,7 +161,7 @@ namespace gtsam {
   bool VectorValues::equals(const VectorValues& x, double tol) const {
     if(this->size() != x.size())
       return false;
-    for(const auto& values: boost::combine(*this, x)) {
+    for(const auto values: boost::combine(*this, x)) {
       if(values.get<0>().first != values.get<1>().first ||
         !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol))
         return false;
@@ -233,7 +233,7 @@ namespace gtsam {
     double result = 0.0;
     typedef boost::tuple<value_type, value_type> ValuePair;
     using boost::adaptors::map_values;
-    for(const ValuePair& values: boost::combine(*this, v)) {
+    for(const ValuePair values: boost::combine(*this, v)) {
       assert_throw(values.get<0>().first == values.get<1>().first,
         invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
       assert_throw(values.get<0>().second.size() == values.get<1>().second.size(),

gtsam/linear/tests/powerMethodExample.h

@@ -0,0 +1,67 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * powerMethodExample.h
+ *
+ * @file   powerMethodExample.h
+ * @date   Nov 2020
+ * @author Jing Wu
+ * @brief  Create sparse and dense factor graph for
+ * PowerMethod/AcceleratedPowerMethod
+ */
+
+#include <gtsam/inference/Symbol.h>
+
+#include <iostream>
+
+
+namespace gtsam {
+namespace linear {
+namespace test {
+namespace example {
+
+/* ************************************************************************* */
+inline GaussianFactorGraph createSparseGraph() {
+  using symbol_shorthand::X;
+  // Let's make a scalar synchronization graph with 4 nodes
+  GaussianFactorGraph fg;
+  auto model = noiseModel::Unit::Create(1);
+  for (size_t j = 0; j < 3; j++) {
+    fg.add(X(j), -I_1x1, X(j + 1), I_1x1, Vector1::Zero(), model);
+  }
+  fg.add(X(3), -I_1x1, X(0), I_1x1, Vector1::Zero(), model);  // extra row
+
+  return fg;
+}
+
+/* ************************************************************************* */
+inline GaussianFactorGraph createDenseGraph() {
+  using symbol_shorthand::X;
+  // Let's make a scalar synchronization graph with 10 nodes
+  GaussianFactorGraph fg;
+  auto model = noiseModel::Unit::Create(1);
+  // Iterate over nodes
+  for (size_t j = 0; j < 10; j++) {
+    // Each node has an edge with all the others
+    for (size_t i = 1; i < 10; i++)
+    fg.add(X(j), -I_1x1, X((j + i) % 10), I_1x1, Vector1::Zero(), model);
+  }
+
+  return fg;
+}
+
+/* ************************************************************************* */
+
+}  // namespace example
+}  // namespace test
+}  // namespace linear
+}  // namespace gtsam

gtsam/linear/tests/testAcceleratedPowerMethod.cpp

@@ -0,0 +1,140 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * testPowerMethod.cpp
+ *
+ * @file   testAcceleratedPowerMethod.cpp
+ * @date   Sept 2020
+ * @author Jing Wu
+ * @brief  Check eigenvalue and eigenvector computed by accelerated power method
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/VectorSpace.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/AcceleratedPowerMethod.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/tests/powerMethodExample.h>
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <Eigen/Eigenvalues>
+#include <iostream>
+#include <random>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(AcceleratedPowerMethod, acceleratedPowerIteration) {
+  // test power iteration, beta is set to 0
+  Sparse A(6, 6);
+  A.coeffRef(0, 0) = 6;
+  A.coeffRef(1, 1) = 5;
+  A.coeffRef(2, 2) = 4;
+  A.coeffRef(3, 3) = 3;
+  A.coeffRef(4, 4) = 2;
+  A.coeffRef(5, 5) = 1;
+  Vector initial = (Vector(6) << 0.24434602, 0.22829942, 0.70094486, 0.15463092, 0.55871359,
+       0.2465342).finished();
+  const double ev1 = 6.0;
+
+  // test accelerated power iteration
+  AcceleratedPowerMethod<Sparse> apf(A, initial);
+  apf.compute(100, 1e-5);
+  EXPECT_LONGS_EQUAL(6, apf.eigenvector().rows());
+
+  Vector6 actual1 = apf.eigenvector();
+  const double ritzValue = actual1.dot(A * actual1);
+  const double ritzResidual = (A * actual1 - ritzValue * actual1).norm();
+  EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);
+
+  EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalue(), 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(AcceleratedPowerMethod, useFactorGraphSparse) {
+  // Let's make a scalar synchronization graph with 4 nodes
+  GaussianFactorGraph fg = gtsam::linear::test::example::createSparseGraph();
+
+  // Get eigenvalues and eigenvectors with Eigen
+  auto L = fg.hessian();
+  Eigen::EigenSolver<Matrix> solver(L.first);
+
+  // find the index of the max eigenvalue
+  size_t maxIdx = 0;
+  for (auto i = 0; i < solver.eigenvalues().rows(); ++i) {
+    if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real())
+      maxIdx = i;
+  }
+  // Store the max eigenvalue and its according eigenvector
+  const auto ev1 = solver.eigenvalues()(maxIdx).real();
+
+  Vector disturb = Vector4::Random();
+  disturb.normalize();
+  Vector initial = L.first.row(0);
+  double magnitude = initial.norm();
+  initial += 0.03 * magnitude * disturb;
+  AcceleratedPowerMethod<Matrix> apf(L.first, initial);
+  apf.compute(100, 1e-5);
+  // Check if the eigenvalue is the maximum eigen value
+  EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalue(), 1e-8);
+
+  // Check if the according ritz residual converged to the threshold
+  Vector actual1 = apf.eigenvector();
+  const double ritzValue = actual1.dot(L.first * actual1);
+  const double ritzResidual = (L.first * actual1 - ritzValue * actual1).norm();
+  EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(AcceleratedPowerMethod, useFactorGraphDense) {
+  // Let's make a scalar synchronization graph with 10 nodes
+  GaussianFactorGraph fg = gtsam::linear::test::example::createDenseGraph();
+
+  // Get eigenvalues and eigenvectors with Eigen
+  auto L = fg.hessian();
+  Eigen::EigenSolver<Matrix> solver(L.first);
+
+  // find the index of the max eigenvalue
+  size_t maxIdx = 0;
+  for (auto i = 0; i < solver.eigenvalues().rows(); ++i) {
+    if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real())
+      maxIdx = i;
+  }
+  // Store the max eigenvalue and its according eigenvector
+  const auto ev1 = solver.eigenvalues()(maxIdx).real();
+
+  Vector disturb = Vector10::Random();
+  disturb.normalize();
+  Vector initial = L.first.row(0);
+  double magnitude = initial.norm();
+  initial += 0.03 * magnitude * disturb;
+  AcceleratedPowerMethod<Matrix> apf(L.first, initial);
+  apf.compute(100, 1e-5);
+  // Check if the eigenvalue is the maximum eigen value
+  EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalue(), 1e-8);
+
+  // Check if the according ritz residual converged to the threshold
+  Vector actual1 = apf.eigenvector();
+  const double ritzValue = actual1.dot(L.first * actual1);
+  const double ritzResidual = (L.first * actual1 - ritzValue * actual1).norm();
+  EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -36,9 +36,18 @@ using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 
-// static SharedDiagonal
+typedef boost::tuple<size_t, size_t, double> BoostTriplet;
-//  sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2),
+bool triplet_equal(BoostTriplet a, BoostTriplet b) {
-//  constraintModel = noiseModel::Constrained::All(2);
+  if (a.get<0>() == b.get<0>() && a.get<1>() == b.get<1>() &&
+      a.get<2>() == b.get<2>()) return true;
+
+  cout << "not equal:" << endl;
+  cout << "\texpected: "
+      "(" << a.get<0>() << ", " << a.get<1>() << ") = " << a.get<2>() << endl;
+  cout << "\tactual:   "
+      "(" << b.get<0>() << ", " << b.get<1>() << ") = " << b.get<2>() << endl;
+  return false;
+}
 
 /* ************************************************************************* */
 TEST(GaussianFactorGraph, initialization) {
@@ -74,8 +83,8 @@ TEST(GaussianFactorGraph, sparseJacobian) {
   //  9 10  0 11 12 13
   //  0  0  0 14 15 16
 
-  // Expected - NOTE that we transpose this!
+  // Expected
-  Matrix expectedT = (Matrix(16, 3) <<
+  Matrix expected = (Matrix(16, 3) <<
       1., 1., 2.,
       1., 2., 4.,
       1., 3., 6.,
@@ -93,17 +102,32 @@ TEST(GaussianFactorGraph, sparseJacobian) {
       3., 6.,26.,
       4., 6.,32.).finished();
 
-  Matrix expected = expectedT.transpose();
+  // expected: in matlab format - NOTE the transpose!)
+  Matrix expectedMatlab = expected.transpose();
 
   GaussianFactorGraph gfg;
   SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
-  gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), Vector2(4., 8.), model);
+  const Key x123 = 0, x45 = 1;
-  gfg.add(0, (Matrix(2, 3) << 9., 10., 0., 0., 0., 0.).finished(), 1,
+  gfg.add(x123, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(),
-          (Matrix(2, 2) << 11., 12., 14., 15.).finished(), Vector2(13., 16.), model);
+          Vector2(4, 8), model);
+  gfg.add(x123, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(),
+          x45,  (Matrix(2, 2) << 11, 12, 14, 15.).finished(),
+          Vector2(13, 16), model);
 
   Matrix actual = gfg.sparseJacobian_();
 
-  EXPECT(assert_equal(expected, actual));
+  EXPECT(assert_equal(expectedMatlab, actual));
+
+  // BoostTriplets
+  auto boostActual = gfg.sparseJacobian();
+  // check the triplets size...
+  EXPECT_LONGS_EQUAL(16, boostActual.size());
+  // check content
+  for (int i = 0; i < 16; i++) {
+    EXPECT(triplet_equal(
+        BoostTriplet(expected(i, 0) - 1, expected(i, 1) - 1, expected(i, 2)),
+        boostActual.at(i)));
+  }
 }
 
 /* ************************************************************************* */

gtsam/linear/tests/testPowerMethod.cpp

@@ -0,0 +1,124 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * testPowerMethod.cpp
+ *
+ * @file   testPowerMethod.cpp
+ * @date   Sept 2020
+ * @author Jing Wu
+ * @brief  Check eigenvalue and eigenvector computed by power method
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/VectorSpace.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/PowerMethod.h>
+#include <gtsam/linear/tests/powerMethodExample.h>
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <Eigen/Eigenvalues>
+#include <iostream>
+#include <random>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(PowerMethod, powerIteration) {
+  // test power iteration, beta is set to 0
+  Sparse A(6, 6);
+  A.coeffRef(0, 0) = 6;
+  A.coeffRef(1, 1) = 5;
+  A.coeffRef(2, 2) = 4;
+  A.coeffRef(3, 3) = 3;
+  A.coeffRef(4, 4) = 2;
+  A.coeffRef(5, 5) = 1;
+  Vector initial = (Vector(6) << 0.24434602, 0.22829942, 0.70094486, 0.15463092, 0.55871359,
+       0.2465342).finished();
+  PowerMethod<Sparse> pf(A, initial);
+  pf.compute(100, 1e-5);
+  EXPECT_LONGS_EQUAL(6, pf.eigenvector().rows());
+
+  Vector6 actual1 = pf.eigenvector();
+  const double ritzValue = actual1.dot(A * actual1);
+  const double ritzResidual = (A * actual1 - ritzValue * actual1).norm();
+  EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);
+
+  const double ev1 = 6.0;
+  EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(PowerMethod, useFactorGraphSparse) {
+  // Let's make a scalar synchronization graph with 4 nodes
+  GaussianFactorGraph fg = gtsam::linear::test::example::createSparseGraph();
+
+  // Get eigenvalues and eigenvectors with Eigen
+  auto L = fg.hessian();
+  Eigen::EigenSolver<Matrix> solver(L.first);
+
+  // find the index of the max eigenvalue
+  size_t maxIdx = 0;
+  for (auto i = 0; i < solver.eigenvalues().rows(); ++i) {
+    if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real())
+      maxIdx = i;
+  }
+  // Store the max eigenvalue and its according eigenvector
+  const auto ev1 = solver.eigenvalues()(maxIdx).real();
+
+  Vector initial = Vector4::Random();
+  PowerMethod<Matrix> pf(L.first, initial);
+  pf.compute(100, 1e-5);
+  EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-8);
+  auto actual2 = pf.eigenvector();
+  const double ritzValue = actual2.dot(L.first * actual2);
+  const double ritzResidual = (L.first * actual2 - ritzValue * actual2).norm();
+  EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(PowerMethod, useFactorGraphDense) {
+  // Let's make a scalar synchronization graph with 10 nodes
+  GaussianFactorGraph fg = gtsam::linear::test::example::createDenseGraph();
+
+  // Get eigenvalues and eigenvectors with Eigen
+  auto L = fg.hessian();
+  Eigen::EigenSolver<Matrix> solver(L.first);
+
+  // find the index of the max eigenvalue
+  size_t maxIdx = 0;
+  for (auto i = 0; i < solver.eigenvalues().rows(); ++i) {
+    if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real())
+      maxIdx = i;
+  }
+  // Store the max eigenvalue and its according eigenvector
+  const auto ev1 = solver.eigenvalues()(maxIdx).real();
+
+  Vector initial = Vector10::Random();
+  PowerMethod<Matrix> pf(L.first, initial);
+  pf.compute(100, 1e-5);
+  EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-8);
+  auto actual2 = pf.eigenvector();
+  const double ritzValue = actual2.dot(L.first * actual2);
+  const double ritzResidual = (L.first * actual2 - ritzValue * actual2).norm();
+  EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/linear/tests/testSparseEigen.cpp

@@ -0,0 +1,72 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testSparseMatrix.cpp
+ * @author  Mandy Xie
+ * @author  Fan Jiang
+ * @author  Gerry Chen
+ * @author  Frank Dellaert
+ * @date    Jan, 2021
+ */
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/SparseEigen.h>
+
+#include <boost/assign/list_of.hpp>
+using boost::assign::list_of;
+
+#include <gtsam/base/TestableAssertions.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(SparseEigen, sparseJacobianEigen) {
+  GaussianFactorGraph gfg;
+  SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
+  const Key x123 = 0, x45 = 1;
+  gfg.add(x123, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(),
+          Vector2(4, 8), model);
+  gfg.add(x123, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(),
+          x45,  (Matrix(2, 2) << 11, 12, 14, 15.).finished(),
+          Vector2(13, 16), model);
+
+  // Sparse Matrix
+  auto sparseResult = sparseJacobianEigen(gfg);
+  EXPECT_LONGS_EQUAL(16, sparseResult.nonZeros());
+  EXPECT(assert_equal(4, sparseResult.rows()));
+  EXPECT(assert_equal(6, sparseResult.cols()));
+  EXPECT(assert_equal(gfg.augmentedJacobian(), Matrix(sparseResult)));
+
+  // Call sparseJacobian with optional ordering...
+  auto ordering = Ordering(list_of(x45)(x123));
+
+  // Eigen Sparse with optional ordering
+  EXPECT(assert_equal(gfg.augmentedJacobian(ordering),
+                      Matrix(sparseJacobianEigen(gfg, ordering))));
+
+  // Check matrix dimensions when zero rows / cols
+  gfg.add(x123, Matrix23::Zero(), Vector2::Zero(), model);  // zero row
+  gfg.add(2, Matrix21::Zero(), Vector2::Zero(), model);     // zero col
+  sparseResult = sparseJacobianEigen(gfg);
+  EXPECT_LONGS_EQUAL(16, sparseResult.nonZeros());
+  EXPECT(assert_equal(8, sparseResult.rows()));
+  EXPECT(assert_equal(7, sparseResult.cols()));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/navigation/ImuBias.h

@@ -29,8 +29,8 @@ namespace imuBias {
 
 class GTSAM_EXPORT ConstantBias {
 private:
-  Vector3 biasAcc_;
+  Vector3 biasAcc_; ///< The units for stddev are σ = m/s² or m √Hz/s²
-  Vector3 biasGyro_;
+  Vector3 biasGyro_; ///< The units for stddev are σ = rad/s or rad √Hz/s
 
 public:
   /// dimension of the variable - used to autodetect sizes

gtsam/navigation/PreintegratedRotation.h

@@ -29,7 +29,9 @@ namespace gtsam {
 /// Parameters for pre-integration:
 /// Usage: Create just a single Params and pass a shared pointer to the constructor
 struct GTSAM_EXPORT PreintegratedRotationParams {
-  Matrix3 gyroscopeCovariance;  ///< continuous-time "Covariance" of gyroscope measurements
+  /// Continuous-time "Covariance" of gyroscope measurements
+  /// The units for stddev are σ = rad/s/√Hz
+  Matrix3 gyroscopeCovariance;
   boost::optional<Vector3> omegaCoriolis;  ///< Coriolis constant
   boost::optional<Pose3> body_P_sensor;    ///< The pose of the sensor in the body frame
 

gtsam/navigation/PreintegrationParams.h

@@ -24,7 +24,9 @@ namespace gtsam {
 /// Parameters for pre-integration:
 /// Usage: Create just a single Params and pass a shared pointer to the constructor
 struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
-  Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
+  /// Continuous-time "Covariance" of accelerometer
+  /// The units for stddev are σ = m/s²/√Hz
+  Matrix3 accelerometerCovariance;
   Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
   bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
   Vector3 n_gravity; ///< Gravity vector in nav frame

gtsam/nonlinear/FunctorizedFactor.h

@@ -87,8 +87,8 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
         NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
   }
 
-  Vector evaluateError(const T &params,
+  Vector evaluateError(const T &params, boost::optional<Matrix &> H =
-                       boost::optional<Matrix &> H = boost::none) const override {
+                                            boost::none) const override {
     R x = func_(params, H);
     Vector error = traits<R>::Local(measured_, x);
     return error;
@@ -96,7 +96,8 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
 
   /// @name Testable
   /// @{
-  void print(const std::string &s = "",
+  void print(
+      const std::string &s = "",
       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
@@ -144,4 +145,111 @@ FunctorizedFactor<R, T> MakeFunctorizedFactor(Key key, const R &z,
   return FunctorizedFactor<R, T>(key, z, model, func);
 }
 
+/**
+ * Factor which evaluates provided binary functor and uses the result to compute
+ * error with respect to the provided measurement.
+ *
+ * Template parameters are
+ * @param R: The return type of the functor after evaluation.
+ * @param T1: The first argument type for the functor.
+ * @param T2: The second argument type for the functor.
+ */
+template <typename R, typename T1, typename T2>
+class GTSAM_EXPORT FunctorizedFactor2 : public NoiseModelFactor2<T1, T2> {
+ private:
+  using Base = NoiseModelFactor2<T1, T2>;
+
+  R measured_;  ///< value that is compared with functor return value
+  SharedNoiseModel noiseModel_;  ///< noise model
+  using FunctionType = std::function<R(T1, T2, boost::optional<Matrix &>,
+                                       boost::optional<Matrix &>)>;
+  FunctionType func_;  ///< functor instance
+
+ public:
+  /** default constructor - only use for serialization */
+  FunctorizedFactor2() {}
+
+  /** Construct with given x and the parameters of the basis
+   *
+   * @param key: Factor key
+   * @param z: Measurement object of same type as that returned by functor
+   * @param model: Noise model
+   * @param func: The instance of the functor object
+   */
+  FunctorizedFactor2(Key key1, Key key2, const R &z,
+                     const SharedNoiseModel &model, const FunctionType func)
+      : Base(model, key1, key2),
+        measured_(z),
+        noiseModel_(model),
+        func_(func) {}
+
+  virtual ~FunctorizedFactor2() {}
+
+  /// @return a deep copy of this factor
+  NonlinearFactor::shared_ptr clone() const override {
+    return boost::static_pointer_cast<NonlinearFactor>(
+        NonlinearFactor::shared_ptr(new FunctorizedFactor2<R, T1, T2>(*this)));
+  }
+
+  Vector evaluateError(
+      const T1 &params1, const T2 &params2,
+      boost::optional<Matrix &> H1 = boost::none,
+      boost::optional<Matrix &> H2 = boost::none) const override {
+    R x = func_(params1, params2, H1, H2);
+    Vector error = traits<R>::Local(measured_, x);
+    return error;
+  }
+
+  /// @name Testable
+  /// @{
+  void print(
+      const std::string &s = "",
+      const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
+    Base::print(s, keyFormatter);
+    std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2("
+              << keyFormatter(this->key1()) << ", "
+              << keyFormatter(this->key2()) << ")" << std::endl;
+    traits<R>::Print(measured_, "  measurement: ");
+    std::cout << "  noise model sigmas: " << noiseModel_->sigmas().transpose()
+              << std::endl;
+  }
+
+  bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
+    const FunctorizedFactor2<R, T1, T2> *e =
+        dynamic_cast<const FunctorizedFactor2<R, T1, T2> *>(&other);
+    return e && Base::equals(other, tol) &&
+           traits<R>::Equals(this->measured_, e->measured_, tol);
+  }
+  /// @}
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &boost::serialization::make_nvp(
+        "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
+    ar &BOOST_SERIALIZATION_NVP(measured_);
+    ar &BOOST_SERIALIZATION_NVP(func_);
+  }
+};
+
+/// traits
+template <typename R, typename T1, typename T2>
+struct traits<FunctorizedFactor2<R, T1, T2>>
+    : public Testable<FunctorizedFactor2<R, T1, T2>> {};
+
+/**
+ * Helper function to create a functorized factor.
+ *
+ * Uses function template deduction to identify return type and functor type, so
+ * template list only needs the functor argument type.
+ */
+template <typename T1, typename T2, typename R, typename FUNC>
+FunctorizedFactor2<R, T1, T2> MakeFunctorizedFactor2(
+    Key key1, Key key2, const R &z, const SharedNoiseModel &model,
+    const FUNC func) {
+  return FunctorizedFactor2<R, T1, T2>(key1, key2, z, model, func);
+}
+
 }  // namespace gtsam

gtsam/nonlinear/GaussNewtonOptimizer.h

@@ -28,6 +28,8 @@ class GaussNewtonOptimizer;
  * NonlinearOptimizationParams.
  */
 class GTSAM_EXPORT GaussNewtonParams : public NonlinearOptimizerParams {
+public:
+  using OptimizerType = GaussNewtonOptimizer;
 };
 
 /**

gtsam/nonlinear/GncOptimizer.h

@@ -0,0 +1,338 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    GncOptimizer.h
+ * @brief   The GncOptimizer class
+ * @author  Jingnan Shi
+ * @author  Luca Carlone
+ * @author  Frank Dellaert
+ *
+ * Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non-Convexity for Robust Spatial Perception:
+ * From Non-Minimal Solvers to Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf)
+ *
+ * See also:
+ * Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications",
+ * arxiv: https://arxiv.org/pdf/2007.15109.pdf, 2020.
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/GncParams.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+template<class GncParameters>
+class GncOptimizer {
+ public:
+  /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
+  typedef typename GncParameters::OptimizerType BaseOptimizer;
+
+ private:
+  NonlinearFactorGraph nfg_; ///< Original factor graph to be solved by GNC.
+  Values state_; ///< Initial values to be used at each iteration by GNC.
+  GncParameters params_; ///< GNC parameters.
+  Vector weights_;  ///< Weights associated to each factor in GNC (this could be a local variable in optimize, but it is useful to make it accessible from outside).
+
+ public:
+  /// Constructor.
+  GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
+               const GncParameters& params = GncParameters())
+      : state_(initialValues),
+        params_(params) {
+
+    // make sure all noiseModels are Gaussian or convert to Gaussian
+    nfg_.resize(graph.size());
+    for (size_t i = 0; i < graph.size(); i++) {
+      if (graph[i]) {
+        NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast<
+            NoiseModelFactor>(graph[i]);
+        auto robust = boost::dynamic_pointer_cast<
+            noiseModel::Robust>(factor->noiseModel());
+        // if the factor has a robust loss, we remove the robust loss
+        nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor;
+      }
+    }
+  }
+
+  /// Access a copy of the internal factor graph.
+  const NonlinearFactorGraph& getFactors() const { return nfg_; }
+
+  /// Access a copy of the internal values.
+  const Values& getState() const { return state_; }
+
+  /// Access a copy of the parameters.
+  const GncParameters& getParams() const { return params_;}
+
+  /// Access a copy of the GNC weights.
+  const Vector& getWeights() const { return weights_;}
+
+  /// Compute optimal solution using graduated non-convexity.
+  Values optimize() {
+    // start by assuming all measurements are inliers
+    weights_ = Vector::Ones(nfg_.size());
+    BaseOptimizer baseOptimizer(nfg_, state_);
+    Values result = baseOptimizer.optimize();
+    double mu = initializeMu();
+    double prev_cost = nfg_.error(result);
+    double cost = 0.0;  // this will be updated in the main loop
+
+    // handle the degenerate case that corresponds to small
+    // maximum residual errors at initialization
+    // For GM: if residual error is small, mu -> 0
+    // For TLS: if residual error is small, mu -> -1
+    if (mu <= 0) {
+      if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
+        std::cout << "GNC Optimizer stopped because maximum residual at "
+                  "initialization is small."
+                  << std::endl;
+      }
+      if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
+        result.print("result\n");
+        std::cout << "mu: " << mu << std::endl;
+      }
+      return result;
+    }
+
+    size_t iter;
+    for (iter = 0; iter < params_.maxIterations; iter++) {
+
+      // display info
+      if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
+        std::cout << "iter: " << iter << std::endl;
+        result.print("result\n");
+        std::cout << "mu: " << mu << std::endl;
+        std::cout << "weights: " << weights_ << std::endl;
+      }
+      // weights update
+      weights_ = calculateWeights(result, mu);
+
+      // variable/values update
+      NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_);
+      BaseOptimizer baseOptimizer_iter(graph_iter, state_);
+      result = baseOptimizer_iter.optimize();
+
+      // stopping condition
+      cost = graph_iter.error(result);
+      if (checkConvergence(mu, weights_, cost, prev_cost)) {
+        break;
+      }
+
+      // update mu
+      mu = updateMu(mu);
+
+      // get ready for next iteration
+      prev_cost = cost;
+
+      // display info
+      if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
+        std::cout << "previous cost: " << prev_cost << std::endl;
+        std::cout << "current cost: " << cost << std::endl;
+      }
+    }
+    // display info
+    if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
+      std::cout << "final iterations: " << iter << std::endl;
+      std::cout << "final mu: " << mu << std::endl;
+      std::cout << "final weights: " << weights_ << std::endl;
+      std::cout << "previous cost: " << prev_cost << std::endl;
+      std::cout << "current cost: " << cost << std::endl;
+    }
+    return result;
+  }
+
+  /// Initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper).
+  double initializeMu() const {
+    // compute largest error across all factors
+    double rmax_sq = 0.0;
+    for (size_t i = 0; i < nfg_.size(); i++) {
+      if (nfg_[i]) {
+        rmax_sq = std::max(rmax_sq, nfg_[i]->error(state_));
+      }
+    }
+    // set initial mu
+    switch (params_.lossType) {
+      case GncLossType::GM:
+        // surrogate cost is convex for large mu
+        return 2 * rmax_sq / params_.barcSq;  // initial mu
+      case GncLossType::TLS:
+        /* initialize mu to the value specified in Remark 5 in GNC paper.
+         surrogate cost is convex for mu close to zero
+         degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main loop)
+         according to remark mu = params_.barcSq / (2 * rmax_sq - params_.barcSq) = params_.barcSq/ excessResidual
+         however, if the denominator is 0 or negative, we return mu = -1 which leads to termination of the main GNC loop
+         */
+        return
+            (2 * rmax_sq - params_.barcSq) > 0 ?
+                params_.barcSq / (2 * rmax_sq - params_.barcSq) : -1;
+      default:
+        throw std::runtime_error(
+            "GncOptimizer::initializeMu: called with unknown loss type.");
+    }
+  }
+
+  /// Update the gnc parameter mu to gradually increase nonconvexity.
+  double updateMu(const double mu) const {
+    switch (params_.lossType) {
+      case GncLossType::GM:
+        // reduce mu, but saturate at 1 (original cost is recovered for mu -> 1)
+        return std::max(1.0, mu / params_.muStep);
+      case GncLossType::TLS:
+        // increases mu at each iteration (original cost is recovered for mu -> inf)
+        return mu * params_.muStep;
+      default:
+        throw std::runtime_error(
+            "GncOptimizer::updateMu: called with unknown loss type.");
+    }
+  }
+
+  /// Check if we have reached the value of mu for which the surrogate loss matches the original loss.
+  bool checkMuConvergence(const double mu) const {
+    bool muConverged = false;
+    switch (params_.lossType) {
+      case GncLossType::GM:
+        muConverged = std::fabs(mu - 1.0) < 1e-9;  // mu=1 recovers the original GM function
+        break;
+      case GncLossType::TLS:
+        muConverged = false;  // for TLS there is no stopping condition on mu (it must tend to infinity)
+        break;
+      default:
+        throw std::runtime_error(
+            "GncOptimizer::checkMuConvergence: called with unknown loss type.");
+    }
+    if (muConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
+      std::cout << "muConverged = true " << std::endl;
+    return muConverged;
+  }
+
+  /// Check convergence of relative cost differences.
+  bool checkCostConvergence(const double cost, const double prev_cost) const {
+    bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7)
+        < params_.relativeCostTol;
+    if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
+      std::cout << "checkCostConvergence = true " << std::endl;
+    return costConverged;
+  }
+
+  /// Check convergence of weights to binary values.
+  bool checkWeightsConvergence(const Vector& weights) const {
+    bool weightsConverged = false;
+    switch (params_.lossType) {
+      case GncLossType::GM:
+        weightsConverged = false;  // for GM, there is no clear binary convergence for the weights
+        break;
+      case GncLossType::TLS:
+        weightsConverged = true;
+        for (size_t i = 0; i < weights.size(); i++) {
+          if (std::fabs(weights[i] - std::round(weights[i]))
+              > params_.weightsTol) {
+            weightsConverged = false;
+            break;
+          }
+        }
+        break;
+      default:
+        throw std::runtime_error(
+            "GncOptimizer::checkWeightsConvergence: called with unknown loss type.");
+    }
+    if (weightsConverged
+        && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
+      std::cout << "weightsConverged = true " << std::endl;
+    return weightsConverged;
+  }
+
+  /// Check for convergence between consecutive GNC iterations.
+  bool checkConvergence(const double mu, const Vector& weights,
+                        const double cost, const double prev_cost) const {
+    return checkCostConvergence(cost, prev_cost)
+        || checkWeightsConvergence(weights) || checkMuConvergence(mu);
+  }
+
+  /// Create a graph where each factor is weighted by the gnc weights.
+  NonlinearFactorGraph makeWeightedGraph(const Vector& weights) const {
+    // make sure all noiseModels are Gaussian or convert to Gaussian
+    NonlinearFactorGraph newGraph;
+    newGraph.resize(nfg_.size());
+    for (size_t i = 0; i < nfg_.size(); i++) {
+      if (nfg_[i]) {
+        auto factor = boost::dynamic_pointer_cast<
+            NoiseModelFactor>(nfg_[i]);
+        auto noiseModel =
+            boost::dynamic_pointer_cast<noiseModel::Gaussian>(
+                factor->noiseModel());
+        if (noiseModel) {
+          Matrix newInfo = weights[i] * noiseModel->information();
+          auto newNoiseModel = noiseModel::Gaussian::Information(newInfo);
+          newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel);
+        } else {
+          throw std::runtime_error(
+              "GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model.");
+        }
+      }
+    }
+    return newGraph;
+  }
+
+  /// Calculate gnc weights.
+  Vector calculateWeights(const Values& currentEstimate, const double mu) {
+    Vector weights = Vector::Ones(nfg_.size());
+
+    // do not update the weights that the user has decided are known inliers
+    std::vector<size_t> allWeights;
+    for (size_t k = 0; k < nfg_.size(); k++) {
+      allWeights.push_back(k);
+    }
+    std::vector<size_t> unknownWeights;
+    std::set_difference(allWeights.begin(), allWeights.end(),
+                        params_.knownInliers.begin(),
+                        params_.knownInliers.end(),
+                        std::inserter(unknownWeights, unknownWeights.begin()));
+
+    // update weights of known inlier/outlier measurements
+    switch (params_.lossType) {
+      case GncLossType::GM: {  // use eq (12) in GNC paper
+        for (size_t k : unknownWeights) {
+          if (nfg_[k]) {
+            double u2_k = nfg_[k]->error(currentEstimate);  // squared (and whitened) residual
+            weights[k] = std::pow(
+                (mu * params_.barcSq) / (u2_k + mu * params_.barcSq), 2);
+          }
+        }
+        return weights;
+      }
+      case GncLossType::TLS: {  // use eq (14) in GNC paper
+        double upperbound = (mu + 1) / mu * params_.barcSq;
+        double lowerbound = mu / (mu + 1) * params_.barcSq;
+        for (size_t k : unknownWeights) {
+          if (nfg_[k]) {
+            double u2_k = nfg_[k]->error(currentEstimate);  // squared (and whitened) residual
+            if (u2_k >= upperbound) {
+              weights[k] = 0;
+            } else if (u2_k <= lowerbound) {
+              weights[k] = 1;
+            } else {
+              weights[k] = std::sqrt(params_.barcSq * mu * (mu + 1) / u2_k)
+                  - mu;
+            }
+          }
+        }
+        return weights;
+      }
+      default:
+        throw std::runtime_error(
+            "GncOptimizer::calculateWeights: called with unknown loss type.");
+    }
+  }
+};
+
+}

gtsam/nonlinear/GncParams.h

@@ -0,0 +1,164 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    GncOptimizer.h
+ * @brief   The GncOptimizer class
+ * @author  Jingnan Shi
+ * @author  Luca Carlone
+ * @author  Frank Dellaert
+ *
+ * Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non-Convexity for Robust Spatial Perception:
+ * From Non-Minimal Solvers to Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf)
+ *
+ * See also:
+ * Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications",
+ * arxiv: https://arxiv.org/pdf/2007.15109.pdf, 2020.
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+/// Choice of robust loss function for GNC.
+enum GncLossType {
+  GM /*Geman McClure*/,
+  TLS /*Truncated least squares*/
+};
+
+template<class BaseOptimizerParameters>
+class GncParams {
+ public:
+  /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
+  typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;
+
+  /// Verbosity levels
+  enum Verbosity {
+    SILENT = 0,
+    SUMMARY,
+    VALUES
+  };
+
+  /// Constructor.
+  GncParams(const BaseOptimizerParameters& baseOptimizerParams)
+      : baseOptimizerParams(baseOptimizerParams) {
+  }
+
+  /// Default constructor.
+  GncParams()
+      : baseOptimizerParams() {
+  }
+
+  /// GNC parameters.
+  BaseOptimizerParameters baseOptimizerParams;  ///< Optimization parameters used to solve the weighted least squares problem at each GNC iteration
+  /// any other specific GNC parameters:
+  GncLossType lossType = TLS;  ///< Default loss
+  size_t maxIterations = 100;  ///<  Maximum number of iterations
+  double barcSq = 1.0;  ///< A factor is considered an inlier if factor.error() < barcSq. Note that factor.error() whitens by the covariance
+  double muStep = 1.4;  ///< Multiplicative factor to reduce/increase the mu in gnc
+  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
+
+  /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
+  void setLossType(const GncLossType type) {
+    lossType = type;
+  }
+
+  /// Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate solutions and is not recommended).
+  void setMaxIterations(const size_t maxIter) {
+    std::cout
+        << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! "
+        << std::endl;
+    maxIterations = maxIter;
+  }
+
+  /** Set the maximum weighted residual error for an inlier. For a factor in the form f(x) = 0.5 * || r(x) ||^2_Omega,
+   * the inlier threshold is the largest value of f(x) for the corresponding measurement to be considered an inlier.
+   * In other words, an inlier at x is such that 0.5 * || r(x) ||^2_Omega <= barcSq.
+   * Assuming a isotropic measurement covariance sigma^2 * Identity, the cost becomes: 0.5 * 1/sigma^2 || r(x) ||^2 <= barcSq.
+   * Hence || r(x) ||^2 <= 2 * barcSq * sigma^2.
+   * */
+  void setInlierCostThreshold(const double inth) {
+    barcSq = inth;
+  }
+
+  /// Set the graduated non-convexity step: at each GNC iteration, mu is updated as mu <- mu * muStep.
+  void setMuStep(const double step) {
+    muStep = step;
+  }
+
+  /// Set the maximum relative difference in mu values to stop iterating.
+  void setRelativeCostTol(double value) {
+    relativeCostTol = value;
+  }
+
+  /// Set the maximum difference between the weights and their rounding in {0,1} to stop iterating.
+  void setWeightsTol(double value) {
+    weightsTol = value;
+  }
+
+  /// Set the verbosity level.
+  void setVerbosityGNC(const Verbosity value) {
+    verbosity = value;
+  }
+
+  /** (Optional) Provide a vector of measurements that must be considered inliers. The enties in the vector
+   * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg,
+   * and you provide  knownIn = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15].
+   * 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) {
+    for (size_t i = 0; i < knownIn.size(); i++)
+      knownInliers.push_back(knownIn[i]);
+  }
+
+  /// Equals.
+  bool equals(const GncParams& other, double tol = 1e-9) const {
+    return baseOptimizerParams.equals(other.baseOptimizerParams)
+        && lossType == other.lossType && maxIterations == other.maxIterations
+        && std::fabs(barcSq - other.barcSq) <= tol
+        && std::fabs(muStep - other.muStep) <= tol
+        && verbosity == other.verbosity && knownInliers == other.knownInliers;
+  }
+
+  /// Print.
+  void print(const std::string& str) const {
+    std::cout << str << "\n";
+    switch (lossType) {
+      case GM:
+        std::cout << "lossType: Geman McClure" << "\n";
+        break;
+      case TLS:
+        std::cout << "lossType: Truncated Least-squares" << "\n";
+        break;
+      default:
+        throw std::runtime_error("GncParams::print: unknown loss type.");
+    }
+    std::cout << "maxIterations: " << maxIterations << "\n";
+    std::cout << "barcSq: " << barcSq << "\n";
+    std::cout << "muStep: " << muStep << "\n";
+    std::cout << "relativeCostTol: " << relativeCostTol << "\n";
+    std::cout << "weightsTol: " << weightsTol << "\n";
+    std::cout << "verbosity: " << verbosity << "\n";
+    for (size_t i = 0; i < knownInliers.size(); i++)
+      std::cout << "knownInliers: " << knownInliers[i] << "\n";
+    baseOptimizerParams.print(str);
+  }
+};
+
+}

gtsam/nonlinear/LevenbergMarquardtParams.h

@@ -25,6 +25,8 @@
 
 namespace gtsam {
 
+class LevenbergMarquardtOptimizer;
+
 /** Parameters for Levenberg-Marquardt optimization.  Note that this parameters
  * class inherits from NonlinearOptimizerParams, which specifies the parameters
  * common to all nonlinear optimization algorithms.  This class also contains
@@ -40,6 +42,7 @@ public:
 
   static VerbosityLM verbosityLMTranslator(const std::string &s);
   static std::string verbosityLMTranslator(VerbosityLM value);
+  using OptimizerType = LevenbergMarquardtOptimizer;
 
 public:
 

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h

@@ -200,6 +200,10 @@ boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
     currentValues = system.advance(prevValues, alpha, direction);
     currentError = system.error(currentValues);
 
+    // User hook:
+    if (params.iterationHook)
+      params.iterationHook(iteration, prevError, currentError);
+
     // Maybe show output
     if (params.verbosity >= NonlinearOptimizerParams::ERROR)
       std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl;

gtsam/nonlinear/NonlinearFactor.cpp

@@ -76,6 +76,14 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
               && noiseModel_->equals(*e->noiseModel_, tol)));
 }
 
+/* ************************************************************************* */
+NoiseModelFactor::shared_ptr NoiseModelFactor::cloneWithNewNoiseModel(
+    const SharedNoiseModel newNoise) const {
+  NoiseModelFactor::shared_ptr new_factor = boost::dynamic_pointer_cast<NoiseModelFactor>(clone());
+  new_factor->noiseModel_ = newNoise;
+  return new_factor;
+}
+
 /* ************************************************************************* */
 static void check(const SharedNoiseModel& noiseModel, size_t m) {
   if (noiseModel && m != noiseModel->dim())

gtsam/nonlinear/NonlinearFactor.h

@@ -244,6 +244,12 @@ public:
    */
   boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
 
+  /**
+   * Creates a shared_ptr clone of the
+   * factor with a new noise model
+   */
+  shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const;
+
  private:
   /** Serialization function */
   friend class boost::serialization::access;

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -34,6 +34,7 @@
 #endif
 
 #include <cmath>
+#include <fstream>
 #include <limits>
 
 using namespace std;
@@ -256,6 +257,16 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
   stm << "}\n";
 }
 
+/* ************************************************************************* */
+void NonlinearFactorGraph::saveGraph(
+    const std::string& file, const Values& values,
+    const GraphvizFormatting& graphvizFormatting,
+    const KeyFormatter& keyFormatter) const {
+  std::ofstream of(file);
+  saveGraph(of, values, graphvizFormatting, keyFormatter);
+  of.close();
+}
+
 /* ************************************************************************* */
 double NonlinearFactorGraph::error(const Values& values) const {
   gttic(NonlinearFactorGraph_error);
@@ -365,7 +376,7 @@ static Scatter scatterFromValues(const Values& values) {
   scatter.reserve(values.size());
 
   // use "natural" ordering with keys taken from the initial values
-  for (const auto& key_value : values) {
+  for (const auto key_value : values) {
     scatter.add(key_value.key, key_value.value.dim());
   }
 

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -11,7 +11,7 @@
 
 /**
  * @file    NonlinearFactorGraph.h
- * @brief   Factor Graph Constsiting of non-linear factors
+ * @brief   Factor Graph consisting of non-linear factors
  * @author  Frank Dellaert
  * @author  Carlos Nieto
  * @author  Christian Potthast
@@ -111,11 +111,21 @@ namespace gtsam {
     /** Test equality */
     bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
 
-    /** Write the graph in GraphViz format for visualization */
+    /// Write the graph in GraphViz format for visualization
     void saveGraph(std::ostream& stm, const Values& values = Values(),
       const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
+    /**
+     * Write the graph in GraphViz format to file for visualization.
+     *
+     * This is a wrapper friendly version since wrapped languages don't have
+     * access to C++ streams.
+     */
+    void saveGraph(const std::string& file, const Values& values = Values(),
+      const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+
     /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */
     double error(const Values& values) const;
 

gtsam/nonlinear/NonlinearOptimizer.cpp

@@ -98,6 +98,10 @@ void NonlinearOptimizer::defaultOptimize() {
     // Update newError for either printouts or conditional-end checks:
     newError = error();
 
+    // User hook:
+    if (params.iterationHook)
+      params.iterationHook(iterations(), currentError, newError);
+
     // Maybe show output
     if (params.verbosity >= NonlinearOptimizerParams::VALUES)
       values().print("newValues");

gtsam/nonlinear/NonlinearOptimizer.h

@@ -81,7 +81,7 @@ protected:
 
 public:
   /** A shared pointer to this class */
-  typedef boost::shared_ptr<const NonlinearOptimizer> shared_ptr;
+  using shared_ptr = boost::shared_ptr<const NonlinearOptimizer>;
 
   /// @name Standard interface
   /// @{

gtsam/nonlinear/NonlinearOptimizerParams.h

@@ -38,21 +38,12 @@ public:
     SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR
   };
 
-  size_t maxIterations; ///< The maximum iterations to stop iterating (default 100)
+  size_t maxIterations = 100; ///< The maximum iterations to stop iterating (default 100)
-  double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5)
+  double relativeErrorTol = 1e-5; ///< The maximum relative error decrease to stop iterating (default 1e-5)
-  double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5)
+  double absoluteErrorTol = 1e-5; ///< The maximum absolute error decrease to stop iterating (default 1e-5)
-  double errorTol; ///< The maximum total error to stop iterating (default 0.0)
+  double errorTol = 0.0; ///< The maximum total error to stop iterating (default 0.0)
-  Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT)
+  Verbosity verbosity = SILENT; ///< The printing verbosity during optimization (default SILENT)
-  Ordering::OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD)
+  Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD)
-
-  NonlinearOptimizerParams() :
-      maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol(
-          0.0), verbosity(SILENT), orderingType(Ordering::COLAMD),
-          linearSolverType(MULTIFRONTAL_CHOLESKY) {}
-
-  virtual ~NonlinearOptimizerParams() {
-  }
-  virtual void print(const std::string& str = "") const;
 
   size_t getMaxIterations() const { return maxIterations; }
   double getRelativeErrorTol() const { return relativeErrorTol; }
@@ -71,6 +62,37 @@ public:
   static Verbosity verbosityTranslator(const std::string &s) ;
   static std::string verbosityTranslator(Verbosity value) ;
 
+  /** Type for an optional user-provided hook to be called after each
+   * internal optimizer iteration. See iterationHook below. */
+  using IterationHook = std::function<
+    void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>;
+
+  /** Optional user-provided iteration hook to be called after each
+   * optimization iteration (Default: none).
+   * Note that `IterationHook` is defined as a std::function<> with this
+   * signature:
+   * \code
+   *  void(size_t iteration, double errorBefore, double errorAfter)
+   * \endcode
+   * which allows binding by means of a reference to a regular function:
+   * \code
+   *  void foo(size_t iteration, double errorBefore, double errorAfter);
+   *  // ...
+   *  lmOpts.iterationHook = &foo;
+   * \endcode
+   * or to a C++11 lambda (preferred if you need to capture additional
+   * context variables, such that the optimizer object itself, the factor graph,
+   * etc.):
+   * \code
+   *  lmOpts.iterationHook = [&](size_t iter, double oldError, double newError)
+   *  {
+   *    // ...
+   *  };
+   * \endcode
+   * or to the result of a properly-formed `std::bind` call.
+   */
+  IterationHook iterationHook;
+
   /** See NonlinearOptimizerParams::linearSolverType */
   enum LinearSolverType {
     MULTIFRONTAL_CHOLESKY,
@@ -81,10 +103,27 @@ public:
     CHOLMOD, /* Experimental Flag */
   };
 
-  LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer
+  LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer
   boost::optional<Ordering> ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty)
   IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers.
 
+  NonlinearOptimizerParams() = default;
+  virtual ~NonlinearOptimizerParams() {
+  }
+
+  virtual void print(const std::string& str = "") const;
+
+  bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const {
+    return maxIterations == other.getMaxIterations()
+        && std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol
+        && std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol
+        && std::abs(errorTol - other.getErrorTol()) <= tol
+        && verbosityTranslator(verbosity) == other.getVerbosity();
+    //  && orderingType.equals(other.getOrderingType()_;
+    // && linearSolverType == other.getLinearSolverType();
+    // TODO: check ordering, iterativeParams, and iterationsHook
+  }
+
   inline bool isMultifrontal() const {
     return (linearSolverType == MULTIFRONTAL_CHOLESKY)
         || (linearSolverType == MULTIFRONTAL_QR);

gtsam/nonlinear/Values-inl.h

@@ -217,7 +217,7 @@ namespace gtsam {
   /** Constructor from a Filtered view copies out all values */
   template<class ValueType>
   Values::Values(const Values::Filtered<ValueType>& view) {
-    for(const typename Filtered<ValueType>::KeyValuePair& key_value: view) {
+    for(const auto key_value: view) {
       Key key = key_value.key;
       insert(key, static_cast<const ValueType&>(key_value.value));
     }
@@ -226,7 +226,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class ValueType>
   Values::Values(const Values::ConstFiltered<ValueType>& view) {
-    for(const typename ConstFiltered<ValueType>::KeyValuePair& key_value: view) {
+    for(const auto key_value: view) {
       Key key = key_value.key;
       insert(key, static_cast<const ValueType&>(key_value.value));
     }
@@ -339,13 +339,12 @@ namespace gtsam {
 
    /* ************************************************************************* */
    template <typename ValueType>
-   ValueType Values::at(Key j) const {
+   const ValueType Values::at(Key j) const {
      // Find the item
      KeyValueMap::const_iterator item = values_.find(j);
 
      // Throw exception if it does not exist
-     if(item == values_.end())
+     if (item == values_.end()) throw ValuesKeyDoesNotExist("at", j);
-       throw ValuesKeyDoesNotExist("at", j);
 
      // Check the type and throw exception if incorrect
      // h() split in two lines to avoid internal compiler error (MSVC2017)

gtsam/nonlinear/Values.cpp

@@ -206,7 +206,7 @@ namespace gtsam {
   /* ************************************************************************* */
   size_t Values::dim() const {
     size_t result = 0;
-    for(const ConstKeyValuePair& key_value: *this) {
+    for(const auto key_value: *this) {
       result += key_value.value.dim();
     }
     return result;
@@ -215,7 +215,7 @@ namespace gtsam {
   /* ************************************************************************* */
   VectorValues Values::zeroVectors() const {
     VectorValues result;
-    for(const ConstKeyValuePair& key_value: *this)
+    for(const auto key_value: *this)
       result.insert(key_value.key, Vector::Zero(key_value.value.dim()));
     return result;
   }

gtsam/nonlinear/Values.h

@@ -188,7 +188,7 @@ namespace gtsam {
      * @return The stored value
      */
     template <typename ValueType>
-    ValueType at(Key j) const;
+    const ValueType at(Key j) const;
 
     /// version for double
     double atDouble(size_t key) const { return at<double>(key);}

gtsam/nonlinear/internal/LevenbergMarquardtState.h

@@ -126,7 +126,7 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState {
     noiseModelCache.resize(0);
     // for each of the variables, add a prior
     damped.reserve(damped.size() + values.size());
-    for (const auto& key_value : values) {
+    for (const auto key_value : values) {
       const Key key = key_value.key;
       const size_t dim = key_value.value.dim();
       const CachedModel* item = getCachedModel(dim);

gtsam/nonlinear/tests/testFunctorizedFactor.cpp

@@ -27,8 +27,15 @@
 using namespace std;
 using namespace gtsam;
 
+// Key for FunctorizedFactor
 Key key = Symbol('X', 0);
+
+// Keys for FunctorizedFactor2
+Key keyA = Symbol('A', 0);
+Key keyx = Symbol('x', 0);
+
 auto model = noiseModel::Isotropic::Sigma(9, 1);
+auto model2 = noiseModel::Isotropic::Sigma(3, 1);
 
 /// Functor that takes a matrix and multiplies every element by m
 class MultiplyFunctor {
@@ -44,6 +51,21 @@ class MultiplyFunctor {
   }
 };
 
+/// Functor that performs Ax where A is a matrix and x is a vector.
+class ProjectionFunctor {
+ public:
+  Vector operator()(const Matrix &A, const Vector &x,
+                    OptionalJacobian<-1, -1> H1 = boost::none,
+                    OptionalJacobian<-1, -1> H2 = boost::none) const {
+    if (H1) {
+      H1->resize(x.size(), A.size());
+      *H1 << I_3x3, I_3x3, I_3x3;
+    } 
+    if (H2) *H2 = A;
+    return A * x;
+  }
+};
+
 /* ************************************************************************* */
 // Test identity operation for FunctorizedFactor.
 TEST(FunctorizedFactor, Identity) {
@@ -88,7 +110,7 @@ TEST(FunctorizedFactor, Equality) {
   EXPECT(factor1.equals(factor2));
 }
 
-/* *************************************************************************** */
+/* ************************************************************************* */
 // Test Jacobians of FunctorizedFactor.
 TEST(FunctorizedFactor, Jacobians) {
   Matrix X = Matrix::Identity(3, 3);
@@ -168,6 +190,83 @@ TEST(FunctorizedFactor, Lambda) {
   EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
 }
 
+/* ************************************************************************* */
+// Test identity operation for FunctorizedFactor2.
+TEST(FunctorizedFactor, Identity2) {
+  // x = Ax since A is I_3x3
+  Matrix A = Matrix::Identity(3, 3);
+  Vector x = Vector::Ones(3);
+
+  auto functor = ProjectionFunctor();
+  auto factor =
+      MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, x, model2, functor);
+
+  Vector error = factor.evaluateError(A, x);
+
+  EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
+}
+
+/* ************************************************************************* */
+// Test Jacobians of FunctorizedFactor2.
+TEST(FunctorizedFactor, Jacobians2) {
+  Matrix A = Matrix::Identity(3, 3);
+  Vector x = Vector::Ones(3);
+  Matrix actualH1, actualH2;
+
+  auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, x, model2,
+                                                       ProjectionFunctor());
+
+  Values values;
+  values.insert<Matrix>(keyA, A);
+  values.insert<Vector>(keyx, x);
+
+  // Check Jacobians
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+// Test FunctorizedFactor2 using a std::function type.
+TEST(FunctorizedFactor, Functional2) {
+  Matrix A = Matrix::Identity(3, 3);
+  Vector3 x(1, 2, 3);
+  Vector measurement = A * x;
+
+  std::function<Matrix(Matrix, Matrix, boost::optional<Matrix &>,
+                       boost::optional<Matrix &>)>
+      functional = ProjectionFunctor();
+  auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement,
+                                                       model2, functional);
+
+  Vector error = factor.evaluateError(A, x);
+
+  EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
+}
+
+/* ************************************************************************* */
+// Test FunctorizedFactor2 with a lambda function.
+TEST(FunctorizedFactor, Lambda2) {
+  Matrix A = Matrix::Identity(3, 3);
+  Vector3 x = Vector3(1, 2, 3);
+  Matrix measurement = A * x;
+
+  auto lambda = [](const Matrix &A, const Vector &x,
+                   OptionalJacobian<-1, -1> H1 = boost::none,
+                   OptionalJacobian<-1, -1> H2 = boost::none) {
+    if (H1) {
+      H1->resize(x.size(), A.size());
+      *H1 << I_3x3, I_3x3, I_3x3;
+    } 
+    if (H2) *H2 = A;
+    return A * x;
+  };
+  // FunctorizedFactor<Matrix> factor(key, measurement, model, lambda);
+  auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement, model2, lambda);
+
+  Vector error = factor.evaluateError(A, x);
+
+  EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/nonlinear/tests/testValues.cpp

@@ -175,10 +175,11 @@ TEST(Values, basic_functions)
 {
   Values values;
   const Values& values_c = values;
-  values.insert(2, Vector3());
+  Matrix23 M1 = Matrix23::Zero(), M2 = Matrix23::Zero();
-  values.insert(4, Vector3());
+  values.insert(2, Vector3(0, 0, 0));
-  values.insert(6, Matrix23());
+  values.insert(4, Vector3(0, 0, 0));
-  values.insert(8, Matrix23());
+  values.insert(6, M1);
+  values.insert(8, M2);
 
   // find
   EXPECT_LONGS_EQUAL(4, values.find(4)->key);
@@ -335,7 +336,7 @@ TEST(Values, filter) {
   int i = 0;
   Values::Filtered<Value> filtered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
   EXPECT_LONGS_EQUAL(2, (long)filtered.size());
-  for(const Values::Filtered<>::KeyValuePair& key_value: filtered) {
+  for(const auto key_value: filtered) {
     if(i == 0) {
       LONGS_EQUAL(2, (long)key_value.key);
       try {key_value.value.cast<Pose2>();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");}
@@ -370,7 +371,7 @@ TEST(Values, filter) {
   i = 0;
   Values::ConstFiltered<Pose3> pose_filtered = values.filter<Pose3>();
   EXPECT_LONGS_EQUAL(2, (long)pose_filtered.size());
-  for(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value: pose_filtered) {
+  for(const auto key_value: pose_filtered) {
     if(i == 0) {
       EXPECT_LONGS_EQUAL(1, (long)key_value.key);
       EXPECT(assert_equal(pose1, key_value.value));
@@ -408,7 +409,7 @@ TEST(Values, Symbol_filter) {
   values.insert(Symbol('y', 3), pose3);
 
   int i = 0;
-  for(const Values::Filtered<Value>::KeyValuePair& key_value: values.filter(Symbol::ChrTest('y'))) {
+  for(const auto key_value: values.filter(Symbol::ChrTest('y'))) {
     if(i == 0) {
       LONGS_EQUAL(Symbol('y', 1), (long)key_value.key);
       EXPECT(assert_equal(pose1, key_value.value.cast<Pose3>()));

gtsam/sfm/BinaryMeasurement.h

@@ -48,7 +48,8 @@ private:
  public:
   BinaryMeasurement(Key key1, Key key2, const T &measured,
                     const SharedNoiseModel &model = nullptr)
-      : Factor(std::vector<Key>({key1, key2})), measured_(measured),
+      : Factor(std::vector<Key>({key1, key2})),
+        measured_(measured),
         noiseModel_(model) {}
 
   /// @name Standard Interface

gtsam/sfm/MFAS.cpp

@@ -121,8 +121,8 @@ MFAS::MFAS(const TranslationEdges& relativeTranslations,
   }
 }
 
-vector<Key> MFAS::computeOrdering() const {
+KeyVector MFAS::computeOrdering() const {
-  vector<Key> ordering;  // Nodes in MFAS order (result).
+  KeyVector ordering;  // Nodes in MFAS order (result).
 
   // A graph is an unordered map from keys to nodes. Each node contains a list
   // of its adjacent nodes. Create the graph from the edgeWeights.
@@ -140,7 +140,7 @@ vector<Key> MFAS::computeOrdering() const {
 
 map<MFAS::KeyPair, double> MFAS::computeOutlierWeights() const {
   // Find the ordering.
-  vector<Key> ordering = computeOrdering();
+  KeyVector ordering = computeOrdering();
 
   // Create a map from the node key to its position in the ordering. This makes
   // it easier to lookup positions of different nodes.