From abe9b281af33bc45f535b10e9424b500a3ac8eb4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Jan 2023 10:54:31 -0500 Subject: [PATCH 1/6] return DiscreteKeys as a vector instead of a set --- gtsam/hybrid/HybridFactorGraph.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 098942f106..2224c4a385 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -25,7 +25,7 @@ namespace gtsam { /* ************************************************************************* */ -std::set HybridFactorGraph::discreteKeys() const { +DiscreteKeys HybridFactorGraph::discreteKeys() const { std::set keys; for (auto& factor : factors_) { if (auto p = boost::dynamic_pointer_cast(factor)) { @@ -39,7 +39,7 @@ std::set HybridFactorGraph::discreteKeys() const { } } } - return keys; + return DiscreteKeys(keys.begin(), keys.end()); } /* ************************************************************************* */ From b010a240f35f42843463c173bb34e798e0cbebfd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Jan 2023 00:59:34 -0500 Subject: [PATCH 2/6] STL-based efficient transformation --- gtsam/hybrid/HybridFactorGraph.cpp | 7 ++++--- gtsam/hybrid/HybridFactorGraph.h | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 2224c4a385..2d46ae201a 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -45,9 +45,10 @@ DiscreteKeys HybridFactorGraph::discreteKeys() const { /* ************************************************************************* */ KeySet HybridFactorGraph::discreteKeySet() const { KeySet keys; - for (const DiscreteKey& k : discreteKeys()) { - keys.insert(k.first); - } + DiscreteKeys key_vector = discreteKeys(); + std::transform(key_vector.begin(), key_vector.end(), + std::inserter(keys, keys.begin()), + [](const DiscreteKey& k) { return k.first; }); return keys; } diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index a02d4a2129..7d30663a3c 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -65,7 +65,7 @@ class HybridFactorGraph : public FactorGraph { /// @{ /// Get all the discrete keys in the factor graph. - std::set discreteKeys() const; + DiscreteKeys discreteKeys() const; /// Get all the discrete keys in the factor graph, as a set. KeySet discreteKeySet() const; From 35d560f3fccc8e6823c94b9149a77f620d18b464 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Jan 2023 00:59:54 -0500 Subject: [PATCH 3/6] implement MixtureFactor::error --- gtsam/hybrid/MixtureFactor.h | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index 38905b8a29..220f1bdbf9 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -160,10 +161,14 @@ class MixtureFactor : public HybridFactor { factor, continuousValues); } - /// Error for HybridValues is not provided for nonlinear hybrid factor. + /** + * @brief Compute error of factor given hybrid values. + * + * @param values The continuous Values and the discrete assignment. + * @return double The error of this factor. + */ double error(const HybridValues& values) const override { - throw std::runtime_error( - "MixtureFactor::error(HybridValues) not implemented."); + return error(values.nonlinear(), values.discrete()); } /** From 86bfdf76cb124177a2ab654c0c1e55ab9fce7e67 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Jan 2023 01:53:21 -0500 Subject: [PATCH 4/6] add formatter to GaussianConditional mean printing --- gtsam/linear/GaussianConditional.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 10f4eabbbf..31b7e2505c 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -124,7 +124,7 @@ namespace gtsam { cout << formatMatrixIndented(" d = ", getb(), true) << "\n"; if (nrParents() == 0) { const auto mean = solve({}); // solve for mean. - mean.print(" mean"); + mean.print(" mean", formatter); } if (model_) model_->print(" Noise model: "); From 64743ef685125481804d953a06859b70dbad9977 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Jan 2023 17:32:25 -0500 Subject: [PATCH 5/6] switch to boost::shared_ptr --- gtsam/hybrid/HybridNonlinearFactorGraph.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index ebefb52cb6..0cc87d5043 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h @@ -74,7 +74,7 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { * @param continuousValues: Dictionary of continuous values. * @return HybridGaussianFactorGraph::shared_ptr */ - HybridGaussianFactorGraph::shared_ptr linearize( + boost::shared_ptr linearize( const Values& continuousValues) const; /// @} }; From 5e1de8c062de6402367874d4ceca44c4642ddf12 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Jan 2023 17:37:29 -0500 Subject: [PATCH 6/6] switch from DiscreteKeys back to std::set --- gtsam/hybrid/HybridFactorGraph.cpp | 8 ++++---- gtsam/hybrid/HybridFactorGraph.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 2d46ae201a..7fb2801162 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -25,7 +25,7 @@ namespace gtsam { /* ************************************************************************* */ -DiscreteKeys HybridFactorGraph::discreteKeys() const { +std::set HybridFactorGraph::discreteKeys() const { std::set keys; for (auto& factor : factors_) { if (auto p = boost::dynamic_pointer_cast(factor)) { @@ -39,14 +39,14 @@ DiscreteKeys HybridFactorGraph::discreteKeys() const { } } } - return DiscreteKeys(keys.begin(), keys.end()); + return keys; } /* ************************************************************************* */ KeySet HybridFactorGraph::discreteKeySet() const { KeySet keys; - DiscreteKeys key_vector = discreteKeys(); - std::transform(key_vector.begin(), key_vector.end(), + std::set key_set = discreteKeys(); + std::transform(key_set.begin(), key_set.end(), std::inserter(keys, keys.begin()), [](const DiscreteKey& k) { return k.first; }); return keys; diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index 7d30663a3c..a02d4a2129 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -65,7 +65,7 @@ class HybridFactorGraph : public FactorGraph { /// @{ /// Get all the discrete keys in the factor graph. - DiscreteKeys discreteKeys() const; + std::set discreteKeys() const; /// Get all the discrete keys in the factor graph, as a set. KeySet discreteKeySet() const;