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

Converting smallExample and GaussianISAM

release/4.3a0
Richard Roberts 2013-07-30 02:11:34 +00:00
parent 7b6edff381
commit 69a9d75dd6
6 changed files with 913 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
gtsam/linear/GaussianISAM.cpp Normal file
View File

@ -0,0 +1,31 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/**
* @file SymbolicISAM.cpp
* @date July 29, 2013
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/inference/ISAM-inst.h>
namespace gtsam {
/* ************************************************************************* */
GaussianISAM::GaussianISAM() {}
/* ************************************************************************* */
GaussianISAM::GaussianISAM(const GaussianBayesTree& bayesTree) :
Base(bayesTree) {}
}

46
gtsam/linear/GaussianISAM.h Normal file
View File

@ -0,0 +1,46 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/**
* @file SymbolicISAM.h
* @date July 29, 2013
* @author Frank Dellaert
* @author Richard Roberts
*/
#pragma once
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/inference/ISAM.h>
namespace gtsam {
class GTSAM_EXPORT GaussianISAM : public ISAM<GaussianBayesTree>
{
public:
typedef ISAM<GaussianBayesTree> Base;
typedef GaussianISAM This;
typedef boost::shared_ptr<This> shared_ptr;
/// @name Standard Constructors
/// @{
/** Create an empty Bayes Tree */
GaussianISAM();
/** Copy constructor */
GaussianISAM(const GaussianBayesTree& bayesTree);
/// @}
};
}

174
tests/smallExample.h
View File

@ -30,13 +30,13 @@ namespace gtsam {
namespace example { namespace example {
namespace { namespace {
typedef NonlinearFactorGraph Graph; typedef NonlinearFactorGraph NonlinearFactorGraph;
/** /**
* Create small example for non-linear factor graph * Create small example for non-linear factor graph
*/ */
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph(); boost::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph();
Graph createNonlinearFactorGraph(); NonlinearFactorGraph createNonlinearFactorGraph();
/** /**
* Create values structure to go with it * Create values structure to go with it
@ -45,7 +45,7 @@ Graph createNonlinearFactorGraph();
Values createValues(); Values createValues();
/** Vector Values equivalent */ /** Vector Values equivalent */
VectorValuesOrdered createVectorValues(); VectorValues createVectorValues();
/** /**
* create a noisy values structure for a nonlinear factor graph * create a noisy values structure for a nonlinear factor graph
@ -56,42 +56,42 @@ Values createNoisyValues();
/** /**
* Zero delta config * Zero delta config
*/ */
VectorValuesOrdered createZeroDelta(const OrderingOrdered& ordering); VectorValues createZeroDelta(const Ordering& ordering);
/** /**
* Delta config that, when added to noisyValues, returns the ground truth * Delta config that, when added to noisyValues, returns the ground truth
*/ */
VectorValuesOrdered createCorrectDelta(const OrderingOrdered& ordering); VectorValues createCorrectDelta(const Ordering& ordering);
/** /**
* create a linear factor graph * create a linear factor graph
* The non-linear graph above evaluated at NoisyValues * The non-linear graph above evaluated at NoisyValues
*/ */
GaussianFactorGraphOrdered createGaussianFactorGraph(const OrderingOrdered& ordering); GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering);
/** /**
* create small Chordal Bayes Net x <- y * create small Chordal Bayes Net x <- y
*/ */
GaussianBayesNetOrdered createSmallGaussianBayesNet(); GaussianBayesNet createSmallGaussianBayesNet();
/** /**
* Create really non-linear factor graph (cos/sin) * Create really non-linear factor graph (cos/sin)
*/ */
boost::shared_ptr<const Graph> boost::shared_ptr<const NonlinearFactorGraph>
sharedReallyNonlinearFactorGraph(); sharedReallyNonlinearFactorGraph();
Graph createReallyNonlinearFactorGraph(); NonlinearFactorGraph createReallyNonlinearFactorGraph();
/** /**
* Create a full nonlinear smoother * Create a full nonlinear smoother
* @param T number of time-steps * @param T number of time-steps
*/ */
std::pair<Graph, Values> createNonlinearSmoother(int T); std::pair<NonlinearFactorGraph, Values> createNonlinearSmoother(int T);
/** /**
* Create a Kalman smoother by linearizing a non-linear factor graph * Create a Kalman smoother by linearizing a non-linear factor graph
* @param T number of time-steps * @param T number of time-steps
*/ */
std::pair<FactorGraphOrdered<GaussianFactorOrdered>, OrderingOrdered> createSmoother(int T, boost::optional<OrderingOrdered> ordering = boost::none); std::pair<GaussianFactorGraph, Ordering> createSmoother(int T, boost::optional<Ordering> ordering = boost::none);
/* ******************************************************* */ /* ******************************************************* */
// Linear Constrained Examples // Linear Constrained Examples
@ -101,22 +101,22 @@ std::pair<FactorGraphOrdered<GaussianFactorOrdered>, OrderingOrdered> createSmoo
* Creates a simple constrained graph with one linear factor and * Creates a simple constrained graph with one linear factor and
* one binary equality constraint that sets x = y * one binary equality constraint that sets x = y
*/ */
GaussianFactorGraphOrdered createSimpleConstraintGraph(); GaussianFactorGraph createSimpleConstraintGraph();
VectorValuesOrdered createSimpleConstraintValues(); VectorValues createSimpleConstraintValues();
/** /**
* Creates a simple constrained graph with one linear factor and * Creates a simple constrained graph with one linear factor and
* one binary constraint. * one binary constraint.
*/ */
GaussianFactorGraphOrdered createSingleConstraintGraph(); GaussianFactorGraph createSingleConstraintGraph();
VectorValuesOrdered createSingleConstraintValues(); VectorValues createSingleConstraintValues();
/** /**
* Creates a constrained graph with a linear factor and two * Creates a constrained graph with a linear factor and two
* binary constraints that share a node * binary constraints that share a node
*/ */
GaussianFactorGraphOrdered createMultiConstraintGraph(); GaussianFactorGraph createMultiConstraintGraph();
VectorValuesOrdered createMultiConstraintValues(); VectorValues createMultiConstraintValues();
/* ******************************************************* */ /* ******************************************************* */
// Planar graph with easy subtree for SubgraphPreconditioner // Planar graph with easy subtree for SubgraphPreconditioner
@ -131,14 +131,14 @@ VectorValuesOrdered createMultiConstraintValues();
* -x11-x21-x31 * -x11-x21-x31
* with x11 clamped at (1,1), and others related by 2D odometry. * with x11 clamped at (1,1), and others related by 2D odometry.
*/ */
boost::tuple<GaussianFactorGraphOrdered, VectorValuesOrdered> planarGraph(size_t N); boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N);
/* /*
* Create canonical ordering for planar graph that also works for tree * Create canonical ordering for planar graph that also works for tree
* With x11 the root, e.g. for N=3 * With x11 the root, e.g. for N=3
* x33 x23 x13 x32 x22 x12 x31 x21 x11 * x33 x23 x13 x32 x22 x12 x31 x21 x11
*/ */
OrderingOrdered planarOrdering(size_t N); Ordering planarOrdering(size_t N);
/* /*
* Split graph into tree and loop closing constraints, e.g., with N=3 * Split graph into tree and loop closing constraints, e.g., with N=3
@ -148,8 +148,8 @@ OrderingOrdered planarOrdering(size_t N);
* | * |
* -x11-x21-x31 * -x11-x21-x31
*/ */
std::pair<GaussianFactorGraphOrdered, GaussianFactorGraphOrdered > splitOffPlanarTree( std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(
size_t N, const GaussianFactorGraphOrdered& original); size_t N, const GaussianFactorGraph& original);
@ -165,19 +165,19 @@ static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
static SharedDiagonal constraintModel = noiseModel::Constrained::All(2); static SharedDiagonal constraintModel = noiseModel::Constrained::All(2);
static const Index _l1_=0, _x1_=1, _x2_=2; static const Key _l1_=0, _x1_=1, _x2_=2;
static const Index _x_=0, _y_=1, _z_=2; static const Key _x_=0, _y_=1, _z_=2;
} // \namespace impl } // \namespace impl
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() { boost::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph() {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
// Create // Create
boost::shared_ptr<Graph> nlfg( boost::shared_ptr<NonlinearFactorGraph> nlfg(
new Graph); new NonlinearFactorGraph);
// prior on x1 // prior on x1
Point2 mu; Point2 mu;
@ -203,7 +203,7 @@ boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Graph createNonlinearFactorGraph() { NonlinearFactorGraph createNonlinearFactorGraph() {
return *sharedNonlinearFactorGraph(); return *sharedNonlinearFactorGraph();
} }
@ -219,9 +219,9 @@ Values createValues() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValuesOrdered createVectorValues() { VectorValues createVectorValues() {
using namespace impl; using namespace impl;
VectorValuesOrdered c(std::vector<size_t>(3, 2)); VectorValues c(std::vector<size_t>(3, 2));
c[_l1_] = Vector_(2, 0.0, -1.0); c[_l1_] = Vector_(2, 0.0, -1.0);
c[_x1_] = Vector_(2, 0.0, 0.0); c[_x1_] = Vector_(2, 0.0, 0.0);
c[_x2_] = Vector_(2, 1.5, 0.0); c[_x2_] = Vector_(2, 1.5, 0.0);
@ -245,47 +245,47 @@ Values createNoisyValues() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValuesOrdered createCorrectDelta(const OrderingOrdered& ordering) { VectorValues createCorrectDelta() {
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
VectorValuesOrdered c(std::vector<size_t>(3,2)); VectorValues c;
c[ordering[L(1)]] = Vector_(2, -0.1, 0.1); c.insert(L(1), Vector_(2, -0.1, 0.1));
c[ordering[X(1)]] = Vector_(2, -0.1, -0.1); c.insert(X(1), Vector_(2, -0.1, -0.1));
c[ordering[X(2)]] = Vector_(2, 0.1, -0.2); c.insert(X(2), Vector_(2, 0.1, -0.2));
return c; return c;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValuesOrdered createZeroDelta(const OrderingOrdered& ordering) { VectorValues createZeroDelta() {
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
VectorValuesOrdered c(std::vector<size_t>(3,2)); VectorValues c(std::vector<size_t>(3,2));
c[ordering[L(1)]] = zero(2); c.insert(L(1), zero(2));
c[ordering[X(1)]] = zero(2); c.insert(X(1), zero(2));
c[ordering[X(2)]] = zero(2); c.insert(X(2), zero(2));
return c; return c;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraphOrdered createGaussianFactorGraph(const OrderingOrdered& ordering) { GaussianFactorGraph createGaussianFactorGraph() {
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
// Create empty graph // Create empty graph
GaussianFactorGraphOrdered fg; GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2); SharedDiagonal unit2 = noiseModel::Unit::Create(2);
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg.push_back(boost::make_shared<JacobianFactorOrdered>(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2)); fg += JacobianFactor(X(1), 10*eye(2), -1.0*ones(2), unit2);
// odometry between x1 and x2: x2-x1=[0.2;-0.1] // odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg.push_back(boost::make_shared<JacobianFactorOrdered>(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2)); fg += JacobianFactor(X(1), -10*eye(2), X(2), 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
// measurement between x1 and l1: l1-x1=[0.0;0.2] // measurement between x1 and l1: l1-x1=[0.0;0.2]
fg.push_back(boost::make_shared<JacobianFactorOrdered>(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2)); fg += JacobianFactor(X(1), -5*eye(2), L(1), 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
// measurement between x2 and l1: l1-x2=[-0.2;0.3] // measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg.push_back(boost::make_shared<JacobianFactorOrdered>(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2)); fg += JacobianFactor(X(2), -5*eye(2), L(1), 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
return fg; return fg;
} }
@ -296,20 +296,18 @@ GaussianFactorGraphOrdered createGaussianFactorGraph(const OrderingOrdered& orde
* 1 1 9 * 1 1 9
* 1 5 * 1 5
*/ */
GaussianBayesNetOrdered createSmallGaussianBayesNet() { GaussianBayesNet createSmallGaussianBayesNet() {
using namespace impl; using namespace impl;
Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0); Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
Matrix R22 = Matrix_(1, 1, 1.0); Matrix R22 = Matrix_(1, 1, 1.0);
Vector d1(1), d2(1); Vector d1(1), d2(1);
d1(0) = 9; d1(0) = 9;
d2(0) = 5; d2(0) = 5;
Vector tau(1);
tau(0) = 1.0;
// define nodes and specify in reverse topological sort (i.e. parents last) // define nodes and specify in reverse topological sort (i.e. parents last)
GaussianConditionalOrdered::shared_ptr Px_y(new GaussianConditionalOrdered(_x_, d1, R11, _y_, S12, tau)); GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12));
GaussianConditionalOrdered::shared_ptr Py(new GaussianConditionalOrdered(_y_, d2, R22, tau)); GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22));
GaussianBayesNetOrdered cbn; GaussianBayesNet cbn;
cbn.push_back(Px_y); cbn.push_back(Px_y);
cbn.push_back(Py); cbn.push_back(Py);
@ -349,10 +347,10 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> {
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const Graph> sharedReallyNonlinearFactorGraph() { boost::shared_ptr<const NonlinearFactorGraph> sharedReallyNonlinearFactorGraph() {
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
boost::shared_ptr<Graph> fg(new Graph); boost::shared_ptr<NonlinearFactorGraph> fg(new NonlinearFactorGraph);
Vector z = Vector_(2, 1.0, 0.0); Vector z = Vector_(2, 1.0, 0.0);
double sigma = 0.1; double sigma = 0.1;
boost::shared_ptr<smallOptimize::UnaryFactor> factor( boost::shared_ptr<smallOptimize::UnaryFactor> factor(
@ -361,18 +359,18 @@ boost::shared_ptr<const Graph> sharedReallyNonlinearFactorGraph() {
return fg; return fg;
} }
Graph createReallyNonlinearFactorGraph() { NonlinearFactorGraph createReallyNonlinearFactorGraph() {
return *sharedReallyNonlinearFactorGraph(); return *sharedReallyNonlinearFactorGraph();
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<Graph, Values> createNonlinearSmoother(int T) { std::pair<NonlinearFactorGraph, Values> createNonlinearSmoother(int T) {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
// Create // Create
Graph nlfg; NonlinearFactorGraph nlfg;
Values poses; Values poses;
// prior on x1 // prior on x1
@ -400,8 +398,8 @@ std::pair<Graph, Values> createNonlinearSmoother(int T) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<FactorGraphOrdered<GaussianFactorOrdered>, OrderingOrdered> createSmoother(int T, boost::optional<OrderingOrdered> ordering) { std::pair<GaussianFactorGraph, Ordering> createSmoother(int T) {
Graph nlfg; NonlinearFactorGraph nlfg;
Values poses; Values poses;
boost::tie(nlfg, poses) = createNonlinearSmoother(T); boost::tie(nlfg, poses) = createNonlinearSmoother(T);
@ -410,7 +408,7 @@ std::pair<FactorGraphOrdered<GaussianFactorOrdered>, OrderingOrdered> createSmoo
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraphOrdered createSimpleConstraintGraph() { GaussianFactorGraph createSimpleConstraintGraph() {
using namespace impl; using namespace impl;
// create unary factor // create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1 // prior on _x_, mean = [1,-1], sigma=0.1
@ -418,7 +416,7 @@ GaussianFactorGraphOrdered createSimpleConstraintGraph() {
Vector b1(2); Vector b1(2);
b1(0) = 1.0; b1(0) = 1.0;
b1(1) = -1.0; b1(1) = -1.0;
JacobianFactorOrdered::shared_ptr f1(new JacobianFactorOrdered(_x_, Ax, b1, sigma0_1)); JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));
// create binary constraint factor // create binary constraint factor
// between _x_ and _y_, that is going to be the only factor on _y_ // between _x_ and _y_, that is going to be the only factor on _y_
@ -427,11 +425,11 @@ GaussianFactorGraphOrdered createSimpleConstraintGraph() {
Matrix Ax1 = eye(2); Matrix Ax1 = eye(2);
Matrix Ay1 = eye(2) * -1; Matrix Ay1 = eye(2) * -1;
Vector b2 = Vector_(2, 0.0, 0.0); Vector b2 = Vector_(2, 0.0, 0.0);
JacobianFactorOrdered::shared_ptr f2(new JacobianFactorOrdered(_x_, Ax1, _y_, Ay1, b2, JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
constraintModel)); constraintModel));
// construct the graph // construct the graph
GaussianFactorGraphOrdered fg; GaussianFactorGraph fg;
fg.push_back(f1); fg.push_back(f1);
fg.push_back(f2); fg.push_back(f2);
@ -439,11 +437,11 @@ GaussianFactorGraphOrdered createSimpleConstraintGraph() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValuesOrdered createSimpleConstraintValues() { VectorValues createSimpleConstraintValues() {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
VectorValuesOrdered config(std::vector<size_t>(2,2)); VectorValues config(std::vector<size_t>(2,2));
Vector v = Vector_(2, 1.0, -1.0); Vector v = Vector_(2, 1.0, -1.0);
config[_x_] = v; config[_x_] = v;
config[_y_] = v; config[_y_] = v;
@ -451,7 +449,7 @@ VectorValuesOrdered createSimpleConstraintValues() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraphOrdered createSingleConstraintGraph() { GaussianFactorGraph createSingleConstraintGraph() {
using namespace impl; using namespace impl;
// create unary factor // create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1 // prior on _x_, mean = [1,-1], sigma=0.1
@ -459,8 +457,8 @@ GaussianFactorGraphOrdered createSingleConstraintGraph() {
Vector b1(2); Vector b1(2);
b1(0) = 1.0; b1(0) = 1.0;
b1(1) = -1.0; b1(1) = -1.0;
//GaussianFactorOrdered::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1)); //GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1));
JacobianFactorOrdered::shared_ptr f1(new JacobianFactorOrdered(_x_, Ax, b1, sigma0_1)); JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));
// create binary constraint factor // create binary constraint factor
// between _x_ and _y_, that is going to be the only factor on _y_ // between _x_ and _y_, that is going to be the only factor on _y_
@ -473,11 +471,11 @@ GaussianFactorGraphOrdered createSingleConstraintGraph() {
Ax1(1, 1) = 1.0; Ax1(1, 1) = 1.0;
Matrix Ay1 = eye(2) * 10; Matrix Ay1 = eye(2) * 10;
Vector b2 = Vector_(2, 1.0, 2.0); Vector b2 = Vector_(2, 1.0, 2.0);
JacobianFactorOrdered::shared_ptr f2(new JacobianFactorOrdered(_x_, Ax1, _y_, Ay1, b2, JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
constraintModel)); constraintModel));
// construct the graph // construct the graph
GaussianFactorGraphOrdered fg; GaussianFactorGraph fg;
fg.push_back(f1); fg.push_back(f1);
fg.push_back(f2); fg.push_back(f2);
@ -485,21 +483,21 @@ GaussianFactorGraphOrdered createSingleConstraintGraph() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValuesOrdered createSingleConstraintValues() { VectorValues createSingleConstraintValues() {
using namespace impl; using namespace impl;
VectorValuesOrdered config(std::vector<size_t>(2,2)); VectorValues config(std::vector<size_t>(2,2));
config[_x_] = Vector_(2, 1.0, -1.0); config[_x_] = Vector_(2, 1.0, -1.0);
config[_y_] = Vector_(2, 0.2, 0.1); config[_y_] = Vector_(2, 0.2, 0.1);
return config; return config;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraphOrdered createMultiConstraintGraph() { GaussianFactorGraph createMultiConstraintGraph() {
using namespace impl; using namespace impl;
// unary factor 1 // unary factor 1
Matrix A = eye(2); Matrix A = eye(2);
Vector b = Vector_(2, -2.0, 2.0); Vector b = Vector_(2, -2.0, 2.0);
JacobianFactorOrdered::shared_ptr lf1(new JacobianFactorOrdered(_x_, A, b, sigma0_1)); JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1));
// constraint 1 // constraint 1
Matrix A11(2, 2); Matrix A11(2, 2);
@ -517,7 +515,7 @@ GaussianFactorGraphOrdered createMultiConstraintGraph() {
Vector b1(2); Vector b1(2);
b1(0) = 1.0; b1(0) = 1.0;
b1(1) = 2.0; b1(1) = 2.0;
JacobianFactorOrdered::shared_ptr lc1(new JacobianFactorOrdered(_x_, A11, _y_, A12, b1, JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1,
constraintModel)); constraintModel));
// constraint 2 // constraint 2
@ -536,11 +534,11 @@ GaussianFactorGraphOrdered createMultiConstraintGraph() {
Vector b2(2); Vector b2(2);
b2(0) = 3.0; b2(0) = 3.0;
b2(1) = 4.0; b2(1) = 4.0;
JacobianFactorOrdered::shared_ptr lc2(new JacobianFactorOrdered(_x_, A21, _z_, A22, b2, JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2,
constraintModel)); constraintModel));
// construct the graph // construct the graph
GaussianFactorGraphOrdered fg; GaussianFactorGraph fg;
fg.push_back(lf1); fg.push_back(lf1);
fg.push_back(lc1); fg.push_back(lc1);
fg.push_back(lc2); fg.push_back(lc2);
@ -549,9 +547,9 @@ GaussianFactorGraphOrdered createMultiConstraintGraph() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValuesOrdered createMultiConstraintValues() { VectorValues createMultiConstraintValues() {
using namespace impl; using namespace impl;
VectorValuesOrdered config(std::vector<size_t>(3,2)); VectorValues config(std::vector<size_t>(3,2));
config[_x_] = Vector_(2, -2.0, 2.0); config[_x_] = Vector_(2, -2.0, 2.0);
config[_y_] = Vector_(2, -0.1, 0.4); config[_y_] = Vector_(2, -0.1, 0.4);
config[_z_] = Vector_(2, -4.0, 5.0); config[_z_] = Vector_(2, -4.0, 5.0);
@ -568,7 +566,7 @@ Symbol key(int x, int y) {
} // \namespace impl } // \namespace impl
/* ************************************************************************* */ /* ************************************************************************* */
boost::tuple<GaussianFactorGraphOrdered, VectorValuesOrdered> planarGraph(size_t N) { boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
using namespace impl; using namespace impl;
// create empty graph // create empty graph
@ -599,20 +597,20 @@ boost::tuple<GaussianFactorGraphOrdered, VectorValuesOrdered> planarGraph(size_t
for (size_t x = 1; x <= N; x++) for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++) for (size_t y = 1; y <= N; y++)
zeros.insert(key(x, y), Point2()); zeros.insert(key(x, y), Point2());
OrderingOrdered ordering(planarOrdering(N)); Ordering ordering(planarOrdering(N));
VectorValuesOrdered xtrue(zeros.dims(ordering)); VectorValues xtrue(zeros.dims(ordering));
for (size_t x = 1; x <= N; x++) for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++) for (size_t y = 1; y <= N; y++)
xtrue[ordering[key(x, y)]] = Point2(x,y).vector(); xtrue[ordering[key(x, y)]] = Point2(x,y).vector();
// linearize around zero // linearize around zero
boost::shared_ptr<GaussianFactorGraphOrdered> gfg = nlfg.linearize(zeros, ordering); boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros, ordering);
return boost::make_tuple(*gfg, xtrue); return boost::make_tuple(*gfg, xtrue);
} }
/* ************************************************************************* */ /* ************************************************************************* */
OrderingOrdered planarOrdering(size_t N) { Ordering planarOrdering(size_t N) {
OrderingOrdered ordering; Ordering ordering;
for (size_t y = N; y >= 1; y--) for (size_t y = N; y >= 1; y--)
for (size_t x = N; x >= 1; x--) for (size_t x = N; x >= 1; x--)
ordering.push_back(impl::key(x, y)); ordering.push_back(impl::key(x, y));
@ -620,9 +618,9 @@ OrderingOrdered planarOrdering(size_t N) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<GaussianFactorGraphOrdered, GaussianFactorGraphOrdered > splitOffPlanarTree(size_t N, std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N,
const GaussianFactorGraphOrdered& original) { const GaussianFactorGraph& original) {
GaussianFactorGraphOrdered T, C; GaussianFactorGraph T, C;
// Add the x11 constraint to the tree // Add the x11 constraint to the tree
T.push_back(original[0]); T.push_back(original[0]);

651
tests/smallExampleOrdered.h Normal file
View File

@ -0,0 +1,651 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/**
* @file smallExample.h
* @brief Create small example with two poses and one landmark
* @brief smallExample
* @author Carlos Nieto
*/
// \callgraph
#pragma once
#include <tests/simulated2D.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <boost/tuple/tuple.hpp>
namespace gtsam {
namespace example {
namespace {
typedef NonlinearFactorGraph Graph;
/**
* Create small example for non-linear factor graph
*/
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph();
Graph createNonlinearFactorGraph();
/**
* Create values structure to go with it
* The ground truth values structure for the example above
*/
Values createValues();
/** Vector Values equivalent */
VectorValuesOrdered createVectorValues();
/**
* create a noisy values structure for a nonlinear factor graph
*/
boost::shared_ptr<const Values> sharedNoisyValues();
Values createNoisyValues();
/**
* Zero delta config
*/
VectorValuesOrdered createZeroDelta(const OrderingOrdered& ordering);
/**
* Delta config that, when added to noisyValues, returns the ground truth
*/
VectorValuesOrdered createCorrectDelta(const OrderingOrdered& ordering);
/**
* create a linear factor graph
* The non-linear graph above evaluated at NoisyValues
*/
GaussianFactorGraphOrdered createGaussianFactorGraph(const OrderingOrdered& ordering);
/**
* create small Chordal Bayes Net x <- y
*/
GaussianBayesNetOrdered createSmallGaussianBayesNet();
/**
* Create really non-linear factor graph (cos/sin)
*/
boost::shared_ptr<const Graph>
sharedReallyNonlinearFactorGraph();
Graph createReallyNonlinearFactorGraph();
/**
* Create a full nonlinear smoother
* @param T number of time-steps
*/
std::pair<Graph, Values> createNonlinearSmoother(int T);
/**
* Create a Kalman smoother by linearizing a non-linear factor graph
* @param T number of time-steps
*/
std::pair<FactorGraphOrdered<GaussianFactorOrdered>, OrderingOrdered> createSmoother(int T, boost::optional<OrderingOrdered> ordering = boost::none);
/* ******************************************************* */
// Linear Constrained Examples
/* ******************************************************* */
/**
* Creates a simple constrained graph with one linear factor and
* one binary equality constraint that sets x = y
*/
GaussianFactorGraphOrdered createSimpleConstraintGraph();
VectorValuesOrdered createSimpleConstraintValues();
/**
* Creates a simple constrained graph with one linear factor and
* one binary constraint.
*/
GaussianFactorGraphOrdered createSingleConstraintGraph();
VectorValuesOrdered createSingleConstraintValues();
/**
* Creates a constrained graph with a linear factor and two
* binary constraints that share a node
*/
GaussianFactorGraphOrdered createMultiConstraintGraph();
VectorValuesOrdered createMultiConstraintValues();
/* ******************************************************* */
// Planar graph with easy subtree for SubgraphPreconditioner
/* ******************************************************* */
/*
* Create factor graph with N^2 nodes, for example for N=3
* x13-x23-x33
* | | |
* x12-x22-x32
* | | |
* -x11-x21-x31
* with x11 clamped at (1,1), and others related by 2D odometry.
*/
boost::tuple<GaussianFactorGraphOrdered, VectorValuesOrdered> planarGraph(size_t N);
/*
* Create canonical ordering for planar graph that also works for tree
* With x11 the root, e.g. for N=3
* x33 x23 x13 x32 x22 x12 x31 x21 x11
*/
OrderingOrdered planarOrdering(size_t N);
/*
* Split graph into tree and loop closing constraints, e.g., with N=3
* x13-x23-x33
* |
* x12-x22-x32
* |
* -x11-x21-x31
*/
std::pair<GaussianFactorGraphOrdered, GaussianFactorGraphOrdered > splitOffPlanarTree(
size_t N, const GaussianFactorGraphOrdered& original);
// Implementations
// using namespace gtsam::noiseModel;
namespace impl {
typedef boost::shared_ptr<NonlinearFactor> shared_nlf;
static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
static SharedDiagonal constraintModel = noiseModel::Constrained::All(2);
static const Index _l1_=0, _x1_=1, _x2_=2;
static const Index _x_=0, _y_=1, _z_=2;
} // \namespace impl
/* ************************************************************************* */
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
// Create
boost::shared_ptr<Graph> nlfg(
new Graph);
// prior on x1
Point2 mu;
shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1)));
nlfg->push_back(f1);
// odometry between x1 and x2
Point2 z2(1.5, 0);
shared_nlf f2(new simulated2D::Odometry(z2, sigma0_1, X(1), X(2)));
nlfg->push_back(f2);
// measurement between x1 and l1
Point2 z3(0, -1);
shared_nlf f3(new simulated2D::Measurement(z3, sigma0_2, X(1), L(1)));
nlfg->push_back(f3);
// measurement between x2 and l1
Point2 z4(-1.5, -1.);
shared_nlf f4(new simulated2D::Measurement(z4, sigma0_2, X(2), L(1)));
nlfg->push_back(f4);
return nlfg;
}
/* ************************************************************************* */
Graph createNonlinearFactorGraph() {
return *sharedNonlinearFactorGraph();
}
/* ************************************************************************* */
Values createValues() {
using symbol_shorthand::X;
using symbol_shorthand::L;
Values c;
c.insert(X(1), Point2(0.0, 0.0));
c.insert(X(2), Point2(1.5, 0.0));
c.insert(L(1), Point2(0.0, -1.0));
return c;
}
/* ************************************************************************* */
VectorValuesOrdered createVectorValues() {
using namespace impl;
VectorValuesOrdered c(std::vector<size_t>(3, 2));
c[_l1_] = Vector_(2, 0.0, -1.0);
c[_x1_] = Vector_(2, 0.0, 0.0);
c[_x2_] = Vector_(2, 1.5, 0.0);
return c;
}
/* ************************************************************************* */
boost::shared_ptr<const Values> sharedNoisyValues() {
using symbol_shorthand::X;
using symbol_shorthand::L;
boost::shared_ptr<Values> c(new Values);
c->insert(X(1), Point2(0.1, 0.1));
c->insert(X(2), Point2(1.4, 0.2));
c->insert(L(1), Point2(0.1, -1.1));
return c;
}
/* ************************************************************************* */
Values createNoisyValues() {
return *sharedNoisyValues();
}
/* ************************************************************************* */
VectorValuesOrdered createCorrectDelta(const OrderingOrdered& ordering) {
using symbol_shorthand::X;
using symbol_shorthand::L;
VectorValuesOrdered c(std::vector<size_t>(3,2));
c[ordering[L(1)]] = Vector_(2, -0.1, 0.1);
c[ordering[X(1)]] = Vector_(2, -0.1, -0.1);
c[ordering[X(2)]] = Vector_(2, 0.1, -0.2);
return c;
}
/* ************************************************************************* */
VectorValuesOrdered createZeroDelta(const OrderingOrdered& ordering) {
using symbol_shorthand::X;
using symbol_shorthand::L;
VectorValuesOrdered c(std::vector<size_t>(3,2));
c[ordering[L(1)]] = zero(2);
c[ordering[X(1)]] = zero(2);
c[ordering[X(2)]] = zero(2);
return c;
}
/* ************************************************************************* */
GaussianFactorGraphOrdered createGaussianFactorGraph(const OrderingOrdered& ordering) {
using symbol_shorthand::X;
using symbol_shorthand::L;
// Create empty graph
GaussianFactorGraphOrdered fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg.push_back(boost::make_shared<JacobianFactorOrdered>(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2));
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg.push_back(boost::make_shared<JacobianFactorOrdered>(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2));
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg.push_back(boost::make_shared<JacobianFactorOrdered>(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2));
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg.push_back(boost::make_shared<JacobianFactorOrdered>(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2));
return fg;
}
/* ************************************************************************* */
/** create small Chordal Bayes Net x <- y
* x y d
* 1 1 9
* 1 5
*/
GaussianBayesNetOrdered createSmallGaussianBayesNet() {
using namespace impl;
Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
Matrix R22 = Matrix_(1, 1, 1.0);
Vector d1(1), d2(1);
d1(0) = 9;
d2(0) = 5;
Vector tau(1);
tau(0) = 1.0;
// define nodes and specify in reverse topological sort (i.e. parents last)
GaussianConditionalOrdered::shared_ptr Px_y(new GaussianConditionalOrdered(_x_, d1, R11, _y_, S12, tau));
GaussianConditionalOrdered::shared_ptr Py(new GaussianConditionalOrdered(_y_, d2, R22, tau));
GaussianBayesNetOrdered cbn;
cbn.push_back(Px_y);
cbn.push_back(Py);
return cbn;
}
/* ************************************************************************* */
// Some nonlinear functions to optimize
/* ************************************************************************* */
namespace smallOptimize {
Point2 h(const Point2& v) {
return Point2(cos(v.x()), sin(v.y()));
}
Matrix H(const Point2& v) {
return Matrix_(2, 2,
-sin(v.x()), 0.0,
0.0, cos(v.y()));
}
struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> {
Point2 z_;
UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) :
gtsam::NoiseModelFactor1<Point2>(model, key), z_(z) {
}
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const {
if (A) *A = H(x);
return (h(x) - z_).vector();
}
};
}
/* ************************************************************************* */
boost::shared_ptr<const Graph> sharedReallyNonlinearFactorGraph() {
using symbol_shorthand::X;
using symbol_shorthand::L;
boost::shared_ptr<Graph> fg(new Graph);
Vector z = Vector_(2, 1.0, 0.0);
double sigma = 0.1;
boost::shared_ptr<smallOptimize::UnaryFactor> factor(
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
fg->push_back(factor);
return fg;
}
Graph createReallyNonlinearFactorGraph() {
return *sharedReallyNonlinearFactorGraph();
}
/* ************************************************************************* */
std::pair<Graph, Values> createNonlinearSmoother(int T) {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
// Create
Graph nlfg;
Values poses;
// prior on x1
Point2 x1(1.0, 0.0);
shared_nlf prior(new simulated2D::Prior(x1, sigma1_0, X(1)));
nlfg.push_back(prior);
poses.insert(X(1), x1);
for (int t = 2; t <= T; t++) {
// odometry between x_t and x_{t-1}
Point2 odo(1.0, 0.0);
shared_nlf odometry(new simulated2D::Odometry(odo, sigma1_0, X(t - 1), X(t)));
nlfg.push_back(odometry);
// measurement on x_t is like perfect GPS
Point2 xt(t, 0);
shared_nlf measurement(new simulated2D::Prior(xt, sigma1_0, X(t)));
nlfg.push_back(measurement);
// initial estimate
poses.insert(X(t), xt);
}
return std::make_pair(nlfg, poses);
}
/* ************************************************************************* */
std::pair<FactorGraphOrdered<GaussianFactorOrdered>, OrderingOrdered> createSmoother(int T, boost::optional<OrderingOrdered> ordering) {
Graph nlfg;
Values poses;
boost::tie(nlfg, poses) = createNonlinearSmoother(T);
if(!ordering) ordering = *poses.orderingArbitrary();
return std::make_pair(*nlfg.linearize(poses, *ordering), *ordering);
}
/* ************************************************************************* */
GaussianFactorGraphOrdered createSimpleConstraintGraph() {
using namespace impl;
// create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1
Matrix Ax = eye(2);
Vector b1(2);
b1(0) = 1.0;
b1(1) = -1.0;
JacobianFactorOrdered::shared_ptr f1(new JacobianFactorOrdered(_x_, Ax, b1, sigma0_1));
// create binary constraint factor
// between _x_ and _y_, that is going to be the only factor on _y_
// |1 0||x_1| + |-1 0||y_1| = |0|
// |0 1||x_2| | 0 -1||y_2| |0|
Matrix Ax1 = eye(2);
Matrix Ay1 = eye(2) * -1;
Vector b2 = Vector_(2, 0.0, 0.0);
JacobianFactorOrdered::shared_ptr f2(new JacobianFactorOrdered(_x_, Ax1, _y_, Ay1, b2,
constraintModel));
// construct the graph
GaussianFactorGraphOrdered fg;
fg.push_back(f1);
fg.push_back(f2);
return fg;
}
/* ************************************************************************* */
VectorValuesOrdered createSimpleConstraintValues() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
VectorValuesOrdered config(std::vector<size_t>(2,2));
Vector v = Vector_(2, 1.0, -1.0);
config[_x_] = v;
config[_y_] = v;
return config;
}
/* ************************************************************************* */
GaussianFactorGraphOrdered createSingleConstraintGraph() {
using namespace impl;
// create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1
Matrix Ax = eye(2);
Vector b1(2);
b1(0) = 1.0;
b1(1) = -1.0;
//GaussianFactorOrdered::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1));
JacobianFactorOrdered::shared_ptr f1(new JacobianFactorOrdered(_x_, Ax, b1, sigma0_1));
// create binary constraint factor
// between _x_ and _y_, that is going to be the only factor on _y_
// |1 2||x_1| + |10 0||y_1| = |1|
// |2 1||x_2| |0 10||y_2| |2|
Matrix Ax1(2, 2);
Ax1(0, 0) = 1.0;
Ax1(0, 1) = 2.0;
Ax1(1, 0) = 2.0;
Ax1(1, 1) = 1.0;
Matrix Ay1 = eye(2) * 10;
Vector b2 = Vector_(2, 1.0, 2.0);
JacobianFactorOrdered::shared_ptr f2(new JacobianFactorOrdered(_x_, Ax1, _y_, Ay1, b2,
constraintModel));
// construct the graph
GaussianFactorGraphOrdered fg;
fg.push_back(f1);
fg.push_back(f2);
return fg;
}
/* ************************************************************************* */
VectorValuesOrdered createSingleConstraintValues() {
using namespace impl;
VectorValuesOrdered config(std::vector<size_t>(2,2));
config[_x_] = Vector_(2, 1.0, -1.0);
config[_y_] = Vector_(2, 0.2, 0.1);
return config;
}
/* ************************************************************************* */
GaussianFactorGraphOrdered createMultiConstraintGraph() {
using namespace impl;
// unary factor 1
Matrix A = eye(2);
Vector b = Vector_(2, -2.0, 2.0);
JacobianFactorOrdered::shared_ptr lf1(new JacobianFactorOrdered(_x_, A, b, sigma0_1));
// constraint 1
Matrix A11(2, 2);
A11(0, 0) = 1.0;
A11(0, 1) = 2.0;
A11(1, 0) = 2.0;
A11(1, 1) = 1.0;
Matrix A12(2, 2);
A12(0, 0) = 10.0;
A12(0, 1) = 0.0;
A12(1, 0) = 0.0;
A12(1, 1) = 10.0;
Vector b1(2);
b1(0) = 1.0;
b1(1) = 2.0;
JacobianFactorOrdered::shared_ptr lc1(new JacobianFactorOrdered(_x_, A11, _y_, A12, b1,
constraintModel));
// constraint 2
Matrix A21(2, 2);
A21(0, 0) = 3.0;
A21(0, 1) = 4.0;
A21(1, 0) = -1.0;
A21(1, 1) = -2.0;
Matrix A22(2, 2);
A22(0, 0) = 1.0;
A22(0, 1) = 1.0;
A22(1, 0) = 1.0;
A22(1, 1) = 2.0;
Vector b2(2);
b2(0) = 3.0;
b2(1) = 4.0;
JacobianFactorOrdered::shared_ptr lc2(new JacobianFactorOrdered(_x_, A21, _z_, A22, b2,
constraintModel));
// construct the graph
GaussianFactorGraphOrdered fg;
fg.push_back(lf1);
fg.push_back(lc1);
fg.push_back(lc2);
return fg;
}
/* ************************************************************************* */
VectorValuesOrdered createMultiConstraintValues() {
using namespace impl;
VectorValuesOrdered config(std::vector<size_t>(3,2));
config[_x_] = Vector_(2, -2.0, 2.0);
config[_y_] = Vector_(2, -0.1, 0.4);
config[_z_] = Vector_(2, -4.0, 5.0);
return config;
}
/* ************************************************************************* */
// Create key for simulated planar graph
namespace impl {
Symbol key(int x, int y) {
using symbol_shorthand::X;
return X(1000*x+y);
}
} // \namespace impl
/* ************************************************************************* */
boost::tuple<GaussianFactorGraphOrdered, VectorValuesOrdered> planarGraph(size_t N) {
using namespace impl;
// create empty graph
NonlinearFactorGraph nlfg;
// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
shared_nlf constraint(new simulated2D::Prior(Point2(1.0, 1.0), noiseModel::Isotropic::Sigma(2,1e-3), key(1,1)));
nlfg.push_back(constraint);
// Create horizontal constraints, 1...N*(N-1)
Point2 z1(1.0, 0.0); // move right
for (size_t x = 1; x < N; x++)
for (size_t y = 1; y <= N; y++) {
shared_nlf f(new simulated2D::Odometry(z1, noiseModel::Isotropic::Sigma(2,0.01), key(x, y), key(x + 1, y)));
nlfg.push_back(f);
}
// Create vertical constraints, N*(N-1)+1..2*N*(N-1)
Point2 z2(0.0, 1.0); // move up
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y < N; y++) {
shared_nlf f(new simulated2D::Odometry(z2, noiseModel::Isotropic::Sigma(2,0.01), key(x, y), key(x, y + 1)));
nlfg.push_back(f);
}
// Create linearization and ground xtrue config
Values zeros;
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++)
zeros.insert(key(x, y), Point2());
OrderingOrdered ordering(planarOrdering(N));
VectorValuesOrdered xtrue(zeros.dims(ordering));
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++)
xtrue[ordering[key(x, y)]] = Point2(x,y).vector();
// linearize around zero
boost::shared_ptr<GaussianFactorGraphOrdered> gfg = nlfg.linearize(zeros, ordering);
return boost::make_tuple(*gfg, xtrue);
}
/* ************************************************************************* */
OrderingOrdered planarOrdering(size_t N) {
OrderingOrdered ordering;
for (size_t y = N; y >= 1; y--)
for (size_t x = N; x >= 1; x--)
ordering.push_back(impl::key(x, y));
return ordering;
}
/* ************************************************************************* */
std::pair<GaussianFactorGraphOrdered, GaussianFactorGraphOrdered > splitOffPlanarTree(size_t N,
const GaussianFactorGraphOrdered& original) {
GaussianFactorGraphOrdered T, C;
// Add the x11 constraint to the tree
T.push_back(original[0]);
// Add all horizontal constraints to the tree
size_t i = 1;
for (size_t x = 1; x < N; x++)
for (size_t y = 1; y <= N; y++, i++)
T.push_back(original[i]);
// Add first vertical column of constraints to T, others to C
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y < N; y++, i++)
if (x == 1)
T.push_back(original[i]);
else
C.push_back(original[i]);
return std::make_pair(T, C);
}
/* ************************************************************************* */
} // anonymous namespace
} // \namespace example
} // \namespace gtsam

33
tests/testGaussianISAM.cpp
View File

@ -16,14 +16,9 @@
*/ */
#include <tests/smallExample.h> #include <tests/smallExample.h>
#include <gtsam/nonlinear/OrderingOrdered.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianBayesNetOrdered.h> #include <gtsam/linear/GaussianISAM.h>
#include <gtsam/linear/GaussianISAMOrdered.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/inference/ISAMOrdered.h>
#include <gtsam/geometry/Rot2.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -44,29 +39,29 @@ using symbol_shorthand::L;
static const double tol = 1e-4; static const double tol = 1e-4;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ISAMOrdered, iSAM_smoother ) TEST( ISAM, iSAM_smoother )
{ {
OrderingOrdered ordering; Ordering ordering;
for (int t = 1; t <= 7; t++) ordering += X(t); for (int t = 1; t <= 7; t++) ordering += X(t);
// Create smoother with 7 nodes // Create smoother with 7 nodes
GaussianFactorGraphOrdered smoother = createSmoother(7, ordering).first; GaussianFactorGraph smoother = createSmoother(7, ordering).first;
// run iSAM for every factor // run iSAM for every factor
GaussianISAMOrdered actual; GaussianISAM actual;
BOOST_FOREACH(boost::shared_ptr<GaussianFactorOrdered> factor, smoother) { BOOST_FOREACH(boost::shared_ptr<GaussianFactor> factor, smoother) {
GaussianFactorGraphOrdered factorGraph; GaussianFactorGraph factorGraph;
factorGraph.push_back(factor); factorGraph.push_back(factor);
actual.update(factorGraph); actual.update(factorGraph);
} }
// Create expected Bayes Tree by solving smoother with "natural" ordering // Create expected Bayes Tree by solving smoother with "natural" ordering
BayesTreeOrdered<GaussianConditionalOrdered>::shared_ptr bayesTree = GaussianMultifrontalSolver(smoother).eliminate(); BayesTree<GaussianConditional>::shared_ptr bayesTree = GaussianMultifrontalSolver(smoother).eliminate();
GaussianISAMOrdered expected(*bayesTree); GaussianISAM expected(*bayesTree);
// Verify sigmas in the bayes tree // Verify sigmas in the bayes tree
BOOST_FOREACH(const GaussianBayesTreeOrdered::sharedClique& clique, bayesTree->nodes()) { BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, bayesTree->nodes()) {
GaussianConditionalOrdered::shared_ptr conditional = clique->conditional(); GaussianConditional::shared_ptr conditional = clique->conditional();
size_t dim = conditional->dim(); size_t dim = conditional->dim();
EXPECT(assert_equal(gtsam::ones(dim), conditional->get_sigmas(), tol)); EXPECT(assert_equal(gtsam::ones(dim), conditional->get_sigmas(), tol));
} }
@ -75,8 +70,8 @@ TEST( ISAMOrdered, iSAM_smoother )
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
// obtain solution // obtain solution
VectorValuesOrdered e(VectorValuesOrdered::Zero(7,2)); // expected solution VectorValues e(VectorValues::Zero(7,2)); // expected solution
VectorValuesOrdered optimized = optimize(actual); // actual solution VectorValues optimized = optimize(actual); // actual solution
EXPECT(assert_equal(e, optimized)); EXPECT(assert_equal(e, optimized));
} }

85
tests/testGaussianISAMOrdered.cpp Normal file
View File

@ -0,0 +1,85 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/**
* @file testGaussianISAM.cpp
* @brief Unit tests for GaussianISAM
* @author Michael Kaess
*/
#include <tests/smallExampleOrdered.h>
#include <gtsam/nonlinear/OrderingOrdered.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianBayesNetOrdered.h>
#include <gtsam/linear/GaussianISAMOrdered.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/inference/ISAMOrdered.h>
#include <gtsam/geometry/Rot2.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace example;
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */
// Some numbers that should be consistent among all smoother tests
static const double tol = 1e-4;
/* ************************************************************************* */
TEST( ISAMOrdered, iSAM_smoother )
{
OrderingOrdered ordering;
for (int t = 1; t <= 7; t++) ordering += X(t);
// Create smoother with 7 nodes
GaussianFactorGraphOrdered smoother = createSmoother(7, ordering).first;
// run iSAM for every factor
GaussianISAMOrdered actual;
BOOST_FOREACH(boost::shared_ptr<GaussianFactorOrdered> factor, smoother) {
GaussianFactorGraphOrdered factorGraph;
factorGraph.push_back(factor);
actual.update(factorGraph);
}
// Create expected Bayes Tree by solving smoother with "natural" ordering
BayesTreeOrdered<GaussianConditionalOrdered>::shared_ptr bayesTree = GaussianMultifrontalSolver(smoother).eliminate();
GaussianISAMOrdered expected(*bayesTree);
// Verify sigmas in the bayes tree
BOOST_FOREACH(const GaussianBayesTreeOrdered::sharedClique& clique, bayesTree->nodes()) {
GaussianConditionalOrdered::shared_ptr conditional = clique->conditional();
size_t dim = conditional->dim();
EXPECT(assert_equal(gtsam::ones(dim), conditional->get_sigmas(), tol));
}
// Check whether BayesTree is correct
EXPECT(assert_equal(expected, actual));
// obtain solution
VectorValuesOrdered e(VectorValuesOrdered::Zero(7,2)); // expected solution
VectorValuesOrdered optimized = optimize(actual); // actual solution
EXPECT(assert_equal(e, optimized));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */