12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

minor fixes to unit test

release/4.3a0
Varun Agrawal 2022-10-11 13:15:50 -04:00
parent 5e99cd7095
commit c2377f3b34
1 changed files with 19 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
View File

@ -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<Pose2>;
/*************** Run Round 2 ***************/
// Add odometry factor with discrete modes.
Pose2 odometry(1.0, 0.0, 0.0);
KeyVector contKeys = {W(0), W(1)};