diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 4e16fc689..7f604086c 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -156,9 +156,9 @@ namespace gtsam { std::vector> DecisionTreeFactor::enumerate() const { // Get all possible assignments - std::vector> pairs = discreteKeys(); + DiscreteKeys pairs = discreteKeys(); // Reverse to make cartesian product output a more natural ordering. - std::vector> rpairs(pairs.rbegin(), pairs.rend()); + DiscreteKeys rpairs(pairs.rbegin(), pairs.rend()); const auto assignments = DiscreteValues::CartesianProduct(rpairs); // Construct unordered_map with values diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index fdeea8ccd..12e88f81d 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -69,8 +69,7 @@ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const { GaussianFactorGraph result; result.push_back(conditional); if (conditional) { - return GraphAndConstant( - result, conditional->logNormalizationConstant()); + return GraphAndConstant(result, conditional->logNormalizationConstant()); } else { return GraphAndConstant(result, 0.0); } @@ -163,7 +162,13 @@ KeyVector GaussianMixture::continuousParents() const { /* ************************************************************************* */ boost::shared_ptr GaussianMixture::likelihood( const VectorValues &frontals) const { - // TODO(dellaert): check that values has all frontals + // Check that values has all frontals + for (auto &&kv : frontals) { + if (frontals.find(kv.first) == frontals.end()) { + throw std::runtime_error("GaussianMixture: frontals missing factor key."); + } + } + const DiscreteKeys discreteParentKeys = discreteKeys(); const KeyVector continuousParentKeys = continuousParents(); const GaussianMixtureFactor::Factors likelihoods( diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index dd8b867be..afdb6472a 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -26,7 +26,6 @@ namespace gtsam { /* ************************************************************************ */ -// TODO(fan): THIS IS VERY VERY DIRTY! We need to get DiscreteFactor right! HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other) : Base(boost::dynamic_pointer_cast(other) ->discreteKeys()), diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 7b32b90b7..f6b713a76 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -60,10 +60,10 @@ template class EliminateableFactorGraph; /* ************************************************************************ */ static GaussianFactorGraphTree addGaussian( - const GaussianFactorGraphTree &sum, + const GaussianFactorGraphTree &gfgTree, const GaussianFactor::shared_ptr &factor) { // If the decision tree is not initialized, then initialize it. - if (sum.empty()) { + if (gfgTree.empty()) { GaussianFactorGraph result; result.push_back(factor); return GaussianFactorGraphTree(GraphAndConstant(result, 0.0)); @@ -74,20 +74,18 @@ static GaussianFactorGraphTree addGaussian( result.push_back(factor); return GraphAndConstant(result, graph_z.constant); }; - return sum.apply(add); + return gfgTree.apply(add); } } /* ************************************************************************ */ -// TODO(dellaert): We need to document why deferredFactors need to be -// added last, which I would undo if possible. Implementation-wise, it's -// probably more efficient to first collect the discrete keys, and then loop -// over all assignments to populate a vector. +// TODO(dellaert): Implementation-wise, it's probably more efficient to first +// collect the discrete keys, and then loop over all assignments to populate a +// vector. GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { gttic(assembleGraphTree); GaussianFactorGraphTree result; - std::vector deferredFactors; for (auto &f : factors_) { // TODO(dellaert): just use a virtual method defined in HybridFactor. @@ -101,10 +99,10 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { } else if (f->isContinuous()) { if (auto gf = boost::dynamic_pointer_cast(f)) { - deferredFactors.push_back(gf->inner()); + result = addGaussian(result, gf->inner()); } if (auto cg = boost::dynamic_pointer_cast(f)) { - deferredFactors.push_back(cg->asGaussian()); + result = addGaussian(result, cg->asGaussian()); } } else if (f->isDiscrete()) { @@ -126,10 +124,6 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { } } - for (auto &f : deferredFactors) { - result = addGaussian(result, f); - } - gttoc(assembleGraphTree); return result; diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index 57e0daf8d..d6b83e30d 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -99,9 +99,11 @@ void HybridNonlinearISAM::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl; - isam_.print("HybridGaussianISAM:\n", keyFormatter); + std::cout << "HybridGaussianISAM:" << std::endl; + isam_.print("", keyFormatter); linPoint_.print("Linearization Point:\n", keyFormatter); - factors_.print("Nonlinear Graph:\n", keyFormatter); + std::cout << "Nonlinear Graph:" << std::endl; + factors_.print("", keyFormatter); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridNonlinearISAM.h b/gtsam/hybrid/HybridNonlinearISAM.h index 47aa81c55..53bacb0ff 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.h +++ b/gtsam/hybrid/HybridNonlinearISAM.h @@ -90,7 +90,7 @@ class GTSAM_EXPORT HybridNonlinearISAM { const Values& getLinearizationPoint() const { return linPoint_; } /** Return the current discrete assignment */ - const DiscreteValues& getAssignment() const { return assignment_; } + const DiscreteValues& assignment() const { return assignment_; } /** get underlying nonlinear graph */ const HybridNonlinearFactorGraph& getFactorsUnsafe() const { diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index fc1a9a2b8..5285dd191 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -162,14 +162,20 @@ class MixtureFactor : public HybridFactor { } /// Error for HybridValues is not provided for nonlinear hybrid factor. - double error(const HybridValues &values) const override { + double error(const HybridValues& values) const override { throw std::runtime_error( "MixtureFactor::error(HybridValues) not implemented."); } + /** + * @brief Get the dimension of the factor (number of rows on linearization). + * Returns the dimension of the first component factor. + * @return size_t + */ size_t dim() const { - // TODO(Varun) - throw std::runtime_error("MixtureFactor::dim not implemented."); + const auto assignments = DiscreteValues::CartesianProduct(discreteKeys_); + auto factor = factors_(assignments.at(0)); + return factor->dim(); } /// Testable diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index a45c5b92c..4a53a3141 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -114,7 +114,7 @@ TEST(HybridEstimation, Full) { /****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST_DISABLED(HybridEstimation, Incremental) { +TEST(HybridEstimation, Incremental) { 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}; @@ -151,9 +151,6 @@ TEST_DISABLED(HybridEstimation, Incremental) { graph.resize(0); } - /*TODO(Varun) Gives degenerate result due to probability underflow. - Need to normalize probabilities. - */ HybridValues delta = smoother.hybridBayesNet().optimize(); Values result = initial.retract(delta.continuous()); diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index fe3212eda..9e4d66bf2 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -70,8 +70,7 @@ MixtureFactor } /* ************************************************************************* */ -// Test the error of the MixtureFactor -TEST(MixtureFactor, Error) { +static MixtureFactor getMixtureFactor() { DiscreteKey m1(1, 2); double between0 = 0.0; @@ -86,7 +85,13 @@ TEST(MixtureFactor, Error) { boost::make_shared>(X(1), X(2), between1, model); std::vector factors{f0, f1}; - MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); + return MixtureFactor({X(1), X(2)}, {m1}, factors); +} + +/* ************************************************************************* */ +// Test the error of the MixtureFactor +TEST(MixtureFactor, Error) { + auto mixtureFactor = getMixtureFactor(); Values continuousValues; continuousValues.insert(X(1), 0); @@ -94,6 +99,7 @@ TEST(MixtureFactor, Error) { AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousValues); + DiscreteKey m1(1, 2); std::vector discrete_keys = {m1}; std::vector errors = {0.5, 0}; AlgebraicDecisionTree expected_error(discrete_keys, errors); @@ -101,6 +107,13 @@ TEST(MixtureFactor, Error) { EXPECT(assert_equal(expected_error, error_tree)); } +/* ************************************************************************* */ +// Test dim of the MixtureFactor +TEST(MixtureFactor, Dim) { + auto mixtureFactor = getMixtureFactor(); + EXPECT_LONGS_EQUAL(1, mixtureFactor.dim()); +} + /* ************************************************************************* */ int main() { TestResult tr;