From e331a47b6f3417256244ed47219463cdd868cd12 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 17 Mar 2023 15:49:03 -0400 Subject: [PATCH 1/6] 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 2/6] 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 3/6] 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 4/6] 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 5/6] 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 6/6] 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)))