diff --git a/CMakeLists.txt b/CMakeLists.txt index 9dce69903..aa6082cb3 100644 --- a/CMakeLists.txt +++ b/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)") ############################################################################### diff --git a/INSTALL.md b/INSTALL.md index b8f73f153..cf66766a1 100644 --- a/INSTALL.md +++ b/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 diff --git a/README.md b/README.md index 3982f5585..cc8af7248 100644 --- a/README.md +++ b/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. diff --git a/gtsam/base/DSFMap.h b/gtsam/base/DSFMap.h index 1d6bfdefa..3a9d21aad 100644 --- a/gtsam/base/DSFMap.h +++ b/gtsam/base/DSFMap.h @@ -21,6 +21,7 @@ #include // Provides size_t #include #include +#include namespace gtsam { @@ -120,4 +121,12 @@ class IndexPair : public std::pair { inline size_t i() const { return first; }; inline size_t j() const { return second; }; }; + +typedef std::vector IndexPairVector; +typedef std::set IndexPairSet; + +inline IndexPairVector IndexPairSetAsArray(IndexPairSet& set) { return IndexPairVector(set.begin(), set.end()); } + +typedef std::map IndexPairSetMap; +typedef DSFMap DSFMapIndexPair; } // namespace gtsam diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 831bb6bcc..34ff0327e 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -700,9 +700,10 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative65(Y (* * @return m*n Jacobian computed via central differencing */ template -typename internal::FixedSizeMatrix::type numericalDerivative66( - boost::function h, const X1& x1, - const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { +typename internal::FixedSizeMatrix::type numericalDerivative66( + boost::function h, + 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::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index b7b649a05..52f5901ee 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -259,11 +259,59 @@ class IndexPair { size_t j() const; }; -template -class DSFMap { - DSFMap(); - KEY find(const KEY& key) const; - void merge(const KEY& x, const KEY& y); +// template +// class DSFMap { +// DSFMap(); +// KEY find(const KEY& key) const; +// void merge(const KEY& x, const KEY& y); +// std::map 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 @@ -2929,6 +2977,13 @@ class BinaryMeasurement { typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; +class BinaryMeasurementsUnit3 { + BinaryMeasurementsUnit3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + #include // TODO(frank): copy/pasta below until we have integer template paremeters in wrap! @@ -3050,6 +3105,16 @@ class ShonanAveraging3 { pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; }; +#include +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 //************************************************************************* diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 7d4797132..0181ea45f 100644 --- a/gtsam/navigation/NavState.cpp +++ b/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; - Matrix3 D_dP_R; - dR(xi) << nRb.unrotate((-dt) * omega, H ? &D_dP_R : 0); - dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper - dV(xi) << ((-2.0 * dt) * omega_cross_vel); + // Get perturbations in nav frame + Vector9 n_xi, xi; + Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav; + dR(n_xi) << ((-dt) * omega); + 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; - dV(xi) -= dt * omega_cross2_t; + dP(n_xi) -= (0.5 * dt2) * 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_t_v(H) << (-dt2) * D_cross_state; - D_v_v(H) << (-2.0 * dt) * D_cross_state; + D_R_R(H) << D_dR_R; + D_t_v(H) << D_body_nav * (-dt2) * 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_v_t(H) -= dt * D_cross2_state; + D_t_t(H) -= D_body_nav * (0.5 * dt2) * D_cross2_state; + D_v_t(H) -= D_body_nav * dt * D_cross2_state; } } return xi; diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 48dd3bc3e..5bafe4392 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/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; diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4506bcdb2..270dbeb95 100644 --- a/gtsam/slam/dataset.cpp +++ b/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; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 0bcdd7e8e..91ceaa5fd 100644 --- a/gtsam/slam/dataset.h +++ b/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>; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 inline boost::optional parseVertex(std::istream &is, const std::string &tag) { diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index e15e571f5..bec02fb64 100644 --- a/python/CMakeLists.txt +++ b/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 diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 9f3032dd0..6166f615e 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -9,3 +9,4 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); +PYBIND11_MAKE_OPAQUE(std::vector); diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 3b60e42cb..3f6b8fa38 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -9,3 +9,6 @@ py::bind_vector >(m_, "Pose3Vector"); py::bind_vector > > >(m_, "BetweenFactorPose3s"); py::bind_vector > > >(m_, "BetweenFactorPose2s"); +py::bind_vector > >(m_, "BinaryMeasurementsUnit3"); +py::bind_map(m_, "IndexPairSetMap"); +py::bind_vector(m_, "IndexPairVector"); diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py new file mode 100644 index 000000000..ec6d6a83f --- /dev/null +++ b/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() + diff --git a/python/gtsam/tests/test_dsf_map.py b/python/gtsam/tests/test_dsf_map.py index 73ddcb050..e36657178 100644 --- a/python/gtsam/tests/test_dsf_map.py +++ b/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()