From 9b481cb790d25ff9a013f9ef40f89dc7814e5ba9 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Thu, 27 Aug 2020 22:18:53 -0700 Subject: [PATCH 01/17] wrapping translation recovery doesnt build --- gtsam/gtsam.i | 11 +++++++++++ python/gtsam/specializations.h | 1 + 2 files changed, 12 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index b9ecf6f3b..a3d5c1a41 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -3033,6 +3033,17 @@ class ShonanAveraging3 { pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; }; +#include +class TranslationRecovery { + TranslationRecovery(const BinaryMeasurementsUnit3& relativeTranslations, + const LevenbergMarquardtParams& lmParams); + TranslationRecovery( + const BinaryMeasurementsUnit3& + relativeTranslations); // default LevenbergMarquardtParams + gtsam::Values run(const double scale) const; + gtsam::Values run() const; // default scale = 1.0 +}; + //************************************************************************* // Navigation //************************************************************************* diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 3b60e42cb..52fffab6b 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -9,3 +9,4 @@ py::bind_vector >(m_, "Pose3Vector"); py::bind_vector > > >(m_, "BetweenFactorPose3s"); py::bind_vector > > >(m_, "BetweenFactorPose2s"); +py::bind_vector > >(m_, "BinaryMeasurementsUnit3"); From 7ffa54f896fc81c0ab175a45dac117c54809e770 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sun, 30 Aug 2020 21:53:12 -0700 Subject: [PATCH 02/17] TAvg wrapper builds --- gtsam/gtsam.i | 14 ++++++++++---- gtsam/slam/dataset.h | 1 + python/CMakeLists.txt | 6 ++++-- 3 files changed, 15 insertions(+), 6 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index a3d5c1a41..dd98c6f99 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2914,6 +2914,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! @@ -3035,11 +3042,10 @@ class ShonanAveraging3 { #include class TranslationRecovery { - TranslationRecovery(const BinaryMeasurementsUnit3& relativeTranslations, - const LevenbergMarquardtParams& lmParams); + TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, + const gtsam::LevenbergMarquardtParams &lmParams); TranslationRecovery( - const BinaryMeasurementsUnit3& - relativeTranslations); // default LevenbergMarquardtParams + const gtsam::BinaryMeasurementsUnit3 & relativeTranslations); // default LevenbergMarquardtParams gtsam::Values run(const double scale) const; gtsam::Values run() const; // default scale = 1.0 }; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 7ceca00f4..2c1bb7a1c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -354,6 +354,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 23898f61d..fa6ec905f 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -30,7 +30,8 @@ set(ignore 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 @@ -72,7 +73,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 From 0c62a9528deeee7eedae43250e9f0328c742c240 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Tue, 1 Sep 2020 07:45:50 -0700 Subject: [PATCH 03/17] fixed issue that pos and vel are in nav frame --- gtsam/navigation/NavState.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 7d4797132..6a196cb75 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -228,18 +228,27 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, dP(xi) -= (0.5 * dt2) * omega_cross2_t; dV(xi) -= dt * omega_cross2_t; } + + // NOTE(tim): position and velocity changes dP and dV are in nav frame; need + // to put them into local frame for subsequent retract + dP(xi) = nRb.unrotate(dP(xi)); + dV(xi) = nRb.unrotate(dV(xi)); + + 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_t_v(H) << nRb.matrix().transpose() * (-dt2) * D_cross_state; + D_t_R(H) << skewSymmetric(dP(xi)); + D_v_v(H) << nRb.matrix().transpose() * (-2.0 * dt) * D_cross_state; + D_v_R(H) << skewSymmetric(dV(xi)); 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) -= nRb.matrix().transpose() * (0.5 * dt2) * D_cross2_state; + D_v_t(H) -= nRb.matrix().transpose() * dt * D_cross2_state; } } return xi; From cb6573ad1fa0484034614e40b51336857a3d2aa2 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Tue, 1 Sep 2020 13:01:52 -0700 Subject: [PATCH 04/17] added another coriolis unit test --- gtsam/navigation/tests/testNavState.cpp | 43 +++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 48dd3bc3e..95fd31c21 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 plate 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; From c3bddf18168fa5c95c7bce42d794368f703b0982 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Tue, 1 Sep 2020 13:07:51 -0700 Subject: [PATCH 05/17] added cached rotation bRn --- gtsam/navigation/NavState.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 6a196cb75..fcb85d2b7 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -215,6 +215,7 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, OptionalJacobian<9, 9> H) const { TIE(nRb, n_t, n_v, *this); + Matrix3 bRn = nRb.matrix().transpose(); const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); @@ -231,8 +232,8 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, // NOTE(tim): position and velocity changes dP and dV are in nav frame; need // to put them into local frame for subsequent retract - dP(xi) = nRb.unrotate(dP(xi)); - dV(xi) = nRb.unrotate(dV(xi)); + dP(xi) = bRn * dP(xi); + dV(xi) = bRn * dV(xi); if (H) { @@ -241,14 +242,14 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, const Matrix3 D_cross_state = Omega * R(); H->setZero(); D_R_R(H) << D_dP_R; - D_t_v(H) << nRb.matrix().transpose() * (-dt2) * D_cross_state; + D_t_v(H) << bRn * (-dt2) * D_cross_state; D_t_R(H) << skewSymmetric(dP(xi)); - D_v_v(H) << nRb.matrix().transpose() * (-2.0 * dt) * D_cross_state; + D_v_v(H) << bRn * (-2.0 * dt) * D_cross_state; D_v_R(H) << skewSymmetric(dV(xi)); if (secondOrder) { const Matrix3 D_cross2_state = Omega * D_cross_state; - D_t_t(H) -= nRb.matrix().transpose() * (0.5 * dt2) * D_cross2_state; - D_v_t(H) -= nRb.matrix().transpose() * dt * D_cross2_state; + D_t_t(H) -= bRn * (0.5 * dt2) * D_cross2_state; + D_v_t(H) -= bRn * dt * D_cross2_state; } } return xi; From 688292f114d04f8b519c6b9681b2bb690bb773d7 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Wed, 2 Sep 2020 07:59:19 -0700 Subject: [PATCH 06/17] cleaned up notation --- gtsam/navigation/NavState.cpp | 39 +++++++++++++++++------------------ 1 file changed, 19 insertions(+), 20 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index fcb85d2b7..0181ea45f 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -215,41 +215,40 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, OptionalJacobian<9, 9> H) const { TIE(nRb, n_t, n_v, *this); - Matrix3 bRn = nRb.matrix().transpose(); 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; } - // NOTE(tim): position and velocity changes dP and dV are in nav frame; need - // to put them into local frame for subsequent retract - dP(xi) = bRn * dP(xi); - dV(xi) = bRn * dV(xi); - + // 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) << bRn * (-dt2) * D_cross_state; - D_t_R(H) << skewSymmetric(dP(xi)); - D_v_v(H) << bRn * (-2.0 * dt) * D_cross_state; - D_v_R(H) << skewSymmetric(dV(xi)); + 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) -= bRn * (0.5 * dt2) * D_cross2_state; - D_v_t(H) -= bRn * 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; From 335b57ca363da3b4a1ca9c9a24c59a6d5b5fc339 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Wed, 2 Sep 2020 08:08:36 -0700 Subject: [PATCH 07/17] fixed typo --- gtsam/navigation/tests/testNavState.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 95fd31c21..5bafe4392 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -193,7 +193,7 @@ TEST(NavState, Coriolis2) { } TEST(NavState, Coriolis3) { - /** Consider a massless plate with an attached nav frame at + /** 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 From f533729d3eaf9f0a44323d655d694af2c1f9ea51 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 3 Sep 2020 00:43:44 -0400 Subject: [PATCH 08/17] follow Google Style for function naming --- gtsam/slam/dataset.cpp | 2 +- gtsam/slam/dataset.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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..2e03c92d8 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 From 268c3609a85431f200c787c5c0601dfb812dfea4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 3 Sep 2020 12:00:04 -0400 Subject: [PATCH 09/17] update minimum Boost version required --- CMakeLists.txt | 6 +++--- INSTALL.md | 2 +- README.md | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) 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. From e43f78bfe8e7f34d5bac95b023a07b25421799cc Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 6 Sep 2020 14:31:48 -0400 Subject: [PATCH 10/17] Wrap DSFMap so SFM can use them --- gtsam/base/DSFMap.h | 8 +++++ gtsam/gtsam.i | 58 +++++++++++++++++++++++++++++++--- python/CMakeLists.txt | 2 ++ python/gtsam/preamble.h | 1 + python/gtsam/specializations.h | 2 ++ 5 files changed, 66 insertions(+), 5 deletions(-) diff --git a/gtsam/base/DSFMap.h b/gtsam/base/DSFMap.h index 1d6bfdefa..6d0fbca20 100644 --- a/gtsam/base/DSFMap.h +++ b/gtsam/base/DSFMap.h @@ -120,4 +120,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/gtsam.i b/gtsam/gtsam.i index 33d8c11fe..75f11b449 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 diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index e15e571f5..07e2d51ac 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -30,6 +30,8 @@ set(ignore gtsam::ISAM2ThresholdMapValue gtsam::FactorIndices gtsam::FactorIndexSet + gtsam::IndexPairSetMap + gtsam::IndexPairVector gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s gtsam::Point2Vector 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..dbb95c9d6 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -9,3 +9,5 @@ py::bind_vector >(m_, "Pose3Vector"); py::bind_vector > > >(m_, "BetweenFactorPose3s"); py::bind_vector > > >(m_, "BetweenFactorPose2s"); +py::bind_map(m_, "IndexPairSetMap"); +py::bind_vector(m_, "IndexPairVector"); \ No newline at end of file From 788a8771c410498e85c2dbe5b3f78b7fac471ae2 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 6 Sep 2020 14:34:30 -0400 Subject: [PATCH 11/17] Add the unit test for DSF in Python --- python/gtsam/tests/test_dsf_map.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) 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() From 00fbea77ddc8697f0384058304acd000dfc0a2e4 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 6 Sep 2020 14:43:10 -0400 Subject: [PATCH 12/17] Fix include issue --- gtsam/base/DSFMap.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/base/DSFMap.h b/gtsam/base/DSFMap.h index 6d0fbca20..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 { From 0fb5c0d2283b2c1d6d9d8727a681ba3c3dc31257 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sun, 6 Sep 2020 11:56:13 -0700 Subject: [PATCH 13/17] translation recovery py test --- .../gtsam/tests/test_TranslationRecovery.py | 48 +++++++++++++++++++ 1 file changed, 48 insertions(+) create mode 100644 python/gtsam/tests/test_TranslationRecovery.py diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py new file mode 100644 index 000000000..d49ce8e11 --- /dev/null +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -0,0 +1,48 @@ +from __future__ import print_function + +import numpy as np +import unittest + +import gtsam + +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(BinaryMeasurementUnit3( \ + edge[0], edge[1], gtsam.Unit3(Tb - Ta), \ + gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) + return measurements + +# Hard-coded values from dubrovnik-3-7-pre.txt +def ExampleValues(): + T = [] + T.append(gtsam.Point3(np.array([7.3030e-01, -2.6490e-01, -1.7127e+00]))) + 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(R[i], T[i])) + return data + +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) + result = algorithm.run(2.0) + for i in range(3): + self.gtsamAssertEquals(result.atPoint3(i), 2*gt_poses.atPose3(i).translation(), 1e-6) + +if __name__ == "__main__": + unittest.main() + From 556531f8b7bf2a157782a7f5db35f9ef2a5d07ef Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 6 Sep 2020 23:54:27 +0000 Subject: [PATCH 14/17] translation recovery unit tests pass --- gtsam/gtsam.i | 2 +- .../gtsam/tests/test_TranslationRecovery.py | 42 ++++++++++++------- 2 files changed, 27 insertions(+), 17 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index dd98c6f99..c2aa39352 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2918,7 +2918,7 @@ class BinaryMeasurementsUnit3 { BinaryMeasurementsUnit3(); size_t size() const; gtsam::BinaryMeasurement at(size_t idx) const; - void push_back(const gtsam::BinaryMeasurement measurement); + void push_back(const gtsam::BinaryMeasurement& measurement); }; #include diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py index d49ce8e11..ec6d6a83f 100644 --- a/python/gtsam/tests/test_TranslationRecovery.py +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -5,28 +5,30 @@ import unittest import gtsam -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(BinaryMeasurementUnit3( \ - edge[0], edge[1], gtsam.Unit3(Tb - Ta), \ - gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) - return measurements - -# Hard-coded values from dubrovnik-3-7-pre.txt +""" Returns example pose values of 3 points A, B and C in the world frame """ def ExampleValues(): T = [] - T.append(gtsam.Point3(np.array([7.3030e-01, -2.6490e-01, -1.7127e+00]))) + 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(R[i], T[i])) + 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.""" @@ -39,9 +41,17 @@ class TestTranslationRecovery(unittest.TestCase): gt_poses = ExampleValues() measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]]) algorithm = gtsam.TranslationRecovery(measurements) - result = algorithm.run(2.0) - for i in range(3): - self.gtsamAssertEquals(result.atPoint3(i), 2*gt_poses.atPose3(i).translation(), 1e-6) + 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() From 15151a7ee9bde1855cab015948c429e5ee912547 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 8 Sep 2020 03:06:43 +0000 Subject: [PATCH 15/17] remove binarymeasurementsunit3 class in wrapper --- gtsam/gtsam.i | 7 ------- 1 file changed, 7 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index c2aa39352..f778fd742 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2914,13 +2914,6 @@ 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! From 57eb143a6f629496ddcbc4cb08ac1f2c0f85a32b Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Tue, 8 Sep 2020 22:24:44 -0700 Subject: [PATCH 16/17] adding binarymeasurementsunit3 --- gtsam/gtsam.i | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 6c61f3008..f52b6cedd 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2961,6 +2961,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! From 4f2e5ac98320abeccc2745021e8f6769fa6f3611 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Thu, 10 Sep 2020 10:34:14 -0400 Subject: [PATCH 17/17] Fix typo --- gtsam/base/numericalDerivative.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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),