Merge branch 'develop' into feature/lm-iteration-hook-impl

.github/scripts/unix.sh

@@ -66,6 +66,8 @@ function configure()
       -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
       -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \
       -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
+      -DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \
+      -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \
       -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
       -DBOOST_ROOT=$BOOST_ROOT \
       -DBoost_NO_SYSTEM_PATHS=ON \

.github/workflows/build-linux.yml

@@ -63,17 +63,17 @@ jobs:
 
           sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
           
-          echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)"
+          echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV
-          echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)"
+          echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV
           
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
-            echo "::set-env name=CC::gcc-${{ matrix.version }}"
+            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
-            echo "::set-env name=CXX::g++-${{ matrix.version }}"
+            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
             sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
-            echo "::set-env name=CC::clang-${{ matrix.version }}"
+            echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
-            echo "::set-env name=CXX::clang++-${{ matrix.version }}"
+            echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
           fi
       - name: Check Boost version
         if: runner.os == 'Linux'

.github/workflows/build-macos.yml

@@ -40,12 +40,12 @@ jobs:
           brew install ProfFan/robotics/boost
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             brew install gcc@${{ matrix.version }}
-            echo "::set-env name=CC::gcc-${{ matrix.version }}"
+            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
-            echo "::set-env name=CXX::g++-${{ matrix.version }}"
+            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
             sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
-            echo "::set-env name=CC::clang"
+            echo "CC=clang" >> $GITHUB_ENV
-            echo "::set-env name=CXX::clang++"
+            echo "CXX=clang++" >> $GITHUB_ENV
           fi
       - name: Build and Test (macOS)
         if: runner.os == 'macOS'

.github/workflows/build-python.yml

@@ -19,7 +19,7 @@ jobs:
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
           ubuntu-18.04-gcc-5,
-          # ubuntu-18.04-gcc-9,  # TODO Disabled for now because of timeouts
+          ubuntu-18.04-gcc-9,
           ubuntu-18.04-clang-9,
           macOS-10.15-xcode-11.3.1,
           ubuntu-18.04-gcc-5-tbb,
@@ -33,11 +33,10 @@ jobs:
             compiler: gcc
             version: "5"
 
-          # TODO Disabled for now because of timeouts
+          - name: ubuntu-18.04-gcc-9
-          # - name: ubuntu-18.04-gcc-9
+            os: ubuntu-18.04
-          #   os: ubuntu-18.04
+            compiler: gcc
-          #   compiler: gcc
+            version: "9"
-          #   version: "9"
 
           - name: ubuntu-18.04-clang-9
             os: ubuntu-18.04
@@ -77,12 +76,12 @@ jobs:
           
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
-            echo "::set-env name=CC::gcc-${{ matrix.version }}"
+            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
-            echo "::set-env name=CXX::g++-${{ matrix.version }}"
+            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
             sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
-            echo "::set-env name=CC::clang-${{ matrix.version }}"
+            echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
-            echo "::set-env name=CXX::clang++-${{ matrix.version }}"
+            echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
           fi
       - name: Install (macOS)
         if: runner.os == 'macOS'
@@ -92,17 +91,17 @@ jobs:
           brew install ProfFan/robotics/boost
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             brew install gcc@${{ matrix.version }}
-            echo "::set-env name=CC::gcc-${{ matrix.version }}"
+            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
-            echo "::set-env name=CXX::g++-${{ matrix.version }}"
+            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
             sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
-            echo "::set-env name=CC::clang"
+            echo "CC=clang" >> $GITHUB_ENV
-            echo "::set-env name=CXX::clang++"
+            echo "CXX=clang++" >> $GITHUB_ENV
           fi
       - name: Set GTSAM_WITH_TBB Flag
         if: matrix.flag == 'tbb'
         run: |
-          echo "::set-env name=GTSAM_WITH_TBB::ON"
+          echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV
           echo "GTSAM Uses TBB"
       - name: Build (Linux)
         if: runner.os == 'Linux'

.github/workflows/build-special.yml

@@ -24,6 +24,7 @@ jobs:
             ubuntu-gcc-deprecated,
             ubuntu-gcc-quaternions,
             ubuntu-gcc-tbb,
+            ubuntu-cayleymap,
           ]
 
         build_type: [Debug, Release]
@@ -47,6 +48,12 @@ jobs:
             version: "9"
             flag: tbb
 
+          - name: ubuntu-cayleymap
+            os: ubuntu-18.04
+            compiler: gcc
+            version: "9"
+            flag: cayley
+
     steps:
       - name: Checkout
         uses: actions/checkout@master
@@ -64,17 +71,17 @@ jobs:
 
           sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
 
-          echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)"
+          echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV
-          echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)"
+          echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV
 
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
-            echo "::set-env name=CC::gcc-${{ matrix.version }}"
+            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
-            echo "::set-env name=CXX::g++-${{ matrix.version }}"
+            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
             sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
-            echo "::set-env name=CC::clang-${{ matrix.version }}"
+            echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
-            echo "::set-env name=CXX::clang++-${{ matrix.version }}"
+            echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
           fi
 
       - name: Install (macOS)
@@ -83,32 +90,39 @@ jobs:
           brew install cmake ninja boost
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             brew install gcc@${{ matrix.version }}
-            echo "::set-env name=CC::gcc-${{ matrix.version }}"
+            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
-            echo "::set-env name=CXX::g++-${{ matrix.version }}"
+            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
             sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
-            echo "::set-env name=CC::clang"
+            echo "CC=clang" >> $GITHUB_ENV
-            echo "::set-env name=CXX::clang++"
+            echo "CXX=clang++" >> $GITHUB_ENV
             fi
 
       - name: Set Allow Deprecated Flag
         if: matrix.flag == 'deprecated'
         run: |
-          echo "::set-env name=GTSAM_ALLOW_DEPRECATED_SINCE_V41::ON"
+          echo "GTSAM_ALLOW_DEPRECATED_SINCE_V41=ON" >> $GITHUB_ENV
           echo "Allow deprecated since version 4.1"
 
       - name: Set Use Quaternions Flag
         if: matrix.flag == 'quaternions'
         run: |
-          echo "::set-env name=GTSAM_USE_QUATERNIONS::ON"
+          echo "GTSAM_USE_QUATERNIONS=ON" >> $GITHUB_ENV
           echo "Use Quaternions for rotations"
 
       - name: Set GTSAM_WITH_TBB Flag
         if: matrix.flag == 'tbb'
         run: |
-          echo "::set-env name=GTSAM_WITH_TBB::ON"
+          echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV
           echo "GTSAM Uses TBB"
 
+      - name: Use Cayley Transform for Rot3
+        if: matrix.flag == 'cayley'
+        run: |
+          echo "GTSAM_POSE3_EXPMAP=OFF" >> $GITHUB_ENV
+          echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV
+          echo "GTSAM Uses Cayley map for Rot3"
+
       - name: Build & Test
         run: |
           bash .github/scripts/unix.sh -t

.github/workflows/build-windows.yml

@@ -18,16 +18,19 @@ jobs:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          windows-2016-cl,
+          #TODO This build keeps timing out, need to understand why.
+          # windows-2016-cl,
           windows-2019-cl,
         ]
 
         build_type: [Debug, Release]
         build_unstable: [ON]
         include:
-          - name: windows-2016-cl
+
-            os: windows-2016
+          #TODO This build keeps timing out, need to understand why.
-            compiler: cl
+          # - name: windows-2016-cl
+          #   os: windows-2016
+          #   compiler: cl
 
           - name: windows-2019-cl
             os: windows-2019
@@ -50,17 +53,17 @@ jobs:
             # See: https://github.com/DaanDeMeyer/doctest/runs/231595515
             # See: https://github.community/t5/GitHub-Actions/Something-is-wrong-with-the-chocolatey-installed-version-of-gcc/td-p/32413
             scoop install gcc --global
-            echo "::set-env name=CC::gcc"
+            echo "CC=gcc" >> $GITHUB_ENV
-            echo "::set-env name=CXX::g++"
+            echo "CXX=g++" >> $GITHUB_ENV
           } elseif ("${{ matrix.compiler }}" -eq "clang") {
-            echo "::set-env name=CC::clang"
+            echo "CC=clang" >> $GITHUB_ENV
-            echo "::set-env name=CXX::clang++"
+            echo "CXX=clang++" >> $GITHUB_ENV
           } else {
-            echo "::set-env name=CC::${{ matrix.compiler }}"
+            echo "CC=${{ matrix.compiler }}" >> $GITHUB_ENV
-            echo "::set-env name=CXX::${{ matrix.compiler }}"
+            echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV
           }
           # Scoop modifies the PATH so we make the modified PATH global.
-          echo "::set-env name=PATH::$env:PATH"
+          echo "$env:PATH" >> $GITHUB_PATH
       - name: Build (Windows)
         if: runner.os == 'Windows'
         run: |

cmake/HandleGeneralOptions.cmake

@@ -31,6 +31,15 @@ if(NOT MSVC AND NOT XCODE_VERSION)
     option(GTSAM_BUILD_WITH_CCACHE           "Use ccache compiler cache" ON)
 endif()
 
+# Enable GTSAM_ROT3_EXPMAP if GTSAM_POSE3_EXPMAP is enabled, and vice versa.
+if(GTSAM_POSE3_EXPMAP)
+    message(STATUS "GTSAM_POSE3_EXPMAP=ON, enabling GTSAM_ROT3_EXPMAP as well")
+    set(GTSAM_ROT3_EXPMAP 1 CACHE BOOL "" FORCE)
+elseif(GTSAM_ROT3_EXPMAP)
+    message(STATUS "GTSAM_ROT3_EXPMAP=ON, enabling GTSAM_POSE3_EXPMAP as well")
+    set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE)
+endif()
+
 # Options relating to MATLAB wrapper
 # TODO: Check for matlab mex binary before handling building of binaries
 option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab toolbox"  OFF)

cmake/HandlePython.cmake

@@ -1,4 +1,5 @@
-if(GTSAM_BUILD_PYTHON)
+# 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")
         # Get info about the Python3 interpreter
         # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3
@@ -14,7 +15,9 @@ if(GTSAM_BUILD_PYTHON)
                 "The version of Python to build the wrappers against."
                 FORCE)
     endif()
+endif()
 
+if(GTSAM_BUILD_PYTHON)
     if(GTSAM_UNSTABLE_BUILD_PYTHON)
         if (NOT GTSAM_BUILD_UNSTABLE)
             message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.")

doc/Doxyfile.in

@@ -1188,7 +1188,7 @@ USE_MATHJAX            = YES
 # MathJax, but it is strongly recommended to install a local copy of MathJax 
 # before deployment.
 
-MATHJAX_RELPATH        = http://cdn.mathjax.org/mathjax/latest
+MATHJAX_RELPATH        = https://cdn.mathjax.org/mathjax/latest
 
 # The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension 
 # names that should be enabled during MathJax rendering.

gtsam/base/TestableAssertions.h

@@ -23,6 +23,7 @@
 #include <boost/optional.hpp>
 #include <map>
 #include <iostream>
+#include <sstream>
 #include <vector>
 
 namespace gtsam {
@@ -349,4 +350,44 @@ bool assert_inequal(const V& expected, const V& actual, double tol = 1e-9) {
   return false;
 }
 
+/**
+ * Capture std out via cout stream and compare against string.
+ */
+template<class V>
+bool assert_stdout_equal(const std::string& expected, const V& actual) {
+  // Redirect output to buffer so we can compare
+  std::stringstream buffer;
+  // Save the original output stream so we can reset later
+  std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
+
+  // We test against actual std::cout for faithful reproduction
+  std::cout << actual;
+
+  // Get output string and reset stdout
+  std::string actual_ = buffer.str();
+  std::cout.rdbuf(old);
+
+  return assert_equal(expected, actual_);
+}
+
+/**
+ * Capture print function output and compare against string.
+ */
+template<class V>
+bool assert_print_equal(const std::string& expected, const V& actual) {
+  // Redirect output to buffer so we can compare
+  std::stringstream buffer;
+  // Save the original output stream so we can reset later
+  std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
+
+  // We test against actual std::cout for faithful reproduction
+  actual.print();
+
+  // Get output string and reset stdout
+  std::string actual_ = buffer.str();
+  std::cout.rdbuf(old);
+
+  return assert_equal(expected, actual_);
+}
+
 } // \namespace gtsam

gtsam/geometry/Rot3M.cpp

@@ -176,7 +176,17 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) {
   if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative");
   // Create a fixed-size matrix
   Matrix3 A = R.matrix();
-  // Mathematica closed form optimization (procrastination?) gone wild:
+
+  // Check if (A+I) is invertible. Same as checking for -1 eigenvalue.
+  if ((A + I_3x3).determinant() == 0.0) {
+    throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation");
+  }
+
+  // Mathematica closed form optimization.
+  // The following are the essential computations for the following algorithm
+  // 1. Compute the inverse of P = (A+I), using a closed-form formula since P is 3x3 
+  // 2. Compute the Cayley transform C = 2 * P^{-1} * (A-I)
+  // 3. C is skew-symmetric, so we pick out the computations corresponding only to x, y, and z.
   const double a = A(0, 0), b = A(0, 1), c = A(0, 2);
   const double d = A(1, 0), e = A(1, 1), f = A(1, 2);
   const double g = A(2, 0), h = A(2, 1), i = A(2, 2);

gtsam/geometry/tests/testCal3_S2.cpp

@@ -16,6 +16,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3_S2.h>
 
@@ -127,6 +128,16 @@ TEST(Cal3_S2, between) {
 
 }
 
+/* ************************************************************************* */
+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()
+     << ", py:" << cal.py() << "}";
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/geometry/tests/testPose3.cpp

@@ -18,6 +18,7 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/testLie.h>
 #include <gtsam/base/lieProxies.h>
+#include <gtsam/base/TestableAssertions.h>
 
 #include <boost/assign/std/vector.hpp> // for operator +=
 using namespace boost::assign;
@@ -906,9 +907,9 @@ TEST(Pose3 , ChartDerivatives) {
   Pose3 id;
   if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
     CHECK_CHART_DERIVATIVES(id,id);
-//    CHECK_CHART_DERIVATIVES(id,T2);
+    CHECK_CHART_DERIVATIVES(id,T2);
-//    CHECK_CHART_DERIVATIVES(T2,id);
+    CHECK_CHART_DERIVATIVES(T2,id);
-//    CHECK_CHART_DERIVATIVES(T2,T3);
+    CHECK_CHART_DERIVATIVES(T2,T3);
   }
 }
 
@@ -1028,32 +1029,13 @@ TEST(Pose3, Create) {
 }
 
 /* ************************************************************************* */
-TEST(Pose3, print) {
+TEST(Pose3, Print) {
-  std::stringstream redirectStream;
-  std::streambuf* ssbuf = redirectStream.rdbuf();
-  std::streambuf* oldbuf  = std::cout.rdbuf();
-  // redirect cout to redirectStream
-  std::cout.rdbuf(ssbuf);
-
   Pose3 pose(Rot3::identity(), Point3(1, 2, 3));
-  // output is captured to redirectStream
-  pose.print();
 
   // Generate the expected output
-  std::stringstream expected;
+  std::string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 1 2 3\n";
-  Point3 translation(1, 2, 3);
 
-  // Add expected rotation
+  EXPECT(assert_print_equal(expected, pose));
-  expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
-  expected << "t: 1 2 3\n";
-
-  // reset cout to the original stream
-  std::cout.rdbuf(oldbuf);
-
-  // Get substring corresponding to translation part
-  std::string actual = redirectStream.str();
-
-  CHECK_EQUAL(expected.str(), actual);
 }
 
 /* ************************************************************************* */

gtsam/gtsam.i

@@ -2767,6 +2767,8 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
 
 class SfmTrack {
   SfmTrack();
+  SfmTrack(const gtsam::Point3& pt);
+  const Point3& point3() const;
 
   double r;
   double g;
@@ -2774,20 +2776,24 @@ class SfmTrack {
   // TODO Need to close wrap#10 to allow this to work.
   // std::vector<pair<size_t, gtsam::Point2>> measurements;
 
-  Point3 point3() const;
   size_t number_measurements() const;
   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);
 };
 
 class SfmData {
+  SfmData();
   size_t number_cameras() const;
   size_t number_tracks() const;
   gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
   gtsam::SfmTrack track(size_t idx) const;
+  void add_track(const gtsam::SfmTrack& t) ;
+  void add_camera(const gtsam::SfmCamera& cam);
 };
 
 gtsam::SfmData readBal(string filename);
+bool writeBAL(string filename, gtsam::SfmData& data);
 gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
 gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
 

gtsam/nonlinear/tests/testFunctorizedFactor.cpp

@@ -19,6 +19,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/FunctorizedFactor.h>
 #include <gtsam/nonlinear/factorTesting.h>
@@ -115,16 +116,6 @@ TEST(FunctorizedFactor, Print) {
   auto factor =
       MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
 
-  // redirect output to buffer so we can compare
-  stringstream buffer;
-  streambuf *old = cout.rdbuf(buffer.rdbuf());
-
-  factor.print();
-
-  // get output string and reset stdout
-  string actual = buffer.str();
-  cout.rdbuf(old);
-
   string expected =
       "  keys = { X0 }\n"
       "  noise model: unit (9) \n"
@@ -135,7 +126,7 @@ TEST(FunctorizedFactor, Print) {
       "]\n"
       "  noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
 
-  CHECK_EQUAL(expected, actual);
+  EXPECT(assert_print_equal(expected, factor));
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/tests/testValues.cpp

@@ -595,15 +595,7 @@ TEST(Values, Demangle) {
   values.insert(key1, v);
   string expected = "Values with 1 values:\nValue v1: (Eigen::Matrix<double, 1, 3, 1, 1, 3>)\n[\n	5, 6, 7\n]\n\n";
 
-  stringstream buffer;
+  EXPECT(assert_print_equal(expected, values));
-  streambuf * old = cout.rdbuf(buffer.rdbuf());
-
-  values.print();
-
-  string actual = buffer.str();
-  cout.rdbuf(old);
-
-  EXPECT(assert_equal(expected, actual));
 }
 
 /* ************************************************************************* */

gtsam/slam/dataset.h

@@ -211,16 +211,18 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
 /// A measurement with its camera index
 typedef std::pair<size_t, Point2> SfmMeasurement;
 
-/// SfmTrack
+/// Sift index for SfmTrack
 typedef std::pair<size_t, size_t> SiftIndex;
 
 /// Define the structure for the 3D points
 struct SfmTrack {
   SfmTrack(): p(0,0,0) {}
+  SfmTrack(const gtsam::Point3& pt) : p(pt) {}
   Point3 p; ///< 3D position of the point
   float r, g, b; ///< RGB color of the 3D point
   std::vector<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
   std::vector<SiftIndex> siftIndices;
+  
   /// Total number of measurements in this track
   size_t number_measurements() const {
     return measurements.size();
@@ -233,11 +235,17 @@ struct SfmTrack {
   SiftIndex siftIndex(size_t idx) const {
     return siftIndices[idx];
   }
-  Point3 point3() const {
+  /// Get 3D point
+  const Point3& point3() const {
     return p;
   }
+  /// Add measurement (camera_idx, Point2) to track
+  void add_measurement(size_t idx, const gtsam::Point2& m) {
+    measurements.emplace_back(idx, m);
+  }
 };
 
+
 /// Define the structure for the camera poses
 typedef PinholeCamera<Cal3Bundler> SfmCamera;
 
@@ -260,6 +268,14 @@ struct SfmData {
   SfmTrack track(size_t idx) const {
     return tracks[idx];
   }
+  /// Add a track to SfmData
+  void add_track(const SfmTrack& t)  {
+    tracks.push_back(t);
+  }
+  /// Add a camera to SfmData
+  void add_camera(const SfmCamera& cam){
+    cameras.push_back(cam);
+  }
 };
 
 /**

gtsam_unstable/geometry/tests/testSimilarity3.cpp

@@ -361,8 +361,12 @@ TEST(Similarity3, AlignPose3) {
 
   vector<Pose3Pair> correspondences{bTa1, bTa2};
 
+  // Cayley transform cannot accommodate 180 degree rotations,
+  // hence we only test for Expmap
+#ifdef GTSAM_ROT3_EXPMAP
   Similarity3 actual_aSb = Similarity3::Align(correspondences);
   EXPECT(assert_equal(expected_aSb, actual_aSb));
+#endif
 }
 
 //******************************************************************************

python/gtsam/tests/test_SfmData.py

@@ -0,0 +1,79 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for testing dataset access.
+Author: Frank Dellaert (Python: Sushmita Warrier)
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+from __future__ import print_function
+
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestSfmData(GtsamTestCase):
+    """Tests for SfmData and SfmTrack modules."""
+
+    def setUp(self):
+        """Initialize SfmData and SfmTrack"""
+        self.data = gtsam.SfmData()
+        # initialize SfmTrack with 3D point
+        self.tracks = gtsam.SfmTrack()
+
+    def test_tracks(self):
+        """Test functions in SfmTrack"""
+        # measurement is of format (camera_idx, imgPoint)
+        # create arbitrary camera indices for two cameras
+        i1, i2 = 4,5
+        # create arbitrary image measurements for cameras i1 and i2
+        uv_i1 = gtsam.Point2(12.6, 82)
+        # translating point uv_i1 along X-axis
+        uv_i2 = gtsam.Point2(24.88, 82)
+        # add measurements to the track
+        self.tracks.add_measurement(i1, uv_i1)
+        self.tracks.add_measurement(i2, uv_i2)
+        # Number of measurements in the track is 2
+        self.assertEqual(self.tracks.number_measurements(), 2)
+        # camera_idx in the first measurement of the track corresponds to i1
+        cam_idx, img_measurement = self.tracks.measurement(0)
+        self.assertEqual(cam_idx, i1)
+        np.testing.assert_array_almost_equal(
+            gtsam.Point3(0.,0.,0.), 
+            self.tracks.point3()
+        )
+
+
+    def test_data(self):
+        """Test functions in SfmData"""
+        # Create new track with 3 measurements
+        i1, i2, i3 = 3,5,6
+        uv_i1 = gtsam.Point2(21.23, 45.64)
+        # translating along X-axis
+        uv_i2 = gtsam.Point2(45.7, 45.64)
+        uv_i3 = gtsam.Point2(68.35, 45.64)
+        # add measurements and arbitrary point to the track
+        measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
+        pt = gtsam.Point3(1.0, 6.0, 2.0)
+        track2 = gtsam.SfmTrack(pt)
+        track2.add_measurement(i1, uv_i1)
+        track2.add_measurement(i2, uv_i2)
+        track2.add_measurement(i3, uv_i3)
+        self.data.add_track(self.tracks)
+        self.data.add_track(track2)
+        # Number of tracks in SfmData is 2
+        self.assertEqual(self.data.number_tracks(), 2)
+        # camera idx of first measurement of second track corresponds to i1
+        cam_idx, img_measurement = self.data.track(1).measurement(0)
+        self.assertEqual(cam_idx, i1)
+
+if __name__ == '__main__':
+    unittest.main()