Merge branch 'develop' into feature/rollingShutterSmartFactors

.github/scripts/boost.sh

@@ -9,10 +9,10 @@ tar -zxf ${BOOST_FOLDER}.tar.gz
 
 # Bootstrap
 cd ${BOOST_FOLDER}/
-./bootstrap.sh
+./bootstrap.sh --with-libraries=serialization,filesystem,thread,system,atomic,date_time,timer,chrono,program_options,regex
 
 # Build and install
-sudo ./b2 install
+sudo ./b2 -j$(nproc) install
 
 # Rebuild ld cache
 sudo ldconfig

.github/scripts/unix.sh

@@ -92,7 +92,11 @@ function build ()
 
   configure
 
-  make -j2
+  if [ "$(uname)" == "Linux" ]; then
+    make -j$(nproc)
+  elif [ "$(uname)" == "Darwin" ]; then
+    make -j$(sysctl -n hw.physicalcpu)
+  fi
 
   finish
 }
@@ -105,8 +109,12 @@ function test ()
 
   configure
 
-  # Actual build:
+  # Actual testing
-  make -j2 check
+  if [ "$(uname)" == "Linux" ]; then
+    make -j$(nproc)
+  elif [ "$(uname)" == "Darwin" ]; then
+    make -j$(sysctl -n hw.physicalcpu)
+  fi
 
   finish
 }

.github/workflows/build-linux.yml

@@ -47,8 +47,7 @@ jobs:
       - name: Checkout
         uses: actions/checkout@v2
 
-      - name: Install (Linux)
+      - name: Install Dependencies
-        if: runner.os == 'Linux'
         run: |
           # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository.
           if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
@@ -63,7 +62,7 @@ jobs:
           fi
           sudo apt-get -y update
 
-          sudo apt-get install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
+          sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
 
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
@@ -79,7 +78,5 @@ jobs:
         run: |
           bash .github/scripts/boost.sh
 
-      - name: Build and Test (Linux)
+      - name: Build and Test
-        if: runner.os == 'Linux'
+        run: bash .github/scripts/unix.sh -t
-        run: |
-          bash .github/scripts/unix.sh -t

.github/workflows/build-macos.yml

@@ -34,8 +34,7 @@ jobs:
       - name: Checkout
         uses: actions/checkout@v2
 
-      - name: Install (macOS)
+      - name: Install Dependencies
-        if: runner.os == 'macOS'
         run: |
           brew install cmake ninja
           brew install boost
@@ -48,7 +47,5 @@ jobs:
             echo "CC=clang" >> $GITHUB_ENV
             echo "CXX=clang++" >> $GITHUB_ENV
           fi
-      - name: Build and Test (macOS)
+      - name: Build and Test
-        if: runner.os == 'macOS'
+        run: bash .github/scripts/unix.sh -t
-        run: |
-          bash .github/scripts/unix.sh -t

.github/workflows/build-python.yml

@@ -81,7 +81,7 @@ jobs:
           fi
           sudo apt-get -y update
           
-          sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
+          sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
           
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib

.github/workflows/build-special.yml

@@ -70,7 +70,7 @@ jobs:
           fi
           sudo apt-get -y update
 
-          sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
+          sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
 
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib

.github/workflows/build-windows.yml

@@ -12,42 +12,46 @@ jobs:
       CTEST_PARALLEL_LEVEL: 2
       CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
       GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
+      BOOST_VERSION: 1.72.0
+      BOOST_EXE: boost_1_72_0-msvc-14.2
+
     strategy:
       fail-fast: false
       matrix:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          #TODO This build keeps timing out, need to understand why.
+            #TODO This build fails, need to understand why.
-          # windows-2016-cl,
+            # windows-2016-cl,
-          windows-2019-cl,
+            windows-2019-cl,
-        ]
+          ]
 
         build_type: [Debug, Release]
         build_unstable: [ON]
         include:
-
+          #TODO This build fails, need to understand why.
-          #TODO This build keeps timing out, need to understand why.
           # - name: windows-2016-cl
           #   os: windows-2016
           #   compiler: cl
+          #   platform: 32
 
           - name: windows-2019-cl
             os: windows-2019
             compiler: cl
+            platform: 64
 
     steps:
-      - name: Checkout
+      - name: Install Dependencies
-        uses: actions/checkout@v2
-      - name: Install (Windows)
-        if: runner.os == 'Windows'
         shell: powershell
         run: |
           Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh')
+          scoop install cmake --global  # So we don't get issues with CMP0074 policy
           scoop install ninja --global
+
           if ("${{ matrix.compiler }}".StartsWith("clang")) {
             scoop install llvm --global
           }
+
           if ("${{ matrix.compiler }}" -eq "gcc") {
             # Chocolatey GCC is broken on the windows-2019 image.
             # See: https://github.com/DaanDeMeyer/doctest/runs/231595515
@@ -55,27 +59,38 @@ jobs:
             scoop install gcc --global
             echo "CC=gcc" >> $GITHUB_ENV
             echo "CXX=g++" >> $GITHUB_ENV
+
           } elseif ("${{ matrix.compiler }}" -eq "clang") {
             echo "CC=clang" >> $GITHUB_ENV
             echo "CXX=clang++" >> $GITHUB_ENV
+
           } else {
-            echo "CC=${{ matrix.compiler }}" >> $GITHUB_ENV
+            echo "CC=${{ matrix.compiler }}" >> $env:GITHUB_ENV
-            echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV
+            echo "CXX=${{ matrix.compiler }}" >> $env:GITHUB_ENV
+
           }
+
           # Scoop modifies the PATH so we make the modified PATH global.
-          echo "$env:PATH" >> $GITHUB_PATH
+          echo "$env:PATH" >> $env:GITHUB_PATH
 
-      - name: Download and install Boost
+      - name: Install Boost
-        uses: MarkusJx/install-boost@v1.0.1
+        shell: powershell
-        id: install-boost
+        run: |
-        with:
+          # Snippet from: https://github.com/actions/virtual-environments/issues/2667
-          boost_version: 1.72.0
+          $BOOST_PATH = "C:\hostedtoolcache\windows\Boost\$env:BOOST_VERSION\x86_64"
-          toolset: msvc14.2
 
-      - name: Build (Windows)
+          # Use the prebuilt binary for Windows
-        if: runner.os == 'Windows'
+          $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE-${{matrix.platform}}.exe"
-        env:
+          (New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe")
-          BOOST_ROOT: ${{ steps.install-boost.outputs.BOOST_ROOT }}
+          Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=$BOOST_PATH"
+
+          # Set the BOOST_ROOT variable
+          echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV
+
+      - name: Checkout
+        uses: actions/checkout@v2
+
+      - name: Build
         run: |
           cmake -E remove_directory build
           cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib"

gtsam/geometry/PinholeCamera.h

@@ -30,7 +30,7 @@ namespace gtsam {
  * \nosubgrouping
  */
 template<typename Calibration>
-class PinholeCamera: public PinholeBaseK<Calibration> {
+class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
 
 public:
 

gtsam/geometry/PinholePose.h

@@ -31,7 +31,7 @@ namespace gtsam {
  * \nosubgrouping
  */
 template<typename CALIBRATION>
-class PinholeBaseK: public PinholeBase {
+class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
 
 private:
 

gtsam/geometry/geometry.i

@@ -982,6 +982,24 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras,
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3_S2* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3DS2* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3Bundler* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
 
 #include <gtsam/geometry/BearingRange.h>
 template <POSE, POINT, BEARING, RANGE>

gtsam/inference/VariableIndex.h

@@ -182,6 +182,16 @@ protected:
     return item->second; 
   }
 
+private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    ar & BOOST_SERIALIZATION_NVP(index_);
+    ar & BOOST_SERIALIZATION_NVP(nFactors_);
+    ar & BOOST_SERIALIZATION_NVP(nEntries_);
+  }
+
   /// @}
 };
 

gtsam/nonlinear/ISAM2.h

@@ -315,6 +315,26 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
   void removeVariables(const KeySet& unusedKeys);
 
   void updateDelta(bool forceFullSolve = false) const;
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+      ar & boost::serialization::base_object<BayesTree<ISAM2Clique> >(*this);
+      ar & BOOST_SERIALIZATION_NVP(theta_);
+      ar & BOOST_SERIALIZATION_NVP(variableIndex_);
+      ar & BOOST_SERIALIZATION_NVP(delta_);
+      ar & BOOST_SERIALIZATION_NVP(deltaNewton_);
+      ar & BOOST_SERIALIZATION_NVP(RgProd_);
+      ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_);
+      ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_);
+      ar & BOOST_SERIALIZATION_NVP(linearFactors_);
+      ar & BOOST_SERIALIZATION_NVP(doglegDelta_);
+      ar & BOOST_SERIALIZATION_NVP(fixedVariables_);
+      ar & BOOST_SERIALIZATION_NVP(update_count_);
+  }
+
 };  // ISAM2
 
 /// traits

gtsam/nonlinear/tests/testExpression.cpp

@@ -58,22 +58,23 @@ TEST(Expression, Constant) {
 /* ************************************************************************* */
 // Leaf
 TEST(Expression, Leaf) {
-  Rot3_ R(100);
+  const Key key = 100;
+  Rot3_ R(key);
   Values values;
-  values.insert(100, someR);
+  values.insert(key, someR);
 
   Rot3 actual2 = R.value(values);
   EXPECT(assert_equal(someR, actual2));
 }
 
 /* ************************************************************************* */
-// Many Leaves
+// Test the function `createUnknowns` to create many leaves at once.
 TEST(Expression, Leaves) {
   Values values;
-  Point3 somePoint(1, 2, 3);
+  const Point3 somePoint(1, 2, 3);
   values.insert(Symbol('p', 10), somePoint);
-  std::vector<Point3_> points = createUnknowns<Point3>(10, 'p', 1);
+  std::vector<Point3_> pointExpressions = createUnknowns<Point3>(10, 'p', 1);
-  EXPECT(assert_equal(somePoint, points.back().value(values)));
+  EXPECT(assert_equal(somePoint, pointExpressions.back().value(values)));
 }
 
 /* ************************************************************************* */
@@ -88,29 +89,34 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) {
 Vector f3(const Point3& p, OptionalJacobian<Eigen::Dynamic, 3> H) {
   return p;
 }
-Point3_ p(1);
+Point3_ pointExpression(1);
 set<Key> expected = list_of(1);
 }  // namespace unary
 
+// Create a unary expression that takes another expression as a single argument.
 TEST(Expression, Unary1) {
   using namespace unary;
-  Expression<Point2> e(f1, p);
+  Expression<Point2> unaryExpression(f1, pointExpression);
-  EXPECT(expected == e.keys());
+  EXPECT(expected == unaryExpression.keys());
-}
+}
-TEST(Expression, Unary2) {
+
-  using namespace unary;
+// Check that also works with a scalar return value.
-  Double_ e(f2, p);
+TEST(Expression, Unary2) {
-  EXPECT(expected == e.keys());
+  using namespace unary;
+  Double_ unaryExpression(f2, pointExpression);
+  EXPECT(expected == unaryExpression.keys());
 }
 
-/* ************************************************************************* */
 // Unary(Leaf), dynamic
 TEST(Expression, Unary3) {
   using namespace unary;
-  //  Expression<Vector> e(f3, p);
+  // TODO(yetongumich): dynamic output arguments do not work yet!
+  // Expression<Vector> unaryExpression(f3, pointExpression);
+  // EXPECT(expected == unaryExpression.keys());
 }
 
 /* ************************************************************************* */
+// Simple test class that implements the `VectorSpace` protocol.
 class Class : public Point3 {
  public:
   enum {dimension = 3};
@@ -133,16 +139,20 @@ template<> struct traits<Class> : public internal::VectorSpace<Class> {};
 // Nullary Method
 TEST(Expression, NullaryMethod) {
   // Create expression
-  Expression<Class> p(67);
+  const Key key(67);
-  Expression<double> norm_(p, &Class::norm);
+  Expression<Class> classExpression(key);
+
+  // Make expression from a class method, note how it differs from the function
+  // expressions by leading with the class expression in the constructor.
+  Expression<double> norm_(classExpression, &Class::norm);
 
   // Create Values
   Values values;
-  values.insert(67, Class(3, 4, 5));
+  values.insert(key, Class(3, 4, 5));
 
   // Check dims as map
   std::map<Key, int> map;
-  norm_.dims(map);
+  norm_.dims(map); // TODO(yetongumich): Change to google style pointer convention.
   LONGS_EQUAL(1, map.size());
 
   // Get value and Jacobians
@@ -150,9 +160,10 @@ TEST(Expression, NullaryMethod) {
   double actual = norm_.value(values, H);
 
   // Check all
-  EXPECT(actual == sqrt(50));
+  const double norm = sqrt(3*3 + 4*4 + 5*5);
+  EXPECT(actual == norm);
   Matrix expected(1, 3);
-  expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0);
+  expected << 3.0 / norm, 4.0 / norm, 5.0 / norm;
   EXPECT(assert_equal(expected, H[0]));
 }
 
@@ -170,21 +181,21 @@ Point3_ p_cam(x, &Pose3::transformTo, p);
 }
 
 /* ************************************************************************* */
-// Check that creating an expression to double compiles
+// Check that creating an expression to double compiles.
 TEST(Expression, BinaryToDouble) {
   using namespace binary;
   Double_ p_cam(doubleF, x, p);
 }
 
 /* ************************************************************************* */
-// keys
+// Check keys of an expression created from class method.
 TEST(Expression, BinaryKeys) {
   set<Key> expected = list_of(1)(2);
   EXPECT(expected == binary::p_cam.keys());
 }
 
 /* ************************************************************************* */
-// dimensions
+// Check dimensions by calling `dims` method.
 TEST(Expression, BinaryDimensions) {
   map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3);
   binary::p_cam.dims(actual);
@@ -192,7 +203,7 @@ TEST(Expression, BinaryDimensions) {
 }
 
 /* ************************************************************************* */
-// dimensions
+// Check dimensions of execution trace.
 TEST(Expression, BinaryTraceSize) {
   typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
   size_t expectedTraceSize = sizeof(Binary::Record);
@@ -247,6 +258,7 @@ TEST(Expression, TreeTraceSize) {
 }
 
 /* ************************************************************************* */
+// Test compose operation with * operator.
 TEST(Expression, compose1) {
   // Create expression
   Rot3_ R1(1), R2(2);
@@ -258,7 +270,7 @@ TEST(Expression, compose1) {
 }
 
 /* ************************************************************************* */
-// Test compose with arguments referring to the same rotation
+// Test compose with arguments referring to the same rotation.
 TEST(Expression, compose2) {
   // Create expression
   Rot3_ R1(1), R2(1);
@@ -270,7 +282,7 @@ TEST(Expression, compose2) {
 }
 
 /* ************************************************************************* */
-// Test compose with one arguments referring to constant rotation
+// Test compose with one arguments referring to constant rotation.
 TEST(Expression, compose3) {
   // Create expression
   Rot3_ R1(Rot3::identity()), R2(3);
@@ -282,7 +294,7 @@ TEST(Expression, compose3) {
 }
 
 /* ************************************************************************* */
-// Test with ternary function
+// Test with ternary function.
 Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1,
                   OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
   // return dummy derivatives (not correct, but that's ok for testing here)
@@ -306,6 +318,7 @@ TEST(Expression, ternary) {
 }
 
 /* ************************************************************************* */
+// Test scalar multiplication with * operator.
 TEST(Expression, ScalarMultiply) {
   const Key key(67);
   const Point3_ expr = 23 * Point3_(key);
@@ -336,6 +349,7 @@ TEST(Expression, ScalarMultiply) {
 }
 
 /* ************************************************************************* */
+// Test sum with + operator.
 TEST(Expression, BinarySum) {
   const Key key(67);
   const Point3_ sum_ = Point3_(key) + Point3_(Point3(1, 1, 1));
@@ -366,6 +380,7 @@ TEST(Expression, BinarySum) {
 }
 
 /* ************************************************************************* */
+// Test sum of 3 variables with + operator.
 TEST(Expression, TripleSum) {
   const Key key(67);
   const Point3_ p1_(Point3(1, 1, 1)), p2_(key);
@@ -387,6 +402,7 @@ TEST(Expression, TripleSum) {
 }
 
 /* ************************************************************************* */
+// Test sum with += operator.
 TEST(Expression, PlusEqual) {
   const Key key(67);
   const Point3_ p1_(Point3(1, 1, 1)), p2_(key);
@@ -461,11 +477,12 @@ TEST(Expression, WeightedSum) {
   EXPECT(actual_dims == expected_dims);
 
   Values values;
-  values.insert<Point3>(key1, Point3(1, 0, 0));
+  const Point3 point1(1, 0, 0), point2(0, 1, 0);
-  values.insert<Point3>(key2, Point3(0, 1, 0));
+  values.insert<Point3>(key1, point1);
+  values.insert<Point3>(key2, point2);
 
   // Check value
-  const Point3 expected = 17 * Point3(1, 0, 0) + 23 * Point3(0, 1, 0);
+  const Point3 expected = 17 * point1 + 23 * point2;
   EXPECT(assert_equal(expected, weighted_sum_.value(values)));
 
   // Check value + Jacobians

gtsam/nonlinear/tests/testSerializationNonlinear.cpp

@@ -17,6 +17,7 @@
  */
 
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Pose2.h>
@@ -31,6 +32,30 @@ using namespace std;
 using namespace gtsam;
 using namespace gtsam::serializationTestHelpers;
 
+
+/* ************************************************************************* */
+// Create GUIDs for Noisemodels
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
+
+/* ************************************************************************* */
+// Create GUIDs for factors
+BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor<gtsam::Pose3>, "gtsam::PriorFactor<gtsam::Pose3>");
+BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
+BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
+BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional");
+
+
 /* ************************************************************************* */
 // Export all classes derived from Value
 GTSAM_VALUE_EXPORT(gtsam::Cal3_S2);
@@ -82,6 +107,61 @@ TEST (Serialization, TemplatedValues) {
   EXPECT(equalsBinary(values));
 }
 
+TEST(Serialization, ISAM2) {
+
+  gtsam::ISAM2Params parameters;
+  gtsam::ISAM2 solver(parameters);
+  gtsam::NonlinearFactorGraph graph;
+  gtsam::Values initialValues;
+  initialValues.clear();
+
+  gtsam::Vector6 temp6;
+  for (int i = 0; i < 6; ++i) {
+    temp6[i] = 0.0001;
+  }
+  gtsam::noiseModel::Diagonal::shared_ptr noiseModel = gtsam::noiseModel::Diagonal::Sigmas(temp6);
+
+  gtsam::Pose3 pose0(gtsam::Rot3(), gtsam::Point3(0, 0, 0));
+  gtsam::Symbol symbol0('x', 0);
+  graph.add(gtsam::PriorFactor<gtsam::Pose3>(symbol0, pose0, noiseModel));
+  initialValues.insert(symbol0, pose0);
+
+  solver.update(graph, initialValues,
+                gtsam::FastVector<gtsam::FactorIndex>());
+
+  std::string binaryPath = "saved_solver.dat";
+  try {
+    std::ofstream outputStream(binaryPath);
+    boost::archive::binary_oarchive outputArchive(outputStream);
+    outputArchive << solver;
+  } catch(...) {
+    EXPECT(false);
+  }
+
+  gtsam::ISAM2 solverFromDisk;
+  try {
+    std::ifstream ifs(binaryPath);
+    boost::archive::binary_iarchive inputArchive(ifs);
+    inputArchive >> solverFromDisk;
+  } catch(...) {
+    EXPECT(false);
+  }
+
+  gtsam::Pose3 p1, p2;
+  try {
+    p1 = solver.calculateEstimate<gtsam::Pose3>(symbol0);
+  } catch(std::exception &e) {
+    EXPECT(false);
+  }
+
+  try {
+    p2 = solverFromDisk.calculateEstimate<gtsam::Pose3>(symbol0);
+  } catch(std::exception &e) {
+    EXPECT(false);
+  }
+  EXPECT(assert_equal(p1, p2));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/sfm/ShonanAveraging.cpp

@@ -955,7 +955,9 @@ static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
         "parseMeasurements<Rot2> can only convert Pose2 measurements "
         "with Gaussian noise models.");
   const Matrix3 M = gaussian->covariance();
-  auto model = noiseModel::Gaussian::Covariance(M.block<1, 1>(2, 2));
+  // the (2,2) entry of Pose2's covariance corresponds to Rot2's covariance
+  // because the tangent space of Pose2 is ordered as (vx, vy, w)
+  auto model = noiseModel::Isotropic::Variance(1, M(2, 2));
   return BinaryMeasurement<Rot2>(f->key1(), f->key2(), f->measured().rotation(),
                                  model);
 }
@@ -1001,7 +1003,9 @@ static BinaryMeasurement<Rot3> convert(
         "parseMeasurements<Rot3> can only convert Pose3 measurements "
         "with Gaussian noise models.");
   const Matrix6 M = gaussian->covariance();
-  auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3));
+  // the upper-left 3x3 sub-block of Pose3's covariance corresponds to Rot3's covariance
+  // because the tangent space of Pose3 is ordered as (w,T) where w and T are both Vector3's
+  auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0));
   return BinaryMeasurement<Rot3>(f->key1(), f->key2(), f->measured().rotation(),
                                  model);
 }

gtsam/slam/ProjectionFactor.h

@@ -164,10 +164,15 @@ namespace gtsam {
     }
 
     /** return the calibration object */
-    inline const boost::shared_ptr<CALIBRATION> calibration() const {
+    const boost::shared_ptr<CALIBRATION> calibration() const {
       return K_;
     }
 
+    /** return the (optional) sensor pose with respect to the vehicle frame */
+    const boost::optional<POSE>& body_P_sensor() const {
+      return body_P_sensor_;
+    }
+
     /** return verbosity */
     inline bool verboseCheirality() const { return verboseCheirality_; }
 

tests/testExpressionFactor.cpp

@@ -58,40 +58,42 @@ Point2_ p(2);
 TEST(ExpressionFactor, Leaf) {
   using namespace leaf;
 
-  // Create old-style factor to create expected value and derivatives
+  // Create old-style factor to create expected value and derivatives.
   PriorFactor<Point2> old(2, Point2(0, 0), model);
 
-  // Concise version
+  // Create the equivalent factor with expression.
   ExpressionFactor<Point2> f(model, Point2(0, 0), p);
+
+  // Check values and derivatives.
   EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
-  EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
+  EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
 }
 
 /* ************************************************************************* */
-// non-zero noise model
+// Test leaf expression with noise model of different variance.
 TEST(ExpressionFactor, Model) {
   using namespace leaf;
 
   SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
 
-  // Create old-style factor to create expected value and derivatives
+  // Create old-style factor to create expected value and derivatives.
   PriorFactor<Point2> old(2, Point2(0, 0), model);
 
-  // Concise version
+  // Create the equivalent factor with expression.
   ExpressionFactor<Point2> f(model, Point2(0, 0), p);
 
-  // Check values and derivatives
+  // Check values and derivatives.
   EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
-  EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
+  EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
   EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way
 }
 
 /* ************************************************************************* */
-// Constrained noise model
+// Test leaf expression with constrained noise model.
 TEST(ExpressionFactor, Constrained) {
   using namespace leaf;
 
@@ -105,7 +107,7 @@ TEST(ExpressionFactor, Constrained) {
   EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
-  EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
+  EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
 }
 
 /* ************************************************************************* */
@@ -129,7 +131,7 @@ TEST(ExpressionFactor, Unary) {
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf, 1e-9));
+  EXPECT(assert_equal(expected, *jf, 1e-9));
 }
 
 /* ************************************************************************* */
@@ -142,11 +144,13 @@ Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) {
   if (H) *H << I_3x3, I_3x3, I_3x3;
   return v;
 }
+
 typedef Eigen::Matrix<double,9,9> Matrix9;
 Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) {
   if (H) *H = Matrix9::Identity();
   return v;
 }
+
 TEST(ExpressionFactor, Wide) {
   // Create some values
   Values values;
@@ -207,6 +211,7 @@ TEST(ExpressionFactor, Binary) {
   EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9));
   EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9));
 }
+
 /* ************************************************************************* */
 // Unary(Binary(Leaf,Leaf))
 TEST(ExpressionFactor, Shallow) {
@@ -263,7 +268,7 @@ TEST(ExpressionFactor, Shallow) {
   EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f2.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
-  EXPECT( assert_equal(*expected, *gf2, 1e-9));
+  EXPECT(assert_equal(*expected, *gf2, 1e-9));
 }
 
 /* ************************************************************************* */
@@ -296,7 +301,7 @@ TEST(ExpressionFactor, tree) {
   EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f.dim());
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
-  EXPECT( assert_equal(*expected, *gf, 1e-9));
+  EXPECT(assert_equal(*expected, *gf, 1e-9));
 
   // Concise version
   ExpressionFactor<Point2> f2(model, measured,
@@ -304,14 +309,14 @@ TEST(ExpressionFactor, tree) {
   EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f2.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
-  EXPECT( assert_equal(*expected, *gf2, 1e-9));
+  EXPECT(assert_equal(*expected, *gf2, 1e-9));
 
   // Try ternary version
   ExpressionFactor<Point2> f3(model, measured, project3(x, p, K));
   EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f3.dim());
   boost::shared_ptr<GaussianFactor> gf3 = f3.linearize(values);
-  EXPECT( assert_equal(*expected, *gf3, 1e-9));
+  EXPECT(assert_equal(*expected, *gf3, 1e-9));
 }
 
 /* ************************************************************************* */
@@ -332,15 +337,15 @@ TEST(ExpressionFactor, Compose1) {
   // Check unwhitenedError
   std::vector<Matrix> H(2);
   Vector actual = f.unwhitenedError(values, H);
-  EXPECT( assert_equal(I_3x3, H[0],1e-9));
+  EXPECT(assert_equal(I_3x3, H[0],1e-9));
-  EXPECT( assert_equal(I_3x3, H[1],1e-9));
+  EXPECT(assert_equal(I_3x3, H[1],1e-9));
 
   // Check linearization
   JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf,1e-9));
+  EXPECT(assert_equal(expected, *jf,1e-9));
 }
 
 /* ************************************************************************* */
@@ -362,14 +367,14 @@ TEST(ExpressionFactor, compose2) {
   std::vector<Matrix> H(1);
   Vector actual = f.unwhitenedError(values, H);
   EXPECT_LONGS_EQUAL(1, H.size());
-  EXPECT( assert_equal(2*I_3x3, H[0],1e-9));
+  EXPECT(assert_equal(2*I_3x3, H[0],1e-9));
 
   // Check linearization
   JacobianFactor expected(1, 2 * I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf,1e-9));
+  EXPECT(assert_equal(expected, *jf,1e-9));
 }
 
 /* ************************************************************************* */
@@ -391,14 +396,14 @@ TEST(ExpressionFactor, compose3) {
   std::vector<Matrix> H(1);
   Vector actual = f.unwhitenedError(values, H);
   EXPECT_LONGS_EQUAL(1, H.size());
-  EXPECT( assert_equal(I_3x3, H[0],1e-9));
+  EXPECT(assert_equal(I_3x3, H[0],1e-9));
 
   // Check linearization
   JacobianFactor expected(3, I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf,1e-9));
+  EXPECT(assert_equal(expected, *jf,1e-9));
 }
 
 /* ************************************************************************* */
@@ -434,16 +439,16 @@ TEST(ExpressionFactor, composeTernary) {
   std::vector<Matrix> H(3);
   Vector actual = f.unwhitenedError(values, H);
   EXPECT_LONGS_EQUAL(3, H.size());
-  EXPECT( assert_equal(I_3x3, H[0],1e-9));
+  EXPECT(assert_equal(I_3x3, H[0],1e-9));
-  EXPECT( assert_equal(I_3x3, H[1],1e-9));
+  EXPECT(assert_equal(I_3x3, H[1],1e-9));
-  EXPECT( assert_equal(I_3x3, H[2],1e-9));
+  EXPECT(assert_equal(I_3x3, H[2],1e-9));
 
   // Check linearization
   JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf,1e-9));
+  EXPECT(assert_equal(expected, *jf,1e-9));
 }
 
 TEST(ExpressionFactor, tree_finite_differences) {
@@ -636,7 +641,7 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) {
 class TestNaryFactor
     : public gtsam::ExpressionFactorN<gtsam::Point3 /*return type*/,
                                       gtsam::Rot3, gtsam::Point3, 
-                                      gtsam::Rot3,gtsam::Point3> {
+                                      gtsam::Rot3, gtsam::Point3> {
 private:
   using This = TestNaryFactor;
   using Base =

wrap/gtwrap/interface_parser/classes.py

@@ -10,7 +10,7 @@ Parser classes and rules for parsing C++ classes.
 Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert
 """
 
-from typing import Iterable, List, Union
+from typing import Any, Iterable, List, Union
 
 from pyparsing import Literal, Optional, ZeroOrMore  # type: ignore
 
@@ -48,12 +48,12 @@ class Method:
                                       args_list, t.is_const))
 
     def __init__(self,
-                 template: str,
+                 template: Union[Template, Any],
                  name: str,
                  return_type: ReturnType,
                  args: ArgumentList,
                  is_const: str,
-                 parent: Union[str, "Class"] = ''):
+                 parent: Union["Class", Any] = ''):
         self.template = template
         self.name = name
         self.return_type = return_type
@@ -98,7 +98,7 @@ class StaticMethod:
                  name: str,
                  return_type: ReturnType,
                  args: ArgumentList,
-                 parent: Union[str, "Class"] = ''):
+                 parent: Union["Class", Any] = ''):
         self.name = name
         self.return_type = return_type
         self.args = args
@@ -129,7 +129,7 @@ class Constructor:
     def __init__(self,
                  name: str,
                  args: ArgumentList,
-                 parent: Union["Class", str] = ''):
+                 parent: Union["Class", Any] = ''):
         self.name = name
         self.args = args
 
@@ -167,7 +167,7 @@ class Operator:
                  return_type: ReturnType,
                  args: ArgumentList,
                  is_const: str,
-                 parent: Union[str, "Class"] = ''):
+                 parent: Union["Class", Any] = ''):
         self.name = name
         self.operator = operator
         self.return_type = return_type
@@ -284,7 +284,7 @@ class Class:
         properties: List[Variable],
         operators: List[Operator],
         enums: List[Enum],
-        parent: str = '',
+        parent: Any = '',
     ):
         self.template = template
         self.is_virtual = is_virtual

wrap/gtwrap/interface_parser/function.py

@@ -169,7 +169,7 @@ class GlobalFunction:
                  return_type: ReturnType,
                  args_list: ArgumentList,
                  template: Template,
-                 parent: str = ''):
+                 parent: Any = ''):
         self.name = name
         self.return_type = return_type
         self.args = args_list

wrap/gtwrap/interface_parser/type.py

@@ -12,7 +12,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae
 
 # pylint: disable=unnecessary-lambda, expression-not-assigned
 
-from typing import Iterable, List, Union
+from typing import List, Sequence, Union
 
 from pyparsing import (Forward, Optional, Or, ParseResults,  # type: ignore
                        delimitedList)
@@ -49,12 +49,12 @@ class Typename:
 
     def __init__(self,
                  t: ParseResults,
-                 instantiations: Iterable[ParseResults] = ()):
+                 instantiations: Sequence[ParseResults] = ()):
         self.name = t[-1]  # the name is the last element in this list
         self.namespaces = t[:-1]
 
         if instantiations:
-            if isinstance(instantiations, Iterable):
+            if isinstance(instantiations, Sequence):
                 self.instantiations = instantiations  # type: ignore
             else:
                 self.instantiations = instantiations.asList()

wrap/gtwrap/matlab_wrapper/mixins.py

@@ -1,5 +1,7 @@
 """Mixins for reducing the amount of boilerplate in the main wrapper class."""
 
+from typing import Any, Tuple, Union
+
 import gtwrap.interface_parser as parser
 import gtwrap.template_instantiator as instantiator
 
@@ -7,13 +9,14 @@ import gtwrap.template_instantiator as instantiator
 class CheckMixin:
     """Mixin to provide various checks."""
     # Data types that are primitive types
-    not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t']
+    not_ptr_type: Tuple = ('int', 'double', 'bool', 'char', 'unsigned char',
+                           'size_t')
     # Ignore the namespace for these datatypes
-    ignore_namespace = ['Matrix', 'Vector', 'Point2', 'Point3']
+    ignore_namespace: Tuple = ('Matrix', 'Vector', 'Point2', 'Point3')
     # Methods that should be ignored
-    ignore_methods = ['pickle']
+    ignore_methods: Tuple = ('pickle', )
     # Methods that should not be wrapped directly
-    whitelist = ['serializable', 'serialize']
+    whitelist: Tuple = ('serializable', 'serialize')
     # Datatypes that do not need to be checked in methods
     not_check_type: list = []
 
@@ -23,7 +26,7 @@ class CheckMixin:
                 return True
         return False
 
-    def is_shared_ptr(self, arg_type):
+    def is_shared_ptr(self, arg_type: parser.Type):
         """
         Determine if the `interface_parser.Type` should be treated as a
         shared pointer in the wrapper.
@@ -33,7 +36,7 @@ class CheckMixin:
             and arg_type.typename.name not in self.ignore_namespace
             and arg_type.typename.name != 'string')
 
-    def is_ptr(self, arg_type):
+    def is_ptr(self, arg_type: parser.Type):
         """
         Determine if the `interface_parser.Type` should be treated as a
         raw pointer in the wrapper.
@@ -43,7 +46,7 @@ class CheckMixin:
             and arg_type.typename.name not in self.ignore_namespace
             and arg_type.typename.name != 'string')
 
-    def is_ref(self, arg_type):
+    def is_ref(self, arg_type: parser.Type):
         """
         Determine if the `interface_parser.Type` should be treated as a
         reference in the wrapper.
@@ -55,7 +58,14 @@ class CheckMixin:
 
 class FormatMixin:
     """Mixin to provide formatting utilities."""
-    def _clean_class_name(self, instantiated_class):
+
+    ignore_namespace: tuple
+    data_type: Any
+    data_type_param: Any
+    _return_count: Any
+
+    def _clean_class_name(self,
+                          instantiated_class: instantiator.InstantiatedClass):
         """Reformatted the C++ class name to fit Matlab defined naming
         standards
         """
@@ -65,23 +75,23 @@ class FormatMixin:
         return instantiated_class.name
 
     def _format_type_name(self,
-                          type_name,
+                          type_name: parser.Typename,
-                          separator='::',
+                          separator: str = '::',
-                          include_namespace=True,
+                          include_namespace: bool = True,
-                          constructor=False,
+                          is_constructor: bool = False,
-                          method=False):
+                          is_method: bool = False):
         """
         Args:
             type_name: an interface_parser.Typename to reformat
             separator: the statement to add between namespaces and typename
             include_namespace: whether to include namespaces when reformatting
-            constructor: if the typename will be in a constructor
+            is_constructor: if the typename will be in a constructor
-            method: if the typename will be in a method
+            is_method: if the typename will be in a method
 
         Raises:
             constructor and method cannot both be true
         """
-        if constructor and method:
+        if is_constructor and is_method:
             raise ValueError(
                 'Constructor and method parameters cannot both be True')
 
@@ -93,9 +103,9 @@ class FormatMixin:
                 if name not in self.ignore_namespace and namespace != '':
                     formatted_type_name += namespace + separator
 
-        if constructor:
+        if is_constructor:
             formatted_type_name += self.data_type.get(name) or name
-        elif method:
+        elif is_method:
             formatted_type_name += self.data_type_param.get(name) or name
         else:
             formatted_type_name += name
@@ -106,8 +116,8 @@ class FormatMixin:
                 template = '{}'.format(
                     self._format_type_name(type_name.instantiations[idx],
                                            include_namespace=include_namespace,
-                                           constructor=constructor,
+                                           is_constructor=is_constructor,
-                                           method=method))
+                                           is_method=is_method))
                 templates.append(template)
 
             if len(templates) > 0:  # If there are no templates
@@ -119,15 +129,15 @@ class FormatMixin:
                     self._format_type_name(type_name.instantiations[idx],
                                            separator=separator,
                                            include_namespace=False,
-                                           constructor=constructor,
+                                           is_constructor=is_constructor,
-                                           method=method))
+                                           is_method=is_method))
 
         return formatted_type_name
 
     def _format_return_type(self,
-                            return_type,
+                            return_type: parser.function.ReturnType,
-                            include_namespace=False,
+                            include_namespace: bool = False,
-                            separator="::"):
+                            separator: str = "::"):
         """Format return_type.
 
         Args:
@@ -154,18 +164,15 @@ class FormatMixin:
 
         return return_wrap
 
-    def _format_class_name(self, instantiated_class, separator=''):
+    def _format_class_name(self,
+                           instantiated_class: instantiator.InstantiatedClass,
+                           separator: str = ''):
         """Format a template_instantiator.InstantiatedClass name."""
         if instantiated_class.parent == '':
             parent_full_ns = ['']
         else:
             parent_full_ns = instantiated_class.parent.full_namespaces()
-        # class_name = instantiated_class.parent.name
+
-        #
-        # if class_name != '':
-        #     class_name += separator
-        #
-        # class_name += instantiated_class.name
         parentname = "".join([separator + x
                               for x in parent_full_ns]) + separator
 
@@ -175,10 +182,12 @@ class FormatMixin:
 
         return class_name
 
-    def _format_static_method(self, static_method, separator=''):
+    def _format_static_method(self,
-        """Example:
+                              static_method: parser.StaticMethod,
-
+                              separator: str = ''):
-                gtsamPoint3.staticFunction
+        """
+        Example:
+                gtsam.Point3.staticFunction()
         """
         method = ''
 
@@ -188,35 +197,17 @@ class FormatMixin:
 
         return method[2 * len(separator):]
 
-    def _format_instance_method(self, instance_method, separator=''):
+    def _format_global_function(self,
+                                function: Union[parser.GlobalFunction, Any],
+                                separator: str = ''):
         """Example:
 
                 gtsamPoint3.staticFunction
         """
         method = ''
 
-        if isinstance(instance_method, instantiator.InstantiatedMethod):
+        if isinstance(function, parser.GlobalFunction):
-            method_list = [
+            method += "".join([separator + x for x in function.parent.full_namespaces()]) + \
-                separator + x
-                for x in instance_method.parent.parent.full_namespaces()
-            ]
-            method += "".join(method_list) + separator
-
-            method += instance_method.parent.name + separator
-            method += instance_method.original.name
-            method += "<" + instance_method.instantiations.to_cpp() + ">"
-
-        return method[2 * len(separator):]
-
-    def _format_global_method(self, static_method, separator=''):
-        """Example:
-
-                gtsamPoint3.staticFunction
-        """
-        method = ''
-
-        if isinstance(static_method, parser.GlobalFunction):
-            method += "".join([separator + x for x in static_method.parent.full_namespaces()]) + \
                       separator
 
         return method[2 * len(separator):]

wrap/gtwrap/matlab_wrapper/wrapper.py

@@ -11,8 +11,6 @@ import textwrap
 from functools import partial, reduce
 from typing import Dict, Iterable, List, Union
 
-from loguru import logger
-
 import gtwrap.interface_parser as parser
 import gtwrap.template_instantiator as instantiator
 from gtwrap.matlab_wrapper.mixins import CheckMixin, FormatMixin
@@ -200,7 +198,7 @@ class MatlabWrapper(CheckMixin, FormatMixin):
                 check_type = self._format_type_name(
                     arg.ctype.typename,
                     separator='.',
-                    constructor=not wrap_datatypes)
+                    is_constructor=not wrap_datatypes)
 
             var_arg_wrap += " && isa(varargin{{{num}}},'{data_type}')".format(
                 num=i, data_type=check_type)
@@ -1090,11 +1088,10 @@ class MatlabWrapper(CheckMixin, FormatMixin):
             if method.instantiations:
                 # method_name += '<{}>'.format(
                 #     self._format_type_name(method.instantiations))
-                # method_name = self._format_instance_method(method, '::')
                 method = method.to_cpp()
 
         elif isinstance(method, parser.GlobalFunction):
-            method_name = self._format_global_method(method, '::')
+            method_name = self._format_global_function(method, '::')
             method_name += method.name
 
         else:

wrap/gtwrap/template_instantiator.py

@@ -4,7 +4,7 @@
 
 import itertools
 from copy import deepcopy
-from typing import Iterable, List
+from typing import Any, Iterable, List, Sequence
 
 import gtwrap.interface_parser as parser
 
@@ -214,17 +214,17 @@ class InstantiatedMethod(parser.Method):
     }
     """
     def __init__(self,
-                 original,
+                 original: parser.Method,
                  instantiations: Iterable[parser.Typename] = ()):
         self.original = original
         self.instantiations = instantiations
-        self.template = ''
+        self.template: Any = ''
         self.is_const = original.is_const
         self.parent = original.parent
 
         # Check for typenames if templated.
         # This way, we can gracefully handle both templated and non-templated methods.
-        typenames = self.original.template.typenames if self.original.template else []
+        typenames: Sequence = self.original.template.typenames if self.original.template else []
         self.name = instantiate_name(original.name, self.instantiations)
         self.return_type = instantiate_return_type(
             original.return_type,
@@ -348,13 +348,12 @@ class InstantiatedClass(parser.Class):
         return "{virtual}Class {cpp_class} : {parent_class}\n"\
             "{ctors}\n{static_methods}\n{methods}\n{operators}".format(
                virtual="virtual " if self.is_virtual else '',
-               name=self.name,
                cpp_class=self.to_cpp(),
                parent_class=self.parent,
                ctors="\n".join([repr(ctor) for ctor in self.ctors]),
-               methods="\n".join([repr(m) for m in self.methods]),
                static_methods="\n".join([repr(m)
                                          for m in self.static_methods]),
+                methods="\n".join([repr(m) for m in self.methods]),
                operators="\n".join([repr(op) for op in self.operators])
             )
 

wrap/requirements.txt

@@ -1,3 +1,2 @@
 pyparsing
 pytest
-loguru

wrap/tests/test_matlab_wrapper.py

@@ -11,8 +11,6 @@ import os.path as osp
 import sys
 import unittest
 
-from loguru import logger
-
 sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__))))
 
 from gtwrap.matlab_wrapper import MatlabWrapper
@@ -44,10 +42,6 @@ class TestWrap(unittest.TestCase):
         # Create the `actual/matlab` directory
         os.makedirs(self.MATLAB_ACTUAL_DIR, exist_ok=True)
 
-        # set the log level to INFO by default
-        logger.remove()  # remove the default sink
-        logger.add(sys.stderr, format="{time} {level} {message}", level="INFO")
-
     def compare_and_diff(self, file):
         """
         Compute the comparison between the expected and actual file,