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

Use simple constructor

release/4.3a0
Frank Dellaert 2024-09-21 15:52:45 -07:00
parent f84a4c71ae
commit 035f2849d0
4 changed files with 78 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -31,6 +31,9 @@
#include <vector> #include <vector>
#include "gtsam/linear/GaussianFactor.h"
#include "gtsam/linear/GaussianFactorGraph.h"
#pragma once #pragma once
namespace gtsam { namespace gtsam {
@ -44,33 +47,28 @@ using symbol_shorthand::X;
* system which depends on a discrete mode at each time step of the chain. * system which depends on a discrete mode at each time step of the chain.
* *
* @param n The number of chain elements. * @param n The number of chain elements.
* @param keyFunc The functional to help specify the continuous key. * @param x The functional to help specify the continuous key.
* @param dKeyFunc The functional to help specify the discrete key. * @param m The functional to help specify the discrete key.
* @return HybridGaussianFactorGraph::shared_ptr * @return HybridGaussianFactorGraph::shared_ptr
*/ */
inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
size_t n, std::function<Key(int)> keyFunc = X, size_t n, std::function<Key(int)> x = X, std::function<Key(int)> m = M) {
std::function<Key(int)> dKeyFunc = M) {
HybridGaussianFactorGraph hfg; 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++) { for (size_t t = 1; t < n; t++) {
DiscreteKeys dKeys{{dKeyFunc(t), 2}}; DiscreteKeys dKeys{{m(t), 2}};
HybridGaussianFactor::FactorValuePairs components( std::vector<GaussianFactor::shared_ptr> components;
dKeys, {{std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, components.emplace_back(
keyFunc(t + 1), I_3x3, Z_3x1), new JacobianFactor(x(t), I_3x3, x(t + 1), I_3x3, Z_3x1));
0.0}, components.emplace_back(
{std::make_shared<JacobianFactor>( new JacobianFactor(x(t), I_3x3, x(t + 1), I_3x3, Vector3::Ones()));
keyFunc(t), I_3x3, keyFunc(t + 1), I_3x3, Vector3::Ones()), hfg.add(HybridGaussianFactor({x(t), x(t + 1)}, {m(t), 2}, components));
0.0}});
hfg.add(
HybridGaussianFactor({keyFunc(t), keyFunc(t + 1)}, dKeys, components));
if (t > 1) { if (t > 1) {
hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}}, hfg.add(DecisionTreeFactor({{m(t - 1), 2}, {m(t), 2}}, "0 1 1 3"));
"0 1 1 3"));
} }
} }

14
gtsam/hybrid/tests/testHybridFactorGraph.cpp
View File

@ -52,10 +52,10 @@ TEST(HybridFactorGraph, Keys) {
// Add a hybrid Gaussian factor ϕ(x1, c1) // Add a hybrid Gaussian factor ϕ(x1, c1)
DiscreteKey m1(M(1), 2); DiscreteKey m1(M(1), 2);
DecisionTree<Key, GaussianFactorValuePair> dt( std::vector<GaussianFactor::shared_ptr> components{
M(1), {std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0}, std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
{std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0}); std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())};
hfg.add(HybridGaussianFactor({X(1)}, {m1}, dt)); hfg.add(HybridGaussianFactor({X(1)}, {m1}, components));
KeySet expected_continuous{X(0), X(1)}; KeySet expected_continuous{X(0), X(1)};
EXPECT( EXPECT(
@ -65,9 +65,11 @@ TEST(HybridFactorGraph, Keys) {
EXPECT(assert_container_equality(expected_discrete, hfg.discreteKeySet())); EXPECT(assert_container_equality(expected_discrete, hfg.discreteKeySet()));
} }
/* ************************************************************************* */ /* *************************************************************************
*/
int main() { int main() {
TestResult tr; TestResult tr;
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }
/* ************************************************************************* */ /* *************************************************************************
*/

60
gtsam/hybrid/tests/testHybridGaussianFactor.cpp
View File

@ -54,32 +54,49 @@ TEST(HybridGaussianFactor, Constructor) {
CHECK(it == factor.end()); 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. // "Add" two hybrid factors together.
TEST(HybridGaussianFactor, Sum) { 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 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 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 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); 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* // 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? // keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey?
// Design review! // Design review!
HybridGaussianFactor hybridFactorA({X(1), X(2)}, {m1}, factorsA); HybridGaussianFactor hybridFactorA({X(1), X(2)}, m1, {f10, f11});
HybridGaussianFactor hybridFactorB({X(1), X(3)}, {m2}, factorsB); HybridGaussianFactor hybridFactorB({X(1), X(3)}, m2, {f20, f21, f22});
// Check that number of keys is 3 // Check that number of keys is 3
EXPECT_LONGS_EQUAL(3, hybridFactorA.keys().size()); EXPECT_LONGS_EQUAL(3, hybridFactorA.keys().size());
@ -104,15 +121,8 @@ TEST(HybridGaussianFactor, Sum) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HybridGaussianFactor, Printing) { TEST(HybridGaussianFactor, Printing) {
DiscreteKey m1(1, 2); using namespace testA;
auto A1 = Matrix::Zero(2, 1); HybridGaussianFactor hybridFactor({X(1), X(2)}, m1, {f10, f11});
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);
std::string expected = std::string expected =
R"(HybridGaussianFactor R"(HybridGaussianFactor
@ -179,9 +189,7 @@ TEST(HybridGaussianFactor, Error) {
auto f0 = std::make_shared<JacobianFactor>(X(1), A01, X(2), A02, b); 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); 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, {f0, f1});
HybridGaussianFactor hybridFactor({X(1), X(2)}, {m1}, factors);
VectorValues continuousValues; VectorValues continuousValues;
continuousValues.insert(X(1), Vector2(0, 0)); continuousValues.insert(X(1), Vector2(0, 0));

65
gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
View File

@ -114,6 +114,14 @@ TEST(HybridGaussianFactorGraph, EliminateMultifrontal) {
EXPECT_LONGS_EQUAL(result.first->size(), 1); EXPECT_LONGS_EQUAL(result.first->size(), 1);
EXPECT_LONGS_EQUAL(result.second->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) { TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
@ -127,10 +135,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
// Add a hybrid gaussian factor ϕ(x1, c1) // Add a hybrid gaussian factor ϕ(x1, c1)
DiscreteKey m1(M(1), 2); DiscreteKey m1(M(1), 2);
DecisionTree<Key, GaussianFactorValuePair> dt( hfg.add(HybridGaussianFactor({X(1)}, m1, two::components(X(1))));
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));
auto result = hfg.eliminateSequential(); auto result = hfg.eliminateSequential();
@ -153,10 +158,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
// Add factor between x0 and x1 // Add factor between x0 and x1
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
std::vector<GaussianFactorValuePair> factors = { hfg.add(HybridGaussianFactor({X(1)}, m1, two::components(X(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}, factors));
// Discrete probability table for c1 // Discrete probability table for c1
hfg.add(DecisionTreeFactor(m1, {2, 8})); 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, Z_3x1));
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
std::vector<GaussianFactorValuePair> factors = { hfg.add(HybridGaussianFactor({X(1)}, {M(1), 2}, two::components(X(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)}, {M(1), 2}, factors));
hfg.add(DecisionTreeFactor(m1, {2, 8})); hfg.add(DecisionTreeFactor(m1, {2, 8}));
// TODO(Varun) Adding extra discrete variable not connected to continuous // TODO(Varun) Adding extra discrete variable not connected to continuous
@ -207,13 +206,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
// Factor between x0-x1 // Factor between x0-x1
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); 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) // 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 // Prior factor on c1
hfg.add(DecisionTreeFactor(m, {2, 8})); hfg.add(DecisionTreeFactor(m, {2, 8}));
@ -241,13 +235,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
std::vector<GaussianFactorValuePair> factors = { std::vector<GaussianFactorValuePair> factors = {
{std::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1), 0.0}, {std::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1), 0.0},
{std::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()), 0.0}}; {std::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()), 0.0}};
hfg.add(HybridGaussianFactor({X(0)}, {M(0), 2}, factors)); hfg.add(HybridGaussianFactor({X(0)}, {M(0), 2}, two::components(X(0))));
hfg.add(HybridGaussianFactor({X(2)}, {M(1), 2}, two::components(X(2))));
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(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); 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)); hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1));
{ {
DecisionTree<Key, GaussianFactorValuePair> dt( hfg.add(HybridGaussianFactor({X(3)}, {M(3), 2}, two::components(X(3))));
M(3), {std::make_shared<JacobianFactor>(X(3), I_3x3, Z_3x1), 0.0}, hfg.add(HybridGaussianFactor({X(5)}, {M(2), 2}, two::components(X(5))));
{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));
} }
auto ordering_full = 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, Z_3x1));
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
hfg.add(HybridGaussianFactor({X(1)}, c1, two::components(X(1))));
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));
auto result = hfg.eliminateSequential(); auto result = hfg.eliminateSequential();
@ -642,13 +617,13 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrimeTree) {
// regression // regression
EXPECT(assert_equal(expected_error, error_tree, 1e-7)); 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, std::vector<double> prob_leaves = {0.36793249, 0.61247742, 0.59489556,
0.99029064}; 0.99029064};
AlgebraicDecisionTree<Key> expected_probs(discrete_keys, prob_leaves); AlgebraicDecisionTree<Key> expected_probabilities(discrete_keys, prob_leaves);
// regression // regression
EXPECT(assert_equal(expected_probs, probs, 1e-7)); EXPECT(assert_equal(expected_probabilities, probabilities, 1e-7));
} }
/* ****************************************************************************/ /* ****************************************************************************/