Merge branch 'develop' into fix/wrapper-todos

CMakeLists.txt

@@ -166,7 +166,7 @@ if(MSVC AND BUILD_SHARED_LIBS)
 endif()
 
 # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
-set(BOOST_FIND_MINIMUM_VERSION 1.43)
+set(BOOST_FIND_MINIMUM_VERSION 1.58)
 set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
 
 find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS})
@@ -174,7 +174,7 @@ find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM
 # Required components
 if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR
     NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY)
-  message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.")
+  message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.")
 endif()
 
 option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
@@ -490,7 +490,7 @@ set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION
 
 # Deb-package specific cpack
 set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev")
-set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")
+set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")
 
 
 ###############################################################################

INSTALL.md

@@ -13,7 +13,7 @@ $ make install
 ## Important Installation Notes
 
 1. GTSAM requires the following libraries to be installed on your system:
-    - BOOST version 1.43 or greater (install through Linux repositories or MacPorts)
+    - BOOST version 1.58 or greater (install through Linux repositories or MacPorts)
     - Cmake version 3.0 or higher
     - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher
 

README.md

@@ -40,7 +40,7 @@ $ make install
 
 Prerequisites:
 
-- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
+- [Boost](http://www.boost.org/users/download/) >= 1.58 (Ubuntu: `sudo apt-get install libboost-all-dev`)
 - [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`)
 - A modern compiler, i.e., at least gcc 4.7.3 on Linux.
 

gtsam/base/DSFMap.h

@@ -21,6 +21,7 @@
 #include <cstdlib>  // Provides size_t
 #include <map>
 #include <set>
+#include <vector>
 
 namespace gtsam {
 
@@ -120,4 +121,12 @@ class IndexPair : public std::pair<size_t,size_t> {
   inline size_t i() const { return first; };
   inline size_t j() const { return second; };
 };
+
+typedef std::vector<IndexPair> IndexPairVector;
+typedef std::set<IndexPair> IndexPairSet;
+
+inline IndexPairVector IndexPairSetAsArray(IndexPairSet& set) { return IndexPairVector(set.begin(), set.end()); }
+
+typedef std::map<IndexPair, IndexPairSet> IndexPairSetMap;
+typedef DSFMap<IndexPair> DSFMapIndexPair;
 }  // namespace gtsam

gtsam/base/numericalDerivative.h

@@ -700,9 +700,10 @@ inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*
  * @return m*n Jacobian computed via central differencing
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
-typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative66(
+typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
-    const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
+    const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
+    double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),

gtsam/gtsam.i

@@ -259,11 +259,59 @@ class IndexPair {
   size_t j() const;
 };
 
-template<KEY = {gtsam::IndexPair}>
+// template<KEY = {gtsam::IndexPair}>
-class DSFMap {
+// class DSFMap {
-  DSFMap();
+//   DSFMap();
-  KEY find(const KEY& key) const;
+//   KEY find(const KEY& key) const;
-  void merge(const KEY& x, const KEY& y);
+//   void merge(const KEY& x, const KEY& y);
+//   std::map<KEY, Set> sets();
+// };
+
+class IndexPairSet {
+  IndexPairSet();
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  void insert(gtsam::IndexPair key);
+  bool erase(gtsam::IndexPair key); // returns true if value was removed
+  bool count(gtsam::IndexPair key) const; // returns true if value exists
+};
+
+class IndexPairVector {
+  IndexPairVector();
+  IndexPairVector(const gtsam::IndexPairVector& other);
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  gtsam::IndexPair at(size_t i) const;
+  void push_back(gtsam::IndexPair key) const;
+};
+
+gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set);
+
+class IndexPairSetMap {
+  IndexPairSetMap();
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  gtsam::IndexPairSet at(gtsam::IndexPair& key);
+};
+
+class DSFMapIndexPair {
+  DSFMapIndexPair();
+  gtsam::IndexPair find(const gtsam::IndexPair& key) const;
+  void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y);
+  gtsam::IndexPairSetMap sets();
 };
 
 #include <gtsam/base/Matrix.h>
@@ -2929,6 +2977,13 @@ class BinaryMeasurement {
 typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
 typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
 
+class BinaryMeasurementsUnit3 {
+  BinaryMeasurementsUnit3();
+  size_t size() const;
+  gtsam::BinaryMeasurement<gtsam::Unit3> at(size_t idx) const;
+  void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
+};
+
 #include <gtsam/sfm/ShonanAveraging.h>
 
 // TODO(frank): copy/pasta below until we have integer template paremeters in wrap!
@@ -3050,6 +3105,16 @@ class ShonanAveraging3 {
   pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
 };
 
+#include <gtsam/sfm/TranslationRecovery.h>
+class TranslationRecovery {
+  TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations,
+                      const gtsam::LevenbergMarquardtParams &lmParams);
+  TranslationRecovery(
+      const gtsam::BinaryMeasurementsUnit3 & relativeTranslations);  // default LevenbergMarquardtParams
+  gtsam::Values run(const double scale) const;
+  gtsam::Values run() const;    // default scale = 1.0
+};
+
 //*************************************************************************
 // Navigation
 //*************************************************************************

gtsam/navigation/NavState.cpp

@@ -218,28 +218,37 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
   const double dt2 = dt * dt;
   const Vector3 omega_cross_vel = omega.cross(n_v);
 
-  Vector9 xi;
+  // Get perturbations in nav frame
-  Matrix3 D_dP_R;
+  Vector9 n_xi, xi;
-  dR(xi) << nRb.unrotate((-dt) * omega, H ? &D_dP_R : 0);
+  Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav;
-  dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
+  dR(n_xi) << ((-dt) * omega);
-  dV(xi) << ((-2.0 * dt) * omega_cross_vel);
+  dP(n_xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
+  dV(n_xi) << ((-2.0 * dt) * omega_cross_vel);
   if (secondOrder) {
     const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t));
-    dP(xi) -= (0.5 * dt2) * omega_cross2_t;
+    dP(n_xi) -= (0.5 * dt2) * omega_cross2_t;
-    dV(xi) -= dt * omega_cross2_t;
+    dV(n_xi) -= dt * omega_cross2_t;
   }
+
+  // Transform n_xi into the body frame
+  xi << nRb.unrotate(dR(n_xi), H ? &D_dR_R : 0, H ? &D_body_nav : 0), 
+        nRb.unrotate(dP(n_xi), H ? &D_dP_R : 0),
+        nRb.unrotate(dV(n_xi), H ? &D_dV_R : 0);
+
   if (H) {
     H->setZero();
     const Matrix3 Omega = skewSymmetric(omega);
     const Matrix3 D_cross_state = Omega * R();
     H->setZero();
-    D_R_R(H) << D_dP_R;
+    D_R_R(H) << D_dR_R;
-    D_t_v(H) << (-dt2) * D_cross_state;
+    D_t_v(H) << D_body_nav * (-dt2) * D_cross_state;
-    D_v_v(H) << (-2.0 * dt) * D_cross_state;
+    D_t_R(H) << D_dP_R;
+    D_v_v(H) << D_body_nav * (-2.0 * dt) * D_cross_state;
+    D_v_R(H) << D_dV_R;
     if (secondOrder) {
       const Matrix3 D_cross2_state = Omega * D_cross_state;
-      D_t_t(H) -= (0.5 * dt2) * D_cross2_state;
+      D_t_t(H) -= D_body_nav * (0.5 * dt2) * D_cross2_state;
-      D_v_t(H) -= dt * D_cross2_state;
+      D_v_t(H) -= D_body_nav * dt * D_cross2_state;
     }
   }
   return xi;

gtsam/navigation/tests/testNavState.cpp

@@ -192,6 +192,49 @@ TEST(NavState, Coriolis2) {
   EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH));
 }
 
+TEST(NavState, Coriolis3) {
+  /** Consider a massless planet with an attached nav frame at 
+   *  n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with 
+   *  velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously
+   *  aligned with the nav frame (i.e., nRb != I_3x3). Test that first and 
+   *  second order Coriolis corrections are as expected.
+   */
+
+  // Get true first and second order coriolis accelerations
+  double dt = 2.0, dt2 = dt * dt;
+  Vector3 n_omega(0.0, 0.0, 1.0), n_t(1.0, 0.0, 0.0), n_v(0.0, 1.0, 0.0);
+  Vector3 n_aCorr1 = -2.0 * n_omega.cross(n_v),
+          n_aCorr2 = -n_omega.cross(n_omega.cross(n_t));
+  Rot3 nRb = Rot3(-1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0),
+       bRn = nRb.inverse();
+
+  // Get expected first and second order corrections in the nav frame
+  Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, 
+          n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2),
+          n_dV1e = dt * n_aCorr1, 
+          n_dV2e = dt * (n_aCorr1 + n_aCorr2);
+
+  // Get expected first and second order corrections in the body frame
+  Vector3 dRe = -dt * (bRn * n_omega),
+          b_dP1e = bRn * n_dP1e, b_dP2e = bRn * n_dP2e,
+          b_dV1e = bRn * n_dV1e, b_dV2e = bRn * n_dV2e;
+
+  // Get actual first and scond order corrections in body frame
+  NavState kState2(nRb, n_t, n_v);
+  Vector9 dXi1a = kState2.coriolis(dt, n_omega, false),
+          dXi2a = kState2.coriolis(dt, n_omega, true);
+  Vector3 dRa = NavState::dR(dXi1a),
+          b_dP1a = NavState::dP(dXi1a), b_dV1a = NavState::dV(dXi1a),
+          b_dP2a = NavState::dP(dXi2a), b_dV2a = NavState::dV(dXi2a);
+
+  EXPECT(assert_equal(dRe, dRa));
+  EXPECT(assert_equal(b_dP1e, b_dP1a));
+  EXPECT(assert_equal(b_dV1e, b_dV1a));
+  EXPECT(assert_equal(b_dP2e, b_dP2a));
+  EXPECT(assert_equal(b_dV2e, b_dV2a));
+
+}
+
 /* ************************************************************************* */
 TEST(NavState, CorrectPIM) {
   Vector9 xi;

gtsam/slam/dataset.cpp

@@ -1130,7 +1130,7 @@ bool readBAL(const string &filename, SfmData &data) {
 }
 
 /* ************************************************************************* */
-SfmData readBAL(const string &filename) {
+SfmData readBal(const string &filename) {
   SfmData data;
   readBAL(filename, data);
   return data;

gtsam/slam/dataset.h

@@ -283,7 +283,7 @@ GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData &data);
  * @param filename The name of the BAL file.
  * @return SfM structure where the data is stored.
  */
-GTSAM_EXPORT SfmData readBAL(const std::string& filename);
+GTSAM_EXPORT SfmData readBal(const std::string& filename);
 
 /**
  * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
@@ -362,6 +362,7 @@ parse3DFactors(const std::string &filename,
                const noiseModel::Diagonal::shared_ptr &model = nullptr,
                size_t maxIndex = 0);
 
+using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
 inline boost::optional<IndexedPose> parseVertex(std::istream &is,
                                                 const std::string &tag) {

python/CMakeLists.txt

@@ -30,11 +30,14 @@ set(ignore
     gtsam::ISAM2ThresholdMapValue
     gtsam::FactorIndices
     gtsam::FactorIndexSet
+    gtsam::IndexPairSetMap
+    gtsam::IndexPairVector
     gtsam::BetweenFactorPose2s
     gtsam::BetweenFactorPose3s
     gtsam::Point2Vector
     gtsam::Pose3Vector
-    gtsam::KeyVector)
+    gtsam::KeyVector
+    gtsam::BinaryMeasurementsUnit3)
 
 pybind_wrap(gtsam_py # target
             ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header
@@ -76,7 +79,8 @@ set(ignore
         gtsam::Point2Vector
         gtsam::Pose3Vector
         gtsam::KeyVector
-        gtsam::FixedLagSmootherKeyTimestampMapValue)
+        gtsam::FixedLagSmootherKeyTimestampMapValue
+        gtsam::BinaryMeasurementsUnit3)
 pybind_wrap(gtsam_unstable_py # target
         ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header
         "gtsam_unstable.cpp" # generated_cpp

python/gtsam/preamble.h

@@ -9,3 +9,4 @@ PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
 PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
 PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);

python/gtsam/specializations.h

@@ -9,3 +9,6 @@ py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point
 py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
 py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
 py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s");
+py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(m_, "BinaryMeasurementsUnit3");
+py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
+py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");

python/gtsam/tests/test_TranslationRecovery.py

@@ -0,0 +1,58 @@
+from __future__ import print_function
+
+import numpy as np
+import unittest
+
+import gtsam
+
+""" Returns example pose values of 3 points A, B and C in the world frame """
+def ExampleValues():
+    T = []
+    T.append(gtsam.Point3(np.array([3.14, 1.59, 2.65])))
+    T.append(gtsam.Point3(np.array([-1.0590e+00, -3.6017e-02, -1.5720e+00])))
+    T.append(gtsam.Point3(np.array([8.5034e+00, 6.7499e+00, -3.6383e+00])))
+    
+    data = gtsam.Values()
+    for i in range(len(T)):
+        data.insert(i, gtsam.Pose3(gtsam.Rot3(), T[i]))
+    return data
+
+""" Returns binary measurements for the points in the given edges."""
+def SimulateMeasurements(gt_poses, graph_edges):
+    measurements = gtsam.BinaryMeasurementsUnit3()
+    for edge in graph_edges:
+        Ta = gt_poses.atPose3(edge[0]).translation()
+        Tb = gt_poses.atPose3(edge[1]).translation()
+        measurements.append(gtsam.BinaryMeasurementUnit3( \
+            edge[0], edge[1], gtsam.Unit3(Tb - Ta), \
+            gtsam.noiseModel.Isotropic.Sigma(3, 0.01)))
+    return measurements
+
+""" Tests for the translation recovery class """
+class TestTranslationRecovery(unittest.TestCase):
+    """Test selected Translation Recovery methods."""
+
+    def test_constructor(self):
+        """Construct from binary measurements."""
+        algorithm = gtsam.TranslationRecovery(gtsam.BinaryMeasurementsUnit3())
+        self.assertIsInstance(algorithm, gtsam.TranslationRecovery)
+
+    def test_run(self):
+        gt_poses = ExampleValues()
+        measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]])
+        algorithm = gtsam.TranslationRecovery(measurements)
+        scale = 2.0
+        result = algorithm.run(scale)
+
+        w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(0).translation()
+        w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(0).translation()
+        w_aTc_expected = w_aTc*scale/np.linalg.norm(w_aTb)
+        w_aTb_expected = w_aTb*scale/np.linalg.norm(w_aTb)
+
+        np.testing.assert_array_almost_equal(result.atPoint3(0), np.array([0,0,0]), 6, "Origin result is incorrect.")
+        np.testing.assert_array_almost_equal(result.atPoint3(1), w_aTb_expected, 6, "Point B result is incorrect.")
+        np.testing.assert_array_almost_equal(result.atPoint3(2), w_aTc_expected, 6, "Point C result is incorrect.")
+
+if __name__ == "__main__":
+    unittest.main()
+

python/gtsam/tests/test_dsf_map.py

@@ -35,6 +35,20 @@ class TestDSFMap(GtsamTestCase):
         dsf.merge(pair1, pair2)
         self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2)))
 
+    def test_sets(self):
+        from gtsam import IndexPair
+        dsf = gtsam.DSFMapIndexPair()
+        dsf.merge(IndexPair(0, 1), IndexPair(1,2))
+        dsf.merge(IndexPair(0, 1), IndexPair(3,4))
+        dsf.merge(IndexPair(4,5), IndexPair(6,8))
+        sets = dsf.sets()
+
+        for i in sets:
+            s = sets[i]
+            for val in gtsam.IndexPairSetAsArray(s):
+                val.i()
+                val.j()
+
 
 if __name__ == '__main__':
     unittest.main()