diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 93a8a1e00..cc83ccac6 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -12,7 +12,7 @@ /** * @file testHybridNonlinearISAM.cpp * @brief Unit tests for nonlinear incremental inference - * @author Fan Jiang, Varun Agrawal, Frank Dellaert + * @author Varun Agrawal, Fan Jiang, Frank Dellaert * @date Jan 2021 */ @@ -391,6 +391,23 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { 5, bayesTree[X(5)]->conditional()->asMixture()->nrComponents()); } +void printConditionals(const HybridNonlinearISAM &inc, const KeyVector &keys) { + HybridGaussianISAM bayesTree = inc.bayesTree(); + for (auto &&key : keys) { + std::cout << DefaultKeyFormatter(key) << std::endl; + auto conditional = bayesTree[key]->conditional(); + conditional->printKeys(); + if (conditional->isHybrid()) { + std::cout << conditional->asMixture()->nrComponents() << std::endl; + } else if (conditional->isDiscrete()) { + std::cout << conditional->asDiscreteConditional()->nrLeaves() + << std::endl; + } else { + // std::cout << conditional->asGaussian()->nrComponents() << std::endl; + } + } +} + /* ************************************************************************/ // A GTSAM-only test for running inference on a single-legged robot. // The leg links are represented by the chain X-Y-Z-W, where X is the base and @@ -432,9 +449,9 @@ TEST(HybridNonlinearISAM, NonTrivial) { // Don't run update now since we don't have discrete variables involved. - /*************** Run Round 2 ***************/ using PlanarMotionModel = BetweenFactor; + /*************** Run Round 2 ***************/ // Add odometry factor with discrete modes. Pose2 odometry(1.0, 0.0, 0.0); KeyVector contKeys = {W(0), W(1)};