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

More tests working

release/4.3a0
Varun Agrawal 2022-05-30 17:51:25 -04:00
parent 0c16799ef6
commit e711a62e2d
2 changed files with 69 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/hybrid/tests/Switching.h
View File

@ -22,6 +22,7 @@
#include <gtsam/hybrid/GaussianMixtureFactor.h> #include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#pragma once #pragma once

155
gtsam/hybrid/tests/testNonlinearHybridFactorGraph.cpp
View File

@ -58,7 +58,7 @@ TEST(HybridFactorGraph, GaussianFactorGraph) {
Values linearizationPoint; Values linearizationPoint;
linearizationPoint.insert<double>(X(0), 0); linearizationPoint.insert<double>(X(0), 0);
GaussianHybridFactorGraph ghfg = *fg.linearize(linearizationPoint); GaussianHybridFactorGraph ghfg = fg.linearize(linearizationPoint);
// Add a factor to the GaussianFactorGraph // Add a factor to the GaussianFactorGraph
ghfg.add(JacobianFactor(X(0), I_1x1, Vector1(5))); ghfg.add(JacobianFactor(X(0), I_1x1, Vector1(5)));
@ -72,121 +72,102 @@ TEST(HybridFactorGraph, GaussianFactorGraph) {
/// NonlinearHybridFactorGraph. /// NonlinearHybridFactorGraph.
TEST(NonlinearHybridFactorGraph, Resize) { TEST(NonlinearHybridFactorGraph, Resize) {
NonlinearHybridFactorGraph fg; NonlinearHybridFactorGraph fg;
// auto nonlinearFactor = boost::make_shared<BetweenFactor<double>>(); auto nonlinearFactor = boost::make_shared<BetweenFactor<double>>();
// fg.push_back(nonlinearFactor); fg.push_back(nonlinearFactor);
// auto discreteFactor = boost::make_shared<DecisionTreeFactor>(); auto discreteFactor = boost::make_shared<DecisionTreeFactor>();
// fg.push_back(discreteFactor); fg.push_back(discreteFactor);
// auto dcFactor = boost::make_shared<MixtureFactor<MotionModel>>(); auto dcFactor = boost::make_shared<MixtureFactor>();
// fg.push_back(dcFactor); fg.push_back(dcFactor);
// EXPECT_LONGS_EQUAL(fg.size(), 3);
EXPECT_LONGS_EQUAL(fg.size(), 3);
fg.resize(0); fg.resize(0);
EXPECT_LONGS_EQUAL(fg.size(), 0); EXPECT_LONGS_EQUAL(fg.size(), 0);
} }
// /* ************************************************************************** /* **************************************************************************
// */ */
// /// Test that the resize method works correctly for a /// Test that the resize method works correctly for a
// /// GaussianHybridFactorGraph. /// GaussianHybridFactorGraph.
// TEST(GaussianHybridFactorGraph, Resize) { TEST(GaussianHybridFactorGraph, Resize) {
// NonlinearHybridFactorGraph nhfg; NonlinearHybridFactorGraph nhfg;
// auto nonlinearFactor = boost::make_shared<BetweenFactor<double>>( auto nonlinearFactor = boost::make_shared<BetweenFactor<double>>(
// X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1)); X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
// nhfg.push_back(nonlinearFactor); nhfg.push_back(nonlinearFactor);
// auto discreteFactor = boost::make_shared<DecisionTreeFactor>(); auto discreteFactor = boost::make_shared<DecisionTreeFactor>();
// nhfg.push_back(discreteFactor); nhfg.push_back(discreteFactor);
// KeyVector contKeys = {X(0), X(1)}; KeyVector contKeys = {X(0), X(1)};
// auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0); auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
// auto still = boost::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model), auto still = boost::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
// moving = boost::make_shared<MotionModel>(X(0), X(1), 1.0, moving = boost::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
// noise_model);
// std::vector<MotionModel::shared_ptr> components = {still, moving};
// auto dcFactor = boost::make_shared<DCMixtureFactor<MotionModel>>(
// contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
// nhfg.push_back(dcFactor);
// Values linearizationPoint; // TODO(Varun) This is declared as NonlinearFactor instead of MotionModel, aka
// linearizationPoint.insert<double>(X(0), 0); // not clear!!
// linearizationPoint.insert<double>(X(1), 1); std::vector<NonlinearFactor::shared_ptr> components = {still, moving};
auto dcFactor = boost::make_shared<MixtureFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
nhfg.push_back(dcFactor);
// // Generate `GaussianHybridFactorGraph` by linearizing Values linearizationPoint;
// GaussianHybridFactorGraph fg = nhfg.linearize(linearizationPoint); linearizationPoint.insert<double>(X(0), 0);
linearizationPoint.insert<double>(X(1), 1);
// EXPECT_LONGS_EQUAL(fg.dcGraph().size(), 1); // Generate `GaussianHybridFactorGraph` by linearizing
// EXPECT_LONGS_EQUAL(fg.discreteGraph().size(), 1); GaussianHybridFactorGraph gfg = nhfg.linearize(linearizationPoint);
// EXPECT_LONGS_EQUAL(fg.gaussianGraph().size(), 1);
// EXPECT_LONGS_EQUAL(fg.size(), 3); EXPECT_LONGS_EQUAL(gfg.size(), 3);
// fg.resize(0); gfg.resize(0);
// EXPECT_LONGS_EQUAL(fg.dcGraph().size(), 0); EXPECT_LONGS_EQUAL(gfg.size(), 0);
// EXPECT_LONGS_EQUAL(fg.discreteGraph().size(), 0); }
// EXPECT_LONGS_EQUAL(fg.gaussianGraph().size(), 0);
// EXPECT_LONGS_EQUAL(fg.size(), 0); /*
// } ****************************************************************************
* Test push_back on HFG makes the correct distinction.
*/
TEST(HybridFactorGraph, PushBack) {
NonlinearHybridFactorGraph fg;
// /* auto nonlinearFactor = boost::make_shared<BetweenFactor<double>>();
// **************************************************************************** fg.push_back(nonlinearFactor);
// * Test push_back on HFG makes the correct distinction.
// */
// TEST(HybridFactorGraph, PushBack) {
// NonlinearHybridFactorGraph fg;
// auto nonlinearFactor = boost::make_shared<BetweenFactor<double>>(); EXPECT_LONGS_EQUAL(fg.size(), 1);
// fg.push_back(nonlinearFactor);
// EXPECT_LONGS_EQUAL(fg.dcGraph().size(), 0); fg = NonlinearHybridFactorGraph();
// EXPECT_LONGS_EQUAL(fg.discreteGraph().size(), 0);
// EXPECT_LONGS_EQUAL(fg.nonlinearGraph().size(), 1);
// fg = NonlinearHybridFactorGraph(); auto discreteFactor = boost::make_shared<DecisionTreeFactor>();
fg.push_back(discreteFactor);
// auto discreteFactor = boost::make_shared<DecisionTreeFactor>(); EXPECT_LONGS_EQUAL(fg.size(), 1);
// fg.push_back(discreteFactor);
// EXPECT_LONGS_EQUAL(fg.dcGraph().size(), 0); fg = NonlinearHybridFactorGraph();
// EXPECT_LONGS_EQUAL(fg.discreteGraph().size(), 1);
// EXPECT_LONGS_EQUAL(fg.nonlinearGraph().size(), 0);
// fg = NonlinearHybridFactorGraph(); auto dcFactor = boost::make_shared<MixtureFactor>();
fg.push_back(dcFactor);
// auto dcFactor = boost::make_shared<DCMixtureFactor<MotionModel>>(); EXPECT_LONGS_EQUAL(fg.size(), 1);
// fg.push_back(dcFactor);
// EXPECT_LONGS_EQUAL(fg.dcGraph().size(), 1); // Now do the same with GaussianHybridFactorGraph
// EXPECT_LONGS_EQUAL(fg.discreteGraph().size(), 0); GaussianHybridFactorGraph ghfg;
// EXPECT_LONGS_EQUAL(fg.nonlinearGraph().size(), 0);
// // Now do the same with GaussianHybridFactorGraph auto gaussianFactor = boost::make_shared<JacobianFactor>();
// GaussianHybridFactorGraph ghfg; ghfg.push_back(gaussianFactor);
// auto gaussianFactor = boost::make_shared<JacobianFactor>(); EXPECT_LONGS_EQUAL(ghfg.size(), 1);
// ghfg.push_back(gaussianFactor);
// EXPECT_LONGS_EQUAL(ghfg.dcGraph().size(), 0); ghfg = GaussianHybridFactorGraph();
// EXPECT_LONGS_EQUAL(ghfg.discreteGraph().size(), 0); ghfg.push_back(discreteFactor);
// EXPECT_LONGS_EQUAL(ghfg.gaussianGraph().size(), 1);
// ghfg = GaussianHybridFactorGraph(); EXPECT_LONGS_EQUAL(ghfg.size(), 1);
// ghfg.push_back(discreteFactor); ghfg = GaussianHybridFactorGraph();
ghfg.push_back(dcFactor);
// EXPECT_LONGS_EQUAL(ghfg.dcGraph().size(), 0); EXPECT_LONGS_EQUAL(ghfg.size(), 1);
// EXPECT_LONGS_EQUAL(ghfg.discreteGraph().size(), 1); }
// EXPECT_LONGS_EQUAL(ghfg.gaussianGraph().size(), 0);
// ghfg = GaussianHybridFactorGraph();
// ghfg.push_back(dcFactor);
// EXPECT_LONGS_EQUAL(ghfg.dcGraph().size(), 1);
// EXPECT_LONGS_EQUAL(ghfg.discreteGraph().size(), 0);
// EXPECT_LONGS_EQUAL(ghfg.gaussianGraph().size(), 0);
// }
// /* // /*
// ****************************************************************************/ // ****************************************************************************/