diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 4819eda65..314d4fc63 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -85,8 +85,8 @@ size_t GaussianMixture::nrComponents() const { /* *******************************************************************************/ GaussianConditional::shared_ptr GaussianMixture::operator()( - const DiscreteValues &discreteVals) const { - auto &ptr = conditionals_(discreteVals); + const DiscreteValues &discreteValues) const { + auto &ptr = conditionals_(discreteValues); if (!ptr) return nullptr; auto conditional = boost::dynamic_pointer_cast(ptr); if (conditional) @@ -209,12 +209,12 @@ void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) { /* *******************************************************************************/ AlgebraicDecisionTree GaussianMixture::error( - const VectorValues &continuousVals) const { + const VectorValues &continuousValues) const { // functor to convert from GaussianConditional to double error value. auto errorFunc = - [continuousVals](const GaussianConditional::shared_ptr &conditional) { + [continuousValues](const GaussianConditional::shared_ptr &conditional) { if (conditional) { - return conditional->error(continuousVals); + return conditional->error(continuousValues); } else { // return arbitrarily large error return 1e50; @@ -225,10 +225,10 @@ AlgebraicDecisionTree GaussianMixture::error( } /* *******************************************************************************/ -double GaussianMixture::error(const VectorValues &continuousVals, +double GaussianMixture::error(const VectorValues &continuousValues, const DiscreteValues &discreteValues) const { auto conditional = conditionals_(discreteValues); - return conditional->error(continuousVals); + return conditional->error(continuousValues); } } // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 4bb13d298..88d5a02c0 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -122,7 +122,7 @@ class GTSAM_EXPORT GaussianMixture /// @{ GaussianConditional::shared_ptr operator()( - const DiscreteValues &discreteVals) const; + const DiscreteValues &discreteValues) const; /// Returns the total number of continuous components size_t nrComponents() const; @@ -147,21 +147,21 @@ class GTSAM_EXPORT GaussianMixture /** * @brief Compute error of the GaussianMixture as a tree. * - * @param continuousVals The continuous VectorValues. - * @return AlgebraicDecisionTree A decision tree with corresponding keys - * as the factor but leaf values as the error. + * @param continuousValues The continuous VectorValues. + * @return AlgebraicDecisionTree A decision tree with the same keys + * as the conditionals, and leaf values as the error. */ - AlgebraicDecisionTree error(const VectorValues &continuousVals) const; + AlgebraicDecisionTree error(const VectorValues &continuousValues) const; /** * @brief Compute the error of this Gaussian Mixture given the continuous * values and a discrete assignment. * - * @param continuousVals The continuous values at which to compute the error. + * @param continuousValues Continuous values at which to compute the error. * @param discreteValues The discrete assignment for a specific mode sequence. * @return double */ - double error(const VectorValues &continuousVals, + double error(const VectorValues &continuousValues, const DiscreteValues &discreteValues) const; /** diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 16802516e..f070fe07a 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -98,21 +98,22 @@ GaussianMixtureFactor::Sum GaussianMixtureFactor::asGaussianFactorGraphTree() /* *******************************************************************************/ AlgebraicDecisionTree GaussianMixtureFactor::error( - const VectorValues &continuousVals) const { + const VectorValues &continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = [continuousVals](const GaussianFactor::shared_ptr &factor) { - return factor->error(continuousVals); - }; + auto errorFunc = + [continuousValues](const GaussianFactor::shared_ptr &factor) { + return factor->error(continuousValues); + }; DecisionTree errorTree(factors_, errorFunc); return errorTree; } /* *******************************************************************************/ double GaussianMixtureFactor::error( - const VectorValues &continuousVals, + const VectorValues &continuousValues, const DiscreteValues &discreteValues) const { auto factor = factors_(discreteValues); - return factor->error(continuousVals); + return factor->error(continuousValues); } } // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 2808ec49f..0b65b5aa9 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -133,21 +133,21 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { /** * @brief Compute error of the GaussianMixtureFactor as a tree. * - * @param continuousVals The continuous VectorValues. - * @return AlgebraicDecisionTree A decision tree with corresponding keys - * as the factor but leaf values as the error. + * @param continuousValues The continuous VectorValues. + * @return AlgebraicDecisionTree A decision tree with the same keys + * as the factors involved, and leaf values as the error. */ - AlgebraicDecisionTree error(const VectorValues &continuousVals) const; + AlgebraicDecisionTree error(const VectorValues &continuousValues) const; /** * @brief Compute the error of this Gaussian Mixture given the continuous * values and a discrete assignment. * - * @param continuousVals The continuous values at which to compute the error. + * @param continuousValues Continuous values at which to compute the error. * @param discreteValues The discrete assignment for a specific mode sequence. * @return double */ - double error(const VectorValues &continuousVals, + double error(const VectorValues &continuousValues, const DiscreteValues &discreteValues) const; /// Add MixtureFactor to a Sum, syntactic sugar. diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index 511705cf3..58a915d57 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -128,14 +128,15 @@ class MixtureFactor : public HybridFactor { /** * @brief Compute error of the MixtureFactor as a tree. * - * @param continuousVals The continuous values for which to compute the error. - * @return AlgebraicDecisionTree A decision tree with corresponding keys - * as the factor but leaf values as the error. + * @param continuousValues The continuous values for which to compute the + * error. + * @return AlgebraicDecisionTree A decision tree with the same keys + * as the factor, and leaf values as the error. */ - AlgebraicDecisionTree error(const Values& continuousVals) const { + AlgebraicDecisionTree error(const Values& continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = [continuousVals](const sharedFactor& factor) { - return factor->error(continuousVals); + auto errorFunc = [continuousValues](const sharedFactor& factor) { + return factor->error(continuousValues); }; DecisionTree errorTree(factors_, errorFunc); return errorTree; @@ -144,19 +145,19 @@ class MixtureFactor : public HybridFactor { /** * @brief Compute error of factor given both continuous and discrete values. * - * @param continuousVals The continuous Values. - * @param discreteVals The discrete Values. + * @param continuousValues The continuous Values. + * @param discreteValues The discrete Values. * @return double The error of this factor. */ - double error(const Values& continuousVals, - const DiscreteValues& discreteVals) const { - // Retrieve the factor corresponding to the assignment in discreteVals. - auto factor = factors_(discreteVals); + double error(const Values& continuousValues, + const DiscreteValues& discreteValues) const { + // Retrieve the factor corresponding to the assignment in discreteValues. + auto factor = factors_(discreteValues); // Compute the error for the selected factor - const double factorError = factor->error(continuousVals); + const double factorError = factor->error(continuousValues); if (normalized_) return factorError; - return factorError + - this->nonlinearFactorLogNormalizingConstant(factor, continuousVals); + return factorError + this->nonlinearFactorLogNormalizingConstant( + factor, continuousValues); } size_t dim() const { @@ -212,17 +213,18 @@ class MixtureFactor : public HybridFactor { /// Linearize specific nonlinear factors based on the assignment in /// discreteValues. GaussianFactor::shared_ptr linearize( - const Values& continuousVals, const DiscreteValues& discreteVals) const { - auto factor = factors_(discreteVals); - return factor->linearize(continuousVals); + const Values& continuousValues, + const DiscreteValues& discreteValues) const { + auto factor = factors_(discreteValues); + return factor->linearize(continuousValues); } /// Linearize all the continuous factors to get a GaussianMixtureFactor. boost::shared_ptr linearize( - const Values& continuousVals) const { + const Values& continuousValues) const { // functional to linearize each factor in the decision tree - auto linearizeDT = [continuousVals](const sharedFactor& factor) { - return factor->linearize(continuousVals); + auto linearizeDT = [continuousValues](const sharedFactor& factor) { + return factor->linearize(continuousValues); }; DecisionTree linearized_factors( diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 86029a48a..90c76593e 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -204,14 +204,14 @@ class MixtureFactor : gtsam::HybridFactor { const std::vector& factors, bool normalized = false); - double error(const gtsam::Values& continuousVals, - const gtsam::DiscreteValues& discreteVals) const; + double error(const gtsam::Values& continuousValues, + const gtsam::DiscreteValues& discreteValues) const; double nonlinearFactorLogNormalizingConstant(const gtsam::NonlinearFactor* factor, const gtsam::Values& values) const; GaussianMixtureFactor* linearize( - const gtsam::Values& continuousVals) const; + const gtsam::Values& continuousValues) const; void print(string s = "MixtureFactor\n", const gtsam::KeyFormatter& keyFormatter = diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 5c25a0931..14e1b8dad 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -170,12 +170,12 @@ TEST(GaussianMixtureFactor, Error) { GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); - VectorValues continuousVals; - continuousVals.insert(X(1), Vector2(0, 0)); - continuousVals.insert(X(2), Vector2(1, 1)); + VectorValues continuousValues; + continuousValues.insert(X(1), Vector2(0, 0)); + continuousValues.insert(X(2), Vector2(1, 1)); // error should return a tree of errors, with nodes for each discrete value. - AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousVals); + AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousValues); std::vector discrete_keys = {m1}; std::vector errors = {1, 4}; @@ -184,10 +184,10 @@ TEST(GaussianMixtureFactor, Error) { EXPECT(assert_equal(expected_error, error_tree)); // Test for single leaf given discrete assignment P(X|M,Z). - DiscreteValues discreteVals; - discreteVals[m1.first] = 1; - EXPECT_DOUBLES_EQUAL(4.0, mixtureFactor.error(continuousVals, discreteVals), - 1e-9); + DiscreteValues discreteValues; + discreteValues[m1.first] = 1; + EXPECT_DOUBLES_EQUAL( + 4.0, mixtureFactor.error(continuousValues, discreteValues), 1e-9); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index c00d70e5a..5167f6ff6 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -87,11 +87,11 @@ TEST(MixtureFactor, Error) { MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); - Values continuousVals; - continuousVals.insert(X(1), 0); - continuousVals.insert(X(2), 1); + Values continuousValues; + continuousValues.insert(X(1), 0); + continuousValues.insert(X(2), 1); - AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousVals); + AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousValues); std::vector discrete_keys = {m1}; std::vector errors = {0.5, 0};