Use simple constructor
Frank Dellaert
parent
f84a4c71ae
commit
035f2849d0
|
|
@ -31,6 +31,9 @@
|
|||
|
||||
#include <vector>
|
||||
|
||||
#include "gtsam/linear/GaussianFactor.h"
|
||||
#include "gtsam/linear/GaussianFactorGraph.h"
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -44,33 +47,28 @@ using symbol_shorthand::X;
|
|||
* system which depends on a discrete mode at each time step of the chain.
|
||||
*
|
||||
* @param n The number of chain elements.
|
||||
* @param keyFunc The functional to help specify the continuous key.
|
||||
* @param dKeyFunc The functional to help specify the discrete key.
|
||||
* @param x The functional to help specify the continuous key.
|
||||
* @param m The functional to help specify the discrete key.
|
||||
* @return HybridGaussianFactorGraph::shared_ptr
|
||||
*/
|
||||
inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
|
||||
size_t n, std::function<Key(int)> keyFunc = X,
|
||||
std::function<Key(int)> dKeyFunc = M) {
|
||||
size_t n, std::function<Key(int)> x = X, std::function<Key(int)> m = M) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1));
|
||||
hfg.add(JacobianFactor(x(1), I_3x3, Z_3x1));
|
||||
|
||||
// keyFunc(1) to keyFunc(n+1)
|
||||
// x(1) to x(n+1)
|
||||
for (size_t t = 1; t < n; t++) {
|
||||
DiscreteKeys dKeys{{dKeyFunc(t), 2}};
|
||||
HybridGaussianFactor::FactorValuePairs components(
|
||||
dKeys, {{std::make_shared<JacobianFactor>(keyFunc(t), I_3x3,
|
||||
keyFunc(t + 1), I_3x3, Z_3x1),
|
||||
0.0},
|
||||
{std::make_shared<JacobianFactor>(
|
||||
keyFunc(t), I_3x3, keyFunc(t + 1), I_3x3, Vector3::Ones()),
|
||||
0.0}});
|
||||
hfg.add(
|
||||
HybridGaussianFactor({keyFunc(t), keyFunc(t + 1)}, dKeys, components));
|
||||
DiscreteKeys dKeys{{m(t), 2}};
|
||||
std::vector<GaussianFactor::shared_ptr> components;
|
||||
components.emplace_back(
|
||||
new JacobianFactor(x(t), I_3x3, x(t + 1), I_3x3, Z_3x1));
|
||||
components.emplace_back(
|
||||
new JacobianFactor(x(t), I_3x3, x(t + 1), I_3x3, Vector3::Ones()));
|
||||
hfg.add(HybridGaussianFactor({x(t), x(t + 1)}, {m(t), 2}, components));
|
||||
|
||||
if (t > 1) {
|
||||
hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}},
|
||||
"0 1 1 3"));
|
||||
hfg.add(DecisionTreeFactor({{m(t - 1), 2}, {m(t), 2}}, "0 1 1 3"));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -52,10 +52,10 @@ TEST(HybridFactorGraph, Keys) {
|
|||
|
||||
// Add a hybrid Gaussian factor ϕ(x1, c1)
|
||||
DiscreteKey m1(M(1), 2);
|
||||
DecisionTree<Key, GaussianFactorValuePair> dt(
|
||||
M(1), {std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
|
||||
{std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0});
|
||||
hfg.add(HybridGaussianFactor({X(1)}, {m1}, dt));
|
||||
std::vector<GaussianFactor::shared_ptr> components{
|
||||
std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
|
||||
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())};
|
||||
hfg.add(HybridGaussianFactor({X(1)}, {m1}, components));
|
||||
|
||||
KeySet expected_continuous{X(0), X(1)};
|
||||
EXPECT(
|
||||
|
|
@ -65,9 +65,11 @@ TEST(HybridFactorGraph, Keys) {
|
|||
EXPECT(assert_container_equality(expected_discrete, hfg.discreteKeySet()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* *************************************************************************
|
||||
*/
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
/* *************************************************************************
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -54,32 +54,49 @@ TEST(HybridGaussianFactor, Constructor) {
|
|||
CHECK(it == factor.end());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace testA {
|
||||
DiscreteKey m1(1, 2);
|
||||
|
||||
auto A1 = Matrix::Zero(2, 1);
|
||||
auto A2 = Matrix::Zero(2, 2);
|
||||
auto b = Matrix::Zero(2, 1);
|
||||
|
||||
auto f10 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
|
||||
auto f11 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
|
||||
} // namespace testA
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test simple to complex constructors...
|
||||
TEST(HybridGaussianFactor, ConstructorVariants) {
|
||||
using namespace testA;
|
||||
HybridGaussianFactor fromFactors({X(1), X(2)}, m1, {f10, f11});
|
||||
|
||||
std::vector<GaussianFactorValuePair> pairs{{f10, 0.0}, {f11, 0.0}};
|
||||
HybridGaussianFactor fromPairs({X(1), X(2)}, {m1}, pairs);
|
||||
assert_equal(fromFactors, fromPairs);
|
||||
|
||||
HybridGaussianFactor::FactorValuePairs decisionTree({m1}, pairs);
|
||||
HybridGaussianFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree);
|
||||
assert_equal(fromDecisionTree, fromPairs);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// "Add" two hybrid factors together.
|
||||
TEST(HybridGaussianFactor, Sum) {
|
||||
DiscreteKey m1(1, 2), m2(2, 3);
|
||||
using namespace testA;
|
||||
DiscreteKey m2(2, 3);
|
||||
|
||||
auto A1 = Matrix::Zero(2, 1);
|
||||
auto A2 = Matrix::Zero(2, 2);
|
||||
auto A3 = Matrix::Zero(2, 3);
|
||||
auto b = Matrix::Zero(2, 1);
|
||||
Vector2 sigmas;
|
||||
sigmas << 1, 2;
|
||||
|
||||
auto f10 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
|
||||
auto f11 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
|
||||
auto f20 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
|
||||
auto f21 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
|
||||
auto f22 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
|
||||
std::vector<GaussianFactorValuePair> factorsA{{f10, 0.0}, {f11, 0.0}};
|
||||
std::vector<GaussianFactorValuePair> factorsB{
|
||||
{f20, 0.0}, {f21, 0.0}, {f22, 0.0}};
|
||||
|
||||
// TODO(Frank): why specify keys at all? And: keys in factor should be *all*
|
||||
// keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey?
|
||||
// Design review!
|
||||
HybridGaussianFactor hybridFactorA({X(1), X(2)}, {m1}, factorsA);
|
||||
HybridGaussianFactor hybridFactorB({X(1), X(3)}, {m2}, factorsB);
|
||||
HybridGaussianFactor hybridFactorA({X(1), X(2)}, m1, {f10, f11});
|
||||
HybridGaussianFactor hybridFactorB({X(1), X(3)}, m2, {f20, f21, f22});
|
||||
|
||||
// Check that number of keys is 3
|
||||
EXPECT_LONGS_EQUAL(3, hybridFactorA.keys().size());
|
||||
|
|
@ -104,15 +121,8 @@ TEST(HybridGaussianFactor, Sum) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactor, Printing) {
|
||||
DiscreteKey m1(1, 2);
|
||||
auto A1 = Matrix::Zero(2, 1);
|
||||
auto A2 = Matrix::Zero(2, 2);
|
||||
auto b = Matrix::Zero(2, 1);
|
||||
auto f10 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
|
||||
auto f11 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
|
||||
std::vector<GaussianFactorValuePair> factors{{f10, 0.0}, {f11, 0.0}};
|
||||
|
||||
HybridGaussianFactor hybridFactor({X(1), X(2)}, {m1}, factors);
|
||||
using namespace testA;
|
||||
HybridGaussianFactor hybridFactor({X(1), X(2)}, m1, {f10, f11});
|
||||
|
||||
std::string expected =
|
||||
R"(HybridGaussianFactor
|
||||
|
|
@ -179,9 +189,7 @@ TEST(HybridGaussianFactor, Error) {
|
|||
|
||||
auto f0 = std::make_shared<JacobianFactor>(X(1), A01, X(2), A02, b);
|
||||
auto f1 = std::make_shared<JacobianFactor>(X(1), A11, X(2), A12, b);
|
||||
std::vector<GaussianFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
|
||||
|
||||
HybridGaussianFactor hybridFactor({X(1), X(2)}, {m1}, factors);
|
||||
HybridGaussianFactor hybridFactor({X(1), X(2)}, m1, {f0, f1});
|
||||
|
||||
VectorValues continuousValues;
|
||||
continuousValues.insert(X(1), Vector2(0, 0));
|
||||
|
|
|
|||
|
|
@ -114,6 +114,14 @@ TEST(HybridGaussianFactorGraph, EliminateMultifrontal) {
|
|||
EXPECT_LONGS_EQUAL(result.first->size(), 1);
|
||||
EXPECT_LONGS_EQUAL(result.second->size(), 1);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
namespace two {
|
||||
std::vector<GaussianFactor::shared_ptr> components(Key key) {
|
||||
return {std::make_shared<JacobianFactor>(key, I_3x3, Z_3x1),
|
||||
std::make_shared<JacobianFactor>(key, I_3x3, Vector3::Ones())};
|
||||
}
|
||||
} // namespace two
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
|
||||
|
|
@ -127,10 +135,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
|
|||
|
||||
// Add a hybrid gaussian factor ϕ(x1, c1)
|
||||
DiscreteKey m1(M(1), 2);
|
||||
DecisionTree<Key, GaussianFactorValuePair> dt(
|
||||
M(1), {std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
|
||||
{std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0});
|
||||
hfg.add(HybridGaussianFactor({X(1)}, {m1}, dt));
|
||||
hfg.add(HybridGaussianFactor({X(1)}, m1, two::components(X(1))));
|
||||
|
||||
auto result = hfg.eliminateSequential();
|
||||
|
||||
|
|
@ -153,10 +158,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
|
|||
// Add factor between x0 and x1
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
|
||||
|
||||
std::vector<GaussianFactorValuePair> factors = {
|
||||
{std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
|
||||
{std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0}};
|
||||
hfg.add(HybridGaussianFactor({X(1)}, {m1}, factors));
|
||||
hfg.add(HybridGaussianFactor({X(1)}, m1, two::components(X(1))));
|
||||
|
||||
// Discrete probability table for c1
|
||||
hfg.add(DecisionTreeFactor(m1, {2, 8}));
|
||||
|
|
@ -178,10 +180,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
|
|||
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
|
||||
|
||||
std::vector<GaussianFactorValuePair> factors = {
|
||||
{std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
|
||||
{std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0}};
|
||||
hfg.add(HybridGaussianFactor({X(1)}, {M(1), 2}, factors));
|
||||
hfg.add(HybridGaussianFactor({X(1)}, {M(1), 2}, two::components(X(1))));
|
||||
|
||||
hfg.add(DecisionTreeFactor(m1, {2, 8}));
|
||||
// TODO(Varun) Adding extra discrete variable not connected to continuous
|
||||
|
|
@ -207,13 +206,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
|
|||
// Factor between x0-x1
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
|
||||
|
||||
// Decision tree with different modes on x1
|
||||
DecisionTree<Key, GaussianFactorValuePair> dt(
|
||||
M(1), {std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
|
||||
{std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0});
|
||||
|
||||
// Hybrid factor P(x1|c1)
|
||||
hfg.add(HybridGaussianFactor({X(1)}, {m}, dt));
|
||||
hfg.add(HybridGaussianFactor({X(1)}, m, two::components(X(1))));
|
||||
// Prior factor on c1
|
||||
hfg.add(DecisionTreeFactor(m, {2, 8}));
|
||||
|
||||
|
|
@ -241,13 +235,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
|
|||
std::vector<GaussianFactorValuePair> factors = {
|
||||
{std::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1), 0.0},
|
||||
{std::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()), 0.0}};
|
||||
hfg.add(HybridGaussianFactor({X(0)}, {M(0), 2}, factors));
|
||||
|
||||
DecisionTree<Key, GaussianFactorValuePair> dt1(
|
||||
M(1), {std::make_shared<JacobianFactor>(X(2), I_3x3, Z_3x1), 0.0},
|
||||
{std::make_shared<JacobianFactor>(X(2), I_3x3, Vector3::Ones()), 0.0});
|
||||
|
||||
hfg.add(HybridGaussianFactor({X(2)}, {{M(1), 2}}, dt1));
|
||||
hfg.add(HybridGaussianFactor({X(0)}, {M(0), 2}, two::components(X(0))));
|
||||
hfg.add(HybridGaussianFactor({X(2)}, {M(1), 2}, two::components(X(2))));
|
||||
}
|
||||
|
||||
hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4"));
|
||||
|
|
@ -256,17 +245,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
|
|||
hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1));
|
||||
|
||||
{
|
||||
DecisionTree<Key, GaussianFactorValuePair> dt(
|
||||
M(3), {std::make_shared<JacobianFactor>(X(3), I_3x3, Z_3x1), 0.0},
|
||||
{std::make_shared<JacobianFactor>(X(3), I_3x3, Vector3::Ones()), 0.0});
|
||||
|
||||
hfg.add(HybridGaussianFactor({X(3)}, {{M(3), 2}}, dt));
|
||||
|
||||
DecisionTree<Key, GaussianFactorValuePair> dt1(
|
||||
M(2), {std::make_shared<JacobianFactor>(X(5), I_3x3, Z_3x1), 0.0},
|
||||
{std::make_shared<JacobianFactor>(X(5), I_3x3, Vector3::Ones()), 0.0});
|
||||
|
||||
hfg.add(HybridGaussianFactor({X(5)}, {{M(2), 2}}, dt1));
|
||||
hfg.add(HybridGaussianFactor({X(3)}, {M(3), 2}, two::components(X(3))));
|
||||
hfg.add(HybridGaussianFactor({X(5)}, {M(2), 2}, two::components(X(5))));
|
||||
}
|
||||
|
||||
auto ordering_full =
|
||||
|
|
@ -551,12 +531,7 @@ TEST(HybridGaussianFactorGraph, optimize) {
|
|||
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
|
||||
|
||||
DecisionTree<Key, GaussianFactorValuePair> dt(
|
||||
C(1), {std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
|
||||
{std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0});
|
||||
|
||||
hfg.add(HybridGaussianFactor({X(1)}, {c1}, dt));
|
||||
hfg.add(HybridGaussianFactor({X(1)}, c1, two::components(X(1))));
|
||||
|
||||
auto result = hfg.eliminateSequential();
|
||||
|
||||
|
|
@ -642,13 +617,13 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrimeTree) {
|
|||
// regression
|
||||
EXPECT(assert_equal(expected_error, error_tree, 1e-7));
|
||||
|
||||
auto probs = graph.probPrime(delta.continuous());
|
||||
auto probabilities = graph.probPrime(delta.continuous());
|
||||
std::vector<double> prob_leaves = {0.36793249, 0.61247742, 0.59489556,
|
||||
0.99029064};
|
||||
AlgebraicDecisionTree<Key> expected_probs(discrete_keys, prob_leaves);
|
||||
AlgebraicDecisionTree<Key> expected_probabilities(discrete_keys, prob_leaves);
|
||||
|
||||
// regression
|
||||
EXPECT(assert_equal(expected_probs, probs, 1e-7));
|
||||
EXPECT(assert_equal(expected_probabilities, probabilities, 1e-7));
|
||||
}
|
||||
|
||||
/* ****************************************************************************/
|
||||
|
|
|
|||
Loading…
Reference in New Issue