From e331a47b6f3417256244ed47219463cdd868cd12 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 17 Mar 2023 15:49:03 -0400 Subject: [PATCH 01/12] fix doc typo --- gtsam/navigation/ManifoldPreintegration.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index a8c97477b..40691c445 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -27,7 +27,7 @@ namespace gtsam { /** - * IMU pre-integration on NavSatet manifold. + * IMU pre-integration on NavState manifold. * This corresponds to the original RSS paper (with one difference: V is rotated) */ class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase { From 1db3cdc780fd995d2d44d5df607f77624efdac85 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 17 Mar 2023 15:50:03 -0400 Subject: [PATCH 02/12] add curly braces to make code more readable --- gtsam/navigation/ManifoldPreintegration.cpp | 8 +++++--- gtsam/navigation/TangentPreintegration.cpp | 8 +++++--- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/ManifoldPreintegration.cpp b/gtsam/navigation/ManifoldPreintegration.cpp index c0c917d9c..278c44b90 100644 --- a/gtsam/navigation/ManifoldPreintegration.cpp +++ b/gtsam/navigation/ManifoldPreintegration.cpp @@ -67,9 +67,11 @@ void ManifoldPreintegration::update(const Vector3& measuredAcc, // Possibly correct for sensor pose Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; - if (p().body_P_sensor) - std::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, - D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); + if (p().body_P_sensor) { + std::tie(acc, omega) = correctMeasurementsBySensorPose( + acc, omega, D_correctedAcc_acc, D_correctedAcc_omega, + D_correctedOmega_omega); + } // Save current rotation for updating Jacobians const Rot3 oldRij = deltaXij_.attitude(); diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp index a472b2cfd..52f730cbb 100644 --- a/gtsam/navigation/TangentPreintegration.cpp +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -111,9 +111,11 @@ void TangentPreintegration::update(const Vector3& measuredAcc, // Possibly correct for sensor pose by converting to body frame Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; - if (p().body_P_sensor) - std::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, - D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); + if (p().body_P_sensor) { + std::tie(acc, omega) = correctMeasurementsBySensorPose( + acc, omega, D_correctedAcc_acc, D_correctedAcc_omega, + D_correctedOmega_omega); + } // Do update deltaTij_ += dt; From 29b245d1dcbd55859fd26f7c39283af6e143095d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 17 Mar 2023 15:50:40 -0400 Subject: [PATCH 03/12] avoid multiple std::string() calls in toc function --- gtsam/base/timing.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 5567ce35d..154a564db 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -272,20 +272,21 @@ void tic(size_t id, const char *labelC) { } /* ************************************************************************* */ -void toc(size_t id, const char *label) { +void toc(size_t id, const char *labelC) { // disable anything which refers to TimingOutline as well, for good measure #ifdef GTSAM_USE_BOOST_FEATURES + const std::string label(labelC); std::shared_ptr current(gCurrentTimer.lock()); if (id != current->id_) { gTimingRoot->print(); throw std::invalid_argument( - "gtsam timing: Mismatched tic/toc: gttoc(\"" + std::string(label) + + "gtsam timing: Mismatched tic/toc: gttoc(\"" + label + "\") called when last tic was \"" + current->label_ + "\"."); } if (!current->parent_.lock()) { gTimingRoot->print(); throw std::invalid_argument( - "gtsam timing: Mismatched tic/toc: extra gttoc(\"" + std::string(label) + + "gtsam timing: Mismatched tic/toc: extra gttoc(\"" + label + "\"), already at the root"); } current->toc(); From 488dd7838f10810eeb7eb81a379581974adecefa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 17 Mar 2023 17:58:31 -0400 Subject: [PATCH 04/12] update HybridSmoother to be more like HybridISAM, compute ordering if not given --- gtsam/hybrid/HybridSmoother.cpp | 12 +++++-- gtsam/hybrid/HybridSmoother.h | 11 +++--- gtsam/hybrid/tests/testHybridEstimation.cpp | 37 ++------------------- 3 files changed, 19 insertions(+), 41 deletions(-) diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index de26bad7e..56c62cf19 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -57,8 +57,16 @@ Ordering HybridSmoother::getOrdering( /* ************************************************************************* */ void HybridSmoother::update(HybridGaussianFactorGraph graph, - const Ordering &ordering, - std::optional maxNrLeaves) { + std::optional maxNrLeaves, + const std::optional given_ordering) { + Ordering ordering; + // If no ordering provided, then we compute one + if (!given_ordering.has_value()) { + ordering = this->getOrdering(graph); + } else { + ordering = *given_ordering; + } + // Add the necessary conditionals from the previous timestep(s). std::tie(graph, hybridBayesNet_) = addConditionals(graph, hybridBayesNet_, ordering); diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index 0494834cd..0767da12f 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -44,13 +44,14 @@ class HybridSmoother { * corresponding to the pruned choices. * * @param graph The new factors, should be linear only - * @param ordering The ordering for elimination, only continuous vars are - * allowed * @param maxNrLeaves The maximum number of leaves in the new discrete factor, * if applicable + * @param given_ordering The (optional) ordering for elimination, only + * continuous variables are allowed */ - void update(HybridGaussianFactorGraph graph, const Ordering& ordering, - std::optional maxNrLeaves = {}); + void update(HybridGaussianFactorGraph graph, + std::optional maxNrLeaves = {}, + const std::optional given_ordering = {}); Ordering getOrdering(const HybridGaussianFactorGraph& newFactors); @@ -74,4 +75,4 @@ class HybridSmoother { const HybridBayesNet& hybridBayesNet() const; }; -}; // namespace gtsam +} // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index e74990fe6..b5f5244fa 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -46,35 +46,6 @@ using namespace gtsam; using symbol_shorthand::X; using symbol_shorthand::Z; -Ordering getOrdering(HybridGaussianFactorGraph& factors, - const HybridGaussianFactorGraph& newFactors) { - factors.push_back(newFactors); - // Get all the discrete keys from the factors - KeySet allDiscrete = factors.discreteKeySet(); - - // Create KeyVector with continuous keys followed by discrete keys. - KeyVector newKeysDiscreteLast; - const KeySet newFactorKeys = newFactors.keys(); - // Insert continuous keys first. - for (auto& k : newFactorKeys) { - if (!allDiscrete.exists(k)) { - newKeysDiscreteLast.push_back(k); - } - } - - // Insert discrete keys at the end - std::copy(allDiscrete.begin(), allDiscrete.end(), - std::back_inserter(newKeysDiscreteLast)); - - const VariableIndex index(factors); - - // Get an ordering where the new keys are eliminated last - Ordering ordering = Ordering::ColamdConstrainedLast( - index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()), - true); - return ordering; -} - TEST(HybridEstimation, Full) { size_t K = 6; std::vector measurements = {0, 1, 2, 2, 2, 3}; @@ -117,7 +88,7 @@ TEST(HybridEstimation, Full) { /****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST(HybridEstimation, Incremental) { +TEST(HybridEstimation, IncrementalSmoother) { size_t K = 15; std::vector measurements = {0, 1, 2, 2, 2, 2, 3, 4, 5, 6, 6, 7, 8, 9, 9, 9, 10, 11, 11, 11, 11}; @@ -136,7 +107,6 @@ TEST(HybridEstimation, Incremental) { initial.insert(X(0), switching.linearizationPoint.at(X(0))); HybridGaussianFactorGraph linearized; - HybridGaussianFactorGraph bayesNet; for (size_t k = 1; k < K; k++) { // Motion Model @@ -146,11 +116,10 @@ TEST(HybridEstimation, Incremental) { initial.insert(X(k), switching.linearizationPoint.at(X(k))); - bayesNet = smoother.hybridBayesNet(); linearized = *graph.linearize(initial); - Ordering ordering = getOrdering(bayesNet, linearized); + Ordering ordering = smoother.getOrdering(linearized); - smoother.update(linearized, ordering, 3); + smoother.update(linearized, 3, ordering); graph.resize(0); } From c46bed7e520f910a34c68aeb5e51fe324ce5a054 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 17 Mar 2023 17:59:51 -0400 Subject: [PATCH 05/12] fix hybrid timing calls to allow working with outer scope --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 897d56272..f0d28e9f5 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -248,7 +248,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, #ifdef HYBRID_TIMING tictoc_print_(); - tictoc_reset_(); #endif // Separate out decision tree into conditionals and remaining factors. @@ -416,9 +415,6 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, return continuousElimination(factors, frontalKeys); } else { // Case 3: We are now in the hybrid land! -#ifdef HYBRID_TIMING - tictoc_reset_(); -#endif return hybridElimination(factors, frontalKeys, continuousSeparator, discreteSeparatorSet); } From baae3e265dbae6142643fed00edeaf314d747f59 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 17 Mar 2023 18:00:18 -0400 Subject: [PATCH 06/12] remove extra semi-colon --- gtsam/navigation/ConstantVelocityFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index f75436ae3..9fe5bef85 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -38,7 +38,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN { public: ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model) : NoiseModelFactorN(model, i, j), dt_(dt) {} - ~ConstantVelocityFactor() override{}; + ~ConstantVelocityFactor() override {} /** * @brief Caclulate error: (x2 - x1.update(dt))) From 329041d724ee39ef9cc2d516700f2a00bfa605a0 Mon Sep 17 00:00:00 2001 From: zubingtan Date: Mon, 27 Mar 2023 11:59:18 +0800 Subject: [PATCH 07/12] use auto for map for-loop 1. reserve vector size in DecisionTreeFactor::apply 2. use auto in range-base for-loop to avoid implictly conversion in VectorValues and DecisionTreeFactor. Some format issues are address, too (add spaces). --- gtsam/discrete/DecisionTreeFactor.cpp | 5 ++++- gtsam/linear/VectorValues.cpp | 21 ++++++++++----------- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index cb6c7761e..5fb5ae2e6 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -94,7 +94,10 @@ namespace gtsam { for (Key j : f.keys()) cs[j] = f.cardinality(j); // Convert map into keys DiscreteKeys keys; - for (const std::pair& key : cs) keys.push_back(key); + keys.reserve(cs.size()); + for (const auto& key : cs) { + keys.emplace_back(key); + } // apply operand ADT result = ADT::apply(f, op); // Make a new factor diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 482654471..075e3b9ec 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -41,7 +41,7 @@ namespace gtsam { /* ************************************************************************ */ VectorValues::VectorValues(const Vector& x, const Dims& dims) { size_t j = 0; - for (const auto& [key,n] : dims) { + for (const auto& [key, n] : dims) { #ifdef TBB_GREATER_EQUAL_2020 values_.emplace(key, x.segment(j, n)); #else @@ -68,7 +68,7 @@ namespace gtsam { VectorValues VectorValues::Zero(const VectorValues& other) { VectorValues result; - for(const auto& [key,value]: other) + for (const auto& [key, value] : other) #ifdef TBB_GREATER_EQUAL_2020 result.values_.emplace(key, Vector::Zero(value.size())); #else @@ -79,7 +79,7 @@ namespace gtsam { /* ************************************************************************ */ VectorValues::iterator VectorValues::insert(const std::pair& key_value) { - std::pair result = values_.insert(key_value); + const std::pair result = values_.insert(key_value); if(!result.second) throw std::invalid_argument( "Requested to insert variable '" + DefaultKeyFormatter(key_value.first) @@ -90,7 +90,7 @@ namespace gtsam { /* ************************************************************************ */ VectorValues& VectorValues::update(const VectorValues& values) { iterator hint = begin(); - for (const auto& [key,value] : values) { + for (const auto& [key, value] : values) { // Use this trick to find the value using a hint, since we are inserting // from another sorted map size_t oldSize = values_.size(); @@ -131,10 +131,10 @@ namespace gtsam { // Change print depending on whether we are using TBB #ifdef GTSAM_USE_TBB std::map sorted; - for (const auto& [key,value] : v) { + for (const auto& [key, value] : v) { sorted.emplace(key, value); } - for (const auto& [key,value] : sorted) + for (const auto& [key, value] : sorted) #else for (const auto& [key,value] : v) #endif @@ -344,14 +344,13 @@ namespace gtsam { } /* ************************************************************************ */ - VectorValues operator*(const double a, const VectorValues &v) - { + VectorValues operator*(const double a, const VectorValues& c) { VectorValues result; - for(const VectorValues::KeyValuePair& key_v: v) + for (const auto& [key, value] : c) #ifdef TBB_GREATER_EQUAL_2020 - result.values_.emplace(key_v.first, a * key_v.second); + result.values_.emplace(key, a * value); #else - result.values_.insert({key_v.first, a * key_v.second}); + result.values_.insert({key, a * value}); #endif return result; } From 908ed316987c5d1d3e1fe79a5e9c5120c571430d Mon Sep 17 00:00:00 2001 From: zubingtan Date: Sat, 8 Apr 2023 16:24:46 +0800 Subject: [PATCH 08/12] add clang-format --- .clang-format | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 000000000..d54a39d88 --- /dev/null +++ b/.clang-format @@ -0,0 +1,8 @@ +BasedOnStyle: Google + +BinPackArguments: false +BinPackParameters: false +ColumnLimit: 100 +DerivePointerAlignment: false +IncludeBlocks: Preserve +PointerAlignment: Left From bde055905203c8c40d97323c16dee62939a3e331 Mon Sep 17 00:00:00 2001 From: zubingtan Date: Mon, 10 Apr 2023 10:00:11 +0800 Subject: [PATCH 09/12] format std_optional_serialization load function --- gtsam/base/std_optional_serialization.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index ec6eec56e..5c250eab4 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -76,8 +76,7 @@ void save(Archive& ar, const std::optional& t, const unsigned int /*version*/ } template -void load(Archive& ar, std::optional& t, const unsigned int /*version*/ -) { +void load(Archive& ar, std::optional& t, const unsigned int /*version*/) { bool tflag; ar >> boost::serialization::make_nvp("initialized", tflag); if (!tflag) { From 906b144580e34e8ad7ab878070f1c47be35a10cf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Apr 2023 21:12:57 -0400 Subject: [PATCH 10/12] change from /std:c++latest to /std:c++17 for Visual Studio --- cmake/GtsamBuildTypes.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 3e8cf7192..b24be5f08 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -150,7 +150,7 @@ if (NOT CMAKE_VERSION VERSION_LESS 3.8) set(CMAKE_CXX_EXTENSIONS OFF) if (MSVC) # NOTE(jlblanco): seems to be required in addition to the cxx_std_17 above? - list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC /std:c++latest) + list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC /std:c++17) endif() else() # Old cmake versions: From 9279d2713f724933743947f834f7b1af3c427195 Mon Sep 17 00:00:00 2001 From: zubingtan Date: Thu, 13 Apr 2023 14:29:18 +0800 Subject: [PATCH 11/12] fix jacobian to line in Line3::transformTo --- gtsam/geometry/Line3.cpp | 4 ++-- gtsam/geometry/tests/testLine3.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Line3.cpp b/gtsam/geometry/Line3.cpp index 9e7b2e13e..f5cf344f5 100644 --- a/gtsam/geometry/Line3.cpp +++ b/gtsam/geometry/Line3.cpp @@ -111,8 +111,8 @@ Line3 transformTo(const Pose3 &wTc, const Line3 &wL, } if (Dline) { Dline->setIdentity(); - (*Dline)(0, 3) = -t[2]; - (*Dline)(1, 2) = t[2]; + (*Dline)(3, 0) = -t[2]; + (*Dline)(2, 1) = t[2]; } return Line3(cRl, c_ab[0], c_ab[1]); } diff --git a/gtsam/geometry/tests/testLine3.cpp b/gtsam/geometry/tests/testLine3.cpp index 09371bad4..ae2a5e05d 100644 --- a/gtsam/geometry/tests/testLine3.cpp +++ b/gtsam/geometry/tests/testLine3.cpp @@ -123,10 +123,10 @@ TEST(Line3, localCoordinatesOfRetract) { // transform from world to camera test TEST(Line3, transformToExpressionJacobians) { Rot3 r = Rot3::Expmap(Vector3(0, M_PI / 3, 0)); - Vector3 t(0, 0, 0); + Vector3 t(-2.0, 2.0, 3.0); Pose3 p(r, t); - Line3 l_c(r.inverse(), 1, 1); + Line3 l_c(r.inverse(), 3, -1); Line3 l_w(Rot3(), 1, 1); EXPECT(l_c.equals(transformTo(p, l_w))); From bb7b175868dd1ead4353244e57880171e26edad2 Mon Sep 17 00:00:00 2001 From: "Michael R. Walker II" Date: Fri, 14 Apr 2023 13:19:17 -0600 Subject: [PATCH 12/12] Windows fix for CMake copy test files For cmake version 3.22.1, existing code worked on Linux, but failed on Windows 10 (?!?). Clarifying relative paths fixed the issue and worked on both systems. --- python/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 524165972..2557da237 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -198,9 +198,9 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) "${GTSAM_UNSTABLE_MODULE_PATH}") # Hack to get python test files copied every time they are modified - file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py") + file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/" "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py") foreach(test_file ${GTSAM_UNSTABLE_PYTHON_TEST_FILES}) - configure_file(${test_file} "${GTSAM_UNSTABLE_MODULE_PATH}/tests/${test_file}" COPYONLY) + configure_file("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/${test_file}" "${GTSAM_UNSTABLE_MODULE_PATH}/${test_file}" COPYONLY) endforeach() # Add gtsam_unstable to the install target