nonlinear update, all but rhs/config
Michael Kaess
parent
0b9451bc4b
commit
42fca8c399
|
|
@ -1,6 +1,6 @@
|
|||
/**
|
||||
* @file ISAM2-inl.h
|
||||
* @brief Incremental update functionality (ISAM2) for BayesTree.
|
||||
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
|
|
@ -24,23 +24,49 @@ namespace gtsam {
|
|||
template<class Conditional, class Config>
|
||||
ISAM2<Conditional, Config>::ISAM2() : BayesTree<Conditional>() {}
|
||||
|
||||
/** Create a Bayes Tree from a Bayes Net */
|
||||
/** Create a Bayes Tree from a nonlinear factor graph */
|
||||
template<class Conditional, class Config>
|
||||
ISAM2<Conditional, Config>::ISAM2(const BayesNet<Conditional>& bayesNet) : BayesTree<Conditional>(bayesNet) {}
|
||||
ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config) {
|
||||
|
||||
BayesTree<Conditional>(nlfg.linearize(config).eliminate(ordering));
|
||||
|
||||
nonlinearFactors_ = nlfg;
|
||||
config_ = config;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Config>
|
||||
void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactorsXXX, const Config& config, Cliques& orphans) {
|
||||
void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
|
||||
const Config& config, Cliques& orphans) {
|
||||
|
||||
config_ = config; // todo
|
||||
FactorGraph<GaussianFactor> newFactors = newFactorsXXX.linearize(config); // todo: just for testing
|
||||
// copy variables into config_, but don't overwrite existing entries (current linearization point!)
|
||||
for (typename Config::const_iterator it = config.begin(); it!=config.end(); it++) {
|
||||
if (!config_.contains(it->first)) {
|
||||
config_.insert(it->first, it->second);
|
||||
}
|
||||
}
|
||||
nonlinearFactors_.push_back(newFactors);
|
||||
|
||||
FactorGraph<GaussianFactor> newFactorsLinearized = newFactors.linearize(config_);
|
||||
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
FactorGraph<GaussianFactor> factors;
|
||||
boost::tie(factors, orphans) = this->removeTop(newFactors);
|
||||
FactorGraph<GaussianFactor> affectedFactors;
|
||||
boost::tie(affectedFactors, orphans) = this->removeTop(newFactorsLinearized);
|
||||
|
||||
// add the factors themselves
|
||||
factors.push_back(newFactors);
|
||||
// find the corresponding original nonlinear factors, and relinearize them
|
||||
NonlinearFactorGraph<Config> nonlinearAffectedFactors;
|
||||
list<string> keys = affectedFactors.keys();
|
||||
for (list<string>::iterator keyIt = keys.begin(); keyIt!=keys.end(); keyIt++) {
|
||||
list<int> indices = nonlinearFactors_.factors(*keyIt);
|
||||
for (list<int>::iterator indIt = indices.begin(); indIt!=indices.end(); indIt++) {
|
||||
// todo - do we need to check if it already exists? probably... if (*indIt)
|
||||
nonlinearAffectedFactors.push_back(nonlinearFactors_[*indIt]);
|
||||
}
|
||||
}
|
||||
FactorGraph<GaussianFactor> factors = nonlinearAffectedFactors.linearize(config_);
|
||||
|
||||
// add the new factors themselves
|
||||
factors.push_back(newFactorsLinearized);
|
||||
|
||||
// create an ordering for the new and contaminated factors
|
||||
Ordering ordering;
|
||||
|
|
@ -70,7 +96,6 @@ namespace gtsam {
|
|||
parent->children_ += orphan;
|
||||
orphan->parent_ = parent; // set new parent!
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
template<class Conditional, class Config>
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/**
|
||||
* @file ISAM2.h
|
||||
* @brief Incremental update functionality (ISAM2) for BayesTree.
|
||||
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
|
|
@ -26,6 +26,8 @@ namespace gtsam {
|
|||
template<class Conditional, class Config>
|
||||
class ISAM2: public BayesTree<Conditional> {
|
||||
|
||||
protected:
|
||||
|
||||
// for keeping all original nonlinear data
|
||||
Config config_;
|
||||
NonlinearFactorGraph<Config> nonlinearFactors_;
|
||||
|
|
@ -36,7 +38,7 @@ namespace gtsam {
|
|||
ISAM2();
|
||||
|
||||
/** Create a Bayes Tree from a Bayes Net */
|
||||
ISAM2(const BayesNet<Conditional>& bayesNet);
|
||||
ISAM2(const NonlinearFactorGraph<Config>& fg, const Ordering& ordering, const Config& config);
|
||||
|
||||
/** Destructor */
|
||||
virtual ~ISAM2() {
|
||||
|
|
|
|||
|
|
@ -19,13 +19,7 @@ using namespace boost::assign;
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Some numbers that should be consistent among all smoother tests
|
||||
|
||||
double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
|
||||
0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* ************************************************************************* *
|
||||
TEST( ISAM2, ISAM2_smoother )
|
||||
{
|
||||
// Create smoother with 7 nodes
|
||||
|
|
@ -44,7 +38,7 @@ TEST( ISAM2, ISAM2_smoother )
|
|||
// Create expected Bayes Tree by solving smoother with "natural" ordering
|
||||
Ordering ordering;
|
||||
for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
|
||||
GaussianISAM2 expected(smoother.linearize(poses).eliminate(ordering));
|
||||
GaussianISAM2 expected(smoother, ordering, poses);
|
||||
|
||||
// Check whether BayesTree is correct
|
||||
CHECK(assert_equal(expected, actual));
|
||||
|
|
@ -70,7 +64,7 @@ TEST( ISAM2, ISAM2_smoother2 )
|
|||
Ordering ord; ord += "x4","x3","x2","x1";
|
||||
ExampleNonlinearFactorGraph factors1;
|
||||
for (int i=0;i<7;i++) factors1.push_back(smoother[i]);
|
||||
GaussianISAM2 actual(factors1.linearize(poses).eliminate(ord)); // todo: subset of poses?
|
||||
GaussianISAM2 actual(factors1, ord, poses);
|
||||
|
||||
// run ISAM2 with remaining factors
|
||||
ExampleNonlinearFactorGraph factors2;
|
||||
|
|
@ -80,227 +74,9 @@ TEST( ISAM2, ISAM2_smoother2 )
|
|||
// Create expected Bayes Tree by solving smoother with "natural" ordering
|
||||
Ordering ordering;
|
||||
for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
|
||||
GaussianISAM2 expected(smoother.linearize(poses).eliminate(ordering));
|
||||
GaussianISAM2 expected(smoother, ordering, poses);
|
||||
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
Bayes tree for smoother with "natural" ordering:
|
||||
C1 x6 x7
|
||||
C2 x5 : x6
|
||||
C3 x4 : x5
|
||||
C4 x3 : x4
|
||||
C5 x2 : x3
|
||||
C6 x1 : x2
|
||||
/* ************************************************************************* */
|
||||
TEST( BayesTree, linear_smoother_shortcuts )
|
||||
{
|
||||
// Create smoother with 7 nodes
|
||||
GaussianFactorGraph smoother = createSmoother(7);
|
||||
Ordering ordering;
|
||||
for (int t = 1; t <= 7; t++)
|
||||
ordering.push_back(symbol('x', t));
|
||||
|
||||
// eliminate using the "natural" ordering
|
||||
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
||||
|
||||
// Create the Bayes tree
|
||||
GaussianISAM2 bayesTree(chordalBayesNet);
|
||||
LONGS_EQUAL(6,bayesTree.size());
|
||||
|
||||
// Check the conditional P(Root|Root)
|
||||
GaussianBayesNet empty;
|
||||
GaussianISAM2::sharedClique R = bayesTree.root();
|
||||
GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(empty,actual1,1e-4));
|
||||
|
||||
// Check the conditional P(C2|Root)
|
||||
GaussianISAM2::sharedClique C2 = bayesTree["x5"];
|
||||
GaussianBayesNet actual2 = C2->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(empty,actual2,1e-4));
|
||||
|
||||
// Check the conditional P(C3|Root)
|
||||
Vector sigma3 = repeat(2, 0.61808);
|
||||
Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022);
|
||||
GaussianBayesNet expected3;
|
||||
push_front(expected3,"x5", zero(2), eye(2), "x6", A56, sigma3);
|
||||
GaussianISAM2::sharedClique C3 = bayesTree["x4"];
|
||||
GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(expected3,actual3,1e-4));
|
||||
|
||||
// Check the conditional P(C4|Root)
|
||||
Vector sigma4 = repeat(2, 0.661968);
|
||||
Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067);
|
||||
GaussianBayesNet expected4;
|
||||
push_front(expected4,"x4", zero(2), eye(2), "x6", A46, sigma4);
|
||||
GaussianISAM2::sharedClique C4 = bayesTree["x3"];
|
||||
GaussianBayesNet actual4 = C4->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(expected4,actual4,1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
Bayes tree for smoother with "nested dissection" ordering:
|
||||
|
||||
Node[x1] P(x1 | x2)
|
||||
Node[x3] P(x3 | x2 x4)
|
||||
Node[x5] P(x5 | x4 x6)
|
||||
Node[x7] P(x7 | x6)
|
||||
Node[x2] P(x2 | x4)
|
||||
Node[x6] P(x6 | x4)
|
||||
Node[x4] P(x4)
|
||||
|
||||
becomes
|
||||
|
||||
C1 x5 x6 x4
|
||||
C2 x3 x2 : x4
|
||||
C3 x1 : x2
|
||||
C4 x7 : x6
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BayesTree, balanced_smoother_marginals )
|
||||
{
|
||||
// Create smoother with 7 nodes
|
||||
GaussianFactorGraph smoother = createSmoother(7);
|
||||
Ordering ordering;
|
||||
ordering += "x1","x3","x5","x7","x2","x6","x4";
|
||||
|
||||
// eliminate using a "nested dissection" ordering
|
||||
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
||||
|
||||
VectorConfig expectedSolution;
|
||||
BOOST_FOREACH(string key, ordering)
|
||||
expectedSolution.insert(key,zero(2));
|
||||
VectorConfig actualSolution = optimize2(chordalBayesNet);
|
||||
CHECK(assert_equal(expectedSolution,actualSolution,1e-4));
|
||||
|
||||
// Create the Bayes tree
|
||||
GaussianISAM2 bayesTree(chordalBayesNet);
|
||||
LONGS_EQUAL(4,bayesTree.size());
|
||||
|
||||
// Check marginal on x1
|
||||
GaussianBayesNet expected1 = simpleGaussian("x1", zero(2), sigmax1);
|
||||
GaussianBayesNet actual1 = bayesTree.marginalBayesNet<GaussianFactor>("x1");
|
||||
CHECK(assert_equal(expected1,actual1,1e-4));
|
||||
|
||||
// Check marginal on x2
|
||||
GaussianBayesNet expected2 = simpleGaussian("x2", zero(2), sigmax2);
|
||||
GaussianBayesNet actual2 = bayesTree.marginalBayesNet<GaussianFactor>("x2");
|
||||
CHECK(assert_equal(expected2,actual2,1e-4));
|
||||
|
||||
// Check marginal on x3
|
||||
GaussianBayesNet expected3 = simpleGaussian("x3", zero(2), sigmax3);
|
||||
GaussianBayesNet actual3 = bayesTree.marginalBayesNet<GaussianFactor>("x3");
|
||||
CHECK(assert_equal(expected3,actual3,1e-4));
|
||||
|
||||
// Check marginal on x4
|
||||
GaussianBayesNet expected4 = simpleGaussian("x4", zero(2), sigmax4);
|
||||
GaussianBayesNet actual4 = bayesTree.marginalBayesNet<GaussianFactor>("x4");
|
||||
CHECK(assert_equal(expected4,actual4,1e-4));
|
||||
|
||||
// Check marginal on x7 (should be equal to x1)
|
||||
GaussianBayesNet expected7 = simpleGaussian("x7", zero(2), sigmax7);
|
||||
GaussianBayesNet actual7 = bayesTree.marginalBayesNet<GaussianFactor>("x7");
|
||||
CHECK(assert_equal(expected7,actual7,1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BayesTree, balanced_smoother_shortcuts )
|
||||
{
|
||||
// Create smoother with 7 nodes
|
||||
GaussianFactorGraph smoother = createSmoother(7);
|
||||
Ordering ordering;
|
||||
ordering += "x1","x3","x5","x7","x2","x6","x4";
|
||||
|
||||
// Create the Bayes tree
|
||||
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
||||
GaussianISAM2 bayesTree(chordalBayesNet);
|
||||
|
||||
// Check the conditional P(Root|Root)
|
||||
GaussianBayesNet empty;
|
||||
GaussianISAM2::sharedClique R = bayesTree.root();
|
||||
GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(empty,actual1,1e-4));
|
||||
|
||||
// Check the conditional P(C2|Root)
|
||||
GaussianISAM2::sharedClique C2 = bayesTree["x3"];
|
||||
GaussianBayesNet actual2 = C2->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(empty,actual2,1e-4));
|
||||
|
||||
// Check the conditional P(C3|Root), which should be equal to P(x2|x4)
|
||||
GaussianConditional::shared_ptr p_x2_x4 = chordalBayesNet["x2"];
|
||||
GaussianBayesNet expected3; expected3.push_back(p_x2_x4);
|
||||
GaussianISAM2::sharedClique C3 = bayesTree["x1"];
|
||||
GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(expected3,actual3,1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BayesTree, balanced_smoother_clique_marginals )
|
||||
{
|
||||
// Create smoother with 7 nodes
|
||||
GaussianFactorGraph smoother = createSmoother(7);
|
||||
Ordering ordering;
|
||||
ordering += "x1","x3","x5","x7","x2","x6","x4";
|
||||
|
||||
// Create the Bayes tree
|
||||
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
||||
GaussianISAM2 bayesTree(chordalBayesNet);
|
||||
|
||||
// Check the clique marginal P(C3)
|
||||
GaussianBayesNet expected = simpleGaussian("x2",zero(2),sigmax2);
|
||||
Vector sigma = repeat(2, 0.707107);
|
||||
Matrix A12 = (-0.5)*eye(2);
|
||||
push_front(expected,"x1", zero(2), eye(2), "x2", A12, sigma);
|
||||
GaussianISAM2::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"];
|
||||
FactorGraph<GaussianFactor> marginal = C3->marginal<GaussianFactor>(R);
|
||||
GaussianBayesNet actual = eliminate<GaussianFactor,GaussianConditional>(marginal,C3->keys());
|
||||
CHECK(assert_equal(expected,actual,1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BayesTree, balanced_smoother_joint )
|
||||
{
|
||||
// Create smoother with 7 nodes
|
||||
GaussianFactorGraph smoother = createSmoother(7);
|
||||
Ordering ordering;
|
||||
ordering += "x1","x3","x5","x7","x2","x6","x4";
|
||||
|
||||
// Create the Bayes tree
|
||||
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
||||
GaussianISAM2 bayesTree(chordalBayesNet);
|
||||
|
||||
// Conditional density elements reused by both tests
|
||||
Vector sigma = repeat(2, 0.786146);
|
||||
Matrix I = eye(2), A = -0.00429185*I;
|
||||
|
||||
// Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
|
||||
GaussianBayesNet expected1 = simpleGaussian("x7", zero(2), sigmax7);
|
||||
push_front(expected1,"x1", zero(2), I, "x7", A, sigma);
|
||||
GaussianBayesNet actual1 = bayesTree.jointBayesNet<GaussianFactor>("x1","x7");
|
||||
CHECK(assert_equal(expected1,actual1,1e-4));
|
||||
|
||||
// Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
|
||||
GaussianBayesNet expected2 = simpleGaussian("x1", zero(2), sigmax1);
|
||||
push_front(expected2,"x7", zero(2), I, "x1", A, sigma);
|
||||
GaussianBayesNet actual2 = bayesTree.jointBayesNet<GaussianFactor>("x7","x1");
|
||||
CHECK(assert_equal(expected2,actual2,1e-4));
|
||||
|
||||
// Check the joint density P(x1,x4), i.e. with a root variable
|
||||
GaussianBayesNet expected3 = simpleGaussian("x4", zero(2), sigmax4);
|
||||
Vector sigma14 = repeat(2, 0.784465);
|
||||
Matrix A14 = -0.0769231*I;
|
||||
push_front(expected3,"x1", zero(2), I, "x4", A14, sigma14);
|
||||
GaussianBayesNet actual3 = bayesTree.jointBayesNet<GaussianFactor>("x1","x4");
|
||||
CHECK(assert_equal(expected3,actual3,1e-4));
|
||||
|
||||
// Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
|
||||
GaussianBayesNet expected4 = simpleGaussian("x1", zero(2), sigmax1);
|
||||
Vector sigma41 = repeat(2, 0.668096);
|
||||
Matrix A41 = -0.055794*I;
|
||||
push_front(expected4,"x4", zero(2), I, "x1", A41, sigma41);
|
||||
GaussianBayesNet actual4 = bayesTree.jointBayesNet<GaussianFactor>("x4","x1");
|
||||
CHECK(assert_equal(expected4,actual4,1e-4));
|
||||
// CHECK(assert_equal(expected, actual)); // todo: actual is wrong...
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue