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

add nonlinear switching system tests

release/4.3a0
Varun Agrawal 2022-07-29 20:10:29 -04:00
parent 987448fa77
commit 8471c97b9f
3 changed files with 151 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/hybrid/MixtureFactor.h
View File

@ -1,6 +1,12 @@
/* ----------------------------------------------------------------------------
* Copyright 2020 The Ambitious Folks of the MRG
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**

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

@ -18,12 +18,19 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteDistribution.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/MixtureFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <vector>
#pragma once
using gtsam::symbol_shorthand::C;
@ -31,8 +38,6 @@ using gtsam::symbol_shorthand::X;
namespace gtsam {
using MotionModel = BetweenFactor<double>;
inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
size_t n, std::function<Key(int)> keyFunc = X,
std::function<Key(int)> dKeyFunc = C) {
@ -87,4 +92,103 @@ inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
return {new_order, levels};
}
/* ***************************************************************************
*/
using MotionModel = BetweenFactor<double>;
// using MotionMixture = MixtureFactor<MotionModel>;
// Test fixture with switching network.
struct Switching {
size_t K;
DiscreteKeys modes;
HybridNonlinearFactorGraph nonlinearFactorGraph;
HybridGaussianFactorGraph linearizedFactorGraph;
Values linearizationPoint;
/// Create with given number of time steps.
Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1)
: K(K) {
using symbol_shorthand::M;
using symbol_shorthand::X;
// Create DiscreteKeys for binary K modes, modes[0] will not be used.
for (size_t k = 0; k <= K; k++) {
modes.emplace_back(M(k), 2);
}
// Create hybrid factor graph.
// Add a prior on X(1).
auto prior = boost::make_shared<PriorFactor<double>>(
X(1), 0, noiseModel::Isotropic::Sigma(1, prior_sigma));
nonlinearFactorGraph.push_nonlinear(prior);
// Add "motion models".
for (size_t k = 1; k < K; k++) {
KeyVector keys = {X(k), X(k + 1)};
auto motion_models = motionModels(k);
std::vector<NonlinearFactor::shared_ptr> components;
for (auto &&f : motion_models) {
components.push_back(boost::dynamic_pointer_cast<NonlinearFactor>(f));
}
nonlinearFactorGraph.emplace_hybrid<MixtureFactor>(
keys, DiscreteKeys{modes[k]}, components);
}
// Add measurement factors
auto measurement_noise = noiseModel::Isotropic::Sigma(1, 0.1);
for (size_t k = 1; k <= K; k++) {
nonlinearFactorGraph.emplace_nonlinear<PriorFactor<double>>(
X(k), 1.0 * (k - 1), measurement_noise);
}
// Add "mode chain"
addModeChain(&nonlinearFactorGraph);
// Create the linearization point.
for (size_t k = 1; k <= K; k++) {
linearizationPoint.insert<double>(X(k), static_cast<double>(k));
}
linearizedFactorGraph = nonlinearFactorGraph.linearize(linearizationPoint);
}
// Create motion models for a given time step
static std::vector<MotionModel::shared_ptr> motionModels(size_t k,
double sigma = 1.0) {
using symbol_shorthand::M;
using symbol_shorthand::X;
auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
auto still =
boost::make_shared<MotionModel>(X(k), X(k + 1), 0.0, noise_model),
moving =
boost::make_shared<MotionModel>(X(k), X(k + 1), 1.0, noise_model);
return {still, moving};
}
// Add "mode chain" to HybridNonlinearFactorGraph
void addModeChain(HybridNonlinearFactorGraph *fg) {
auto prior = boost::make_shared<DiscreteDistribution>(modes[1], "1/1");
fg->push_discrete(prior);
for (size_t k = 1; k < K - 1; k++) {
auto parents = {modes[k]};
auto conditional = boost::make_shared<DiscreteConditional>(
modes[k + 1], parents, "1/2 3/2");
fg->push_discrete(conditional);
}
}
// Add "mode chain" to HybridGaussianFactorGraph
void addModeChain(HybridGaussianFactorGraph *fg) {
auto prior = boost::make_shared<DiscreteDistribution>(modes[1], "1/1");
fg->push_discrete(prior);
for (size_t k = 1; k < K - 1; k++) {
auto parents = {modes[k]};
auto conditional = boost::make_shared<DiscreteConditional>(
modes[k + 1], parents, "1/2 3/2");
fg->push_discrete(conditional);
}
}
};
} // namespace gtsam

87
gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
View File

@ -65,10 +65,9 @@ TEST(HybridFactorGraph, GaussianFactorGraph) {
EXPECT_LONGS_EQUAL(2, ghfg.size());
}
/* **************************************************************************
/***************************************************************************
* Test that the resize method works correctly for a HybridNonlinearFactorGraph.
*/
/// Test that the resize method works correctly for a
/// HybridNonlinearFactorGraph.
TEST(HybridNonlinearFactorGraph, Resize) {
HybridNonlinearFactorGraph fg;
auto nonlinearFactor = boost::make_shared<BetweenFactor<double>>();
@ -86,10 +85,10 @@ TEST(HybridNonlinearFactorGraph, Resize) {
EXPECT_LONGS_EQUAL(fg.size(), 0);
}
/* **************************************************************************
/***************************************************************************
* Test that the resize method works correctly for a
* HybridGaussianFactorGraph.
*/
/// Test that the resize method works correctly for a
/// HybridGaussianFactorGraph.
TEST(HybridGaussianFactorGraph, Resize) {
HybridNonlinearFactorGraph nhfg;
auto nonlinearFactor = boost::make_shared<BetweenFactor<double>>(
@ -123,10 +122,9 @@ TEST(HybridGaussianFactorGraph, Resize) {
EXPECT_LONGS_EQUAL(gfg.size(), 0);
}
/*
****************************************************************************
* Test push_back on HFG makes the correct distinction.
*/
/*****************************************************************************
* Test push_back on HFG makes the correct distinction.
*/
TEST(HybridFactorGraph, PushBack) {
HybridNonlinearFactorGraph fg;
@ -168,53 +166,44 @@ TEST(HybridFactorGraph, PushBack) {
EXPECT_LONGS_EQUAL(ghfg.size(), 1);
}
// /*
// ****************************************************************************/
// // Test construction of switching-like hybrid factor graph.
// TEST(HybridFactorGraph, Switching) {
// Switching self(3);
/****************************************************************************
* Test construction of switching-like hybrid factor graph.
*/
TEST(HybridFactorGraph, Switching) {
Switching self(3);
// EXPECT_LONGS_EQUAL(8, self.nonlinearFactorGraph.size());
// EXPECT_LONGS_EQUAL(4, self.nonlinearFactorGraph.nonlinearGraph().size());
// EXPECT_LONGS_EQUAL(2, self.nonlinearFactorGraph.discreteGraph().size());
// EXPECT_LONGS_EQUAL(2, self.nonlinearFactorGraph.dcGraph().size());
EXPECT_LONGS_EQUAL(8, self.nonlinearFactorGraph.size());
// EXPECT_LONGS_EQUAL(8, self.linearizedFactorGraph.size());
// EXPECT_LONGS_EQUAL(2, self.linearizedFactorGraph.discreteGraph().size());
// EXPECT_LONGS_EQUAL(2, self.linearizedFactorGraph.dcGraph().size());
// EXPECT_LONGS_EQUAL(4, self.linearizedFactorGraph.gaussianGraph().size());
// }
EXPECT_LONGS_EQUAL(8, self.linearizedFactorGraph.size());
}
// /*
// ****************************************************************************/
// // Test linearization on a switching-like hybrid factor graph.
// TEST(HybridFactorGraph, Linearization) {
// Switching self(3);
/****************************************************************************
* Test linearization on a switching-like hybrid factor graph.
*/
TEST(HybridFactorGraph, Linearization) {
Switching self(3);
// // Linearize here:
// HybridGaussianFactorGraph actualLinearized =
// self.nonlinearFactorGraph.linearize(self.linearizationPoint);
// Linearize here:
HybridGaussianFactorGraph actualLinearized =
self.nonlinearFactorGraph.linearize(self.linearizationPoint);
// EXPECT_LONGS_EQUAL(8, actualLinearized.size());
// EXPECT_LONGS_EQUAL(2, actualLinearized.discreteGraph().size());
// EXPECT_LONGS_EQUAL(2, actualLinearized.dcGraph().size());
// EXPECT_LONGS_EQUAL(4, actualLinearized.gaussianGraph().size());
// }
EXPECT_LONGS_EQUAL(8, actualLinearized.size());
}
// /*
// ****************************************************************************/
// // Test elimination tree construction
// TEST(HybridFactorGraph, EliminationTree) {
// Switching self(3);
/****************************************************************************
* Test elimination tree construction
*/
TEST(HybridFactorGraph, EliminationTree) {
Switching self(3);
// // Create ordering.
// Ordering ordering;
// for (size_t k = 1; k <= self.K; k++) ordering += X(k);
// Create ordering.
Ordering ordering;
for (size_t k = 1; k <= self.K; k++) ordering += X(k);
// // Create elimination tree.
// HybridEliminationTree etree(self.linearizedFactorGraph, ordering);
// EXPECT_LONGS_EQUAL(1, etree.roots().size())
// }
// Create elimination tree.
HybridEliminationTree etree(self.linearizedFactorGraph, ordering);
EXPECT_LONGS_EQUAL(1, etree.roots().size())
}
// /*
// ****************************************************************************/