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

Added planar graph with easy subtree

release/4.3a0
Frank Dellaert 2009-12-31 12:55:51 +00:00
parent 07cc95e4c4
commit 730f4a546f
2 changed files with 578 additions and 432 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

556
cpp/smallExample.cpp
View File

@ -26,87 +26,95 @@ using namespace std;
namespace gtsam { namespace gtsam {
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared; typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedNonlinearFactorGraph() { boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedNonlinearFactorGraph() {
// Create // Create
boost::shared_ptr<ExampleNonlinearFactorGraph> nlfg(new ExampleNonlinearFactorGraph); boost::shared_ptr<ExampleNonlinearFactorGraph> nlfg(
new ExampleNonlinearFactorGraph);
// prior on x1 // prior on x1
double sigma1=0.1; double sigma1 = 0.1;
Vector mu = zero(2); Vector mu = zero(2);
shared f1(new Point2Prior(mu, sigma1, "x1")); shared f1(new Point2Prior(mu, sigma1, "x1"));
nlfg->push_back(f1); nlfg->push_back(f1);
// odometry between x1 and x2 // odometry between x1 and x2
double sigma2=0.1; double sigma2 = 0.1;
Vector z2(2); z2(0) = 1.5 ; z2(1) = 0; Vector z2(2);
z2(0) = 1.5;
z2(1) = 0;
shared f2(new Simulated2DOdometry(z2, sigma2, "x1", "x2")); shared f2(new Simulated2DOdometry(z2, sigma2, "x1", "x2"));
nlfg->push_back(f2); nlfg->push_back(f2);
// measurement between x1 and l1 // measurement between x1 and l1
double sigma3=0.2; double sigma3 = 0.2;
Vector z3(2); z3(0) = 0. ; z3(1) = -1.; Vector z3(2);
z3(0) = 0.;
z3(1) = -1.;
shared f3(new Simulated2DMeasurement(z3, sigma3, "x1", "l1")); shared f3(new Simulated2DMeasurement(z3, sigma3, "x1", "l1"));
nlfg->push_back(f3); nlfg->push_back(f3);
// measurement between x2 and l1 // measurement between x2 and l1
double sigma4=0.2; double sigma4 = 0.2;
Vector z4(2); z4(0)= -1.5 ; z4(1) = -1.; Vector z4(2);
z4(0) = -1.5;
z4(1) = -1.;
shared f4(new Simulated2DMeasurement(z4, sigma4, "x2", "l1")); shared f4(new Simulated2DMeasurement(z4, sigma4, "x2", "l1"));
nlfg->push_back(f4); nlfg->push_back(f4);
return nlfg; return nlfg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
ExampleNonlinearFactorGraph createNonlinearFactorGraph() { ExampleNonlinearFactorGraph createNonlinearFactorGraph() {
return *sharedNonlinearFactorGraph(); return *sharedNonlinearFactorGraph();
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig createConfig() { VectorConfig createConfig() {
VectorConfig c; VectorConfig c;
c.insert("x1", Vector_(2, 0.0, 0.0)); c.insert("x1", Vector_(2, 0.0, 0.0));
c.insert("x2", Vector_(2, 1.5, 0.0)); c.insert("x2", Vector_(2, 1.5, 0.0));
c.insert("l1", Vector_(2, 0.0,-1.0)); c.insert("l1", Vector_(2, 0.0, -1.0));
return c; return c;
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const VectorConfig> sharedNoisyConfig() { boost::shared_ptr<const VectorConfig> sharedNoisyConfig() {
boost::shared_ptr<VectorConfig> c(new VectorConfig); boost::shared_ptr<VectorConfig> c(new VectorConfig);
c->insert("x1", Vector_(2, 0.1, 0.1)); c->insert("x1", Vector_(2, 0.1, 0.1));
c->insert("x2", Vector_(2, 1.4, 0.2)); c->insert("x2", Vector_(2, 1.4, 0.2));
c->insert("l1", Vector_(2, 0.1,-1.1)); c->insert("l1", Vector_(2, 0.1, -1.1));
return c; return c;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig createNoisyConfig() { return *sharedNoisyConfig();} VectorConfig createNoisyConfig() {
return *sharedNoisyConfig();
}
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig createCorrectDelta() { VectorConfig createCorrectDelta() {
VectorConfig c; VectorConfig c;
c.insert("x1", Vector_(2,-0.1,-0.1)); c.insert("x1", Vector_(2, -0.1, -0.1));
c.insert("x2", Vector_(2, 0.1,-0.2)); c.insert("x2", Vector_(2, 0.1, -0.2));
c.insert("l1", Vector_(2,-0.1, 0.1)); c.insert("l1", Vector_(2, -0.1, 0.1));
return c; return c;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig createZeroDelta() { VectorConfig createZeroDelta() {
VectorConfig c; VectorConfig c;
c.insert("x1", zero(2)); c.insert("x1", zero(2));
c.insert("x2", zero(2)); c.insert("x2", zero(2));
c.insert("l1", zero(2)); c.insert("l1", zero(2));
return c; return c;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph createGaussianFactorGraph() GaussianFactorGraph createGaussianFactorGraph() {
{
Matrix I = eye(2); Matrix I = eye(2);
VectorConfig c = createNoisyConfig(); VectorConfig c = createNoisyConfig();
@ -115,84 +123,86 @@ GaussianFactorGraph createGaussianFactorGraph()
// 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"]
double sigma1 = 0.1; double sigma1 = 0.1;
Vector b1 = - c["x1"]; Vector b1 = -c["x1"];
fg.add("x1", I, b1, sigma1); fg.add("x1", I, b1, sigma1);
// odometry between x1 and x2: x2-x1=[0.2;-0.1] // odometry between x1 and x2: x2-x1=[0.2;-0.1]
double sigma2 = 0.1; double sigma2 = 0.1;
Vector b2 = Vector_(2,0.2,-0.1); Vector b2 = Vector_(2, 0.2, -0.1);
fg.add("x1", -I, "x2", I, b2, sigma2); fg.add("x1", -I, "x2", I, b2, sigma2);
// measurement between x1 and l1: l1-x1=[0.0;0.2] // measurement between x1 and l1: l1-x1=[0.0;0.2]
double sigma3 = 0.2; double sigma3 = 0.2;
Vector b3 = Vector_(2,0.0,0.2); Vector b3 = Vector_(2, 0.0, 0.2);
fg.add("x1", -I, "l1", I, b3, sigma3); fg.add("x1", -I, "l1", I, b3, sigma3);
// measurement between x2 and l1: l1-x2=[-0.2;0.3] // measurement between x2 and l1: l1-x2=[-0.2;0.3]
double sigma4 = 0.2; double sigma4 = 0.2;
Vector b4 = Vector_(2,-0.2,0.3); Vector b4 = Vector_(2, -0.2, 0.3);
fg.add("x2", -I, "l1", I, b4, sigma4); fg.add("x2", -I, "l1", I, b4, sigma4);
return fg; return fg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
/** create small Chordal Bayes Net x <- y /** create small Chordal Bayes Net x <- y
* x y d * x y d
* 1 1 9 * 1 1 9
* 1 5 * 1 5
*/ */
GaussianBayesNet createSmallGaussianBayesNet() GaussianBayesNet createSmallGaussianBayesNet() {
{ 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; d2(0) = 5; d1(0) = 9;
Vector tau(1); tau(0) = 1.0; 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)
GaussianConditional::shared_ptr GaussianConditional::shared_ptr Px_y(new GaussianConditional("x", d1, R11,
Px_y(new GaussianConditional("x",d1,R11,"y",S12,tau)), "y", S12, tau)), Py(new GaussianConditional("y", d2, R22, tau));
Py(new GaussianConditional("y",d2,R22,tau));
GaussianBayesNet cbn; GaussianBayesNet cbn;
cbn.push_back(Px_y); cbn.push_back(Px_y);
cbn.push_back(Py); cbn.push_back(Py);
return cbn; return cbn;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Some nonlinear functions to optimize // Some nonlinear functions to optimize
/* ************************************************************************* */ /* ************************************************************************* */
namespace smallOptimize { namespace smallOptimize {
Vector h(const Vector& v) { Vector h(const Vector& v) {
double x = v(0); double x = v(0);
return Vector_(2,cos(x),sin(x)); return Vector_(2, cos(x), sin(x));
}; }
;
Matrix H(const Vector& v) { Matrix H(const Vector& v) {
double x = v(0); double x = v(0);
return Matrix_(2,1,-sin(x),cos(x)); return Matrix_(2, 1, -sin(x), cos(x));
}; }
} ;
}
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph() boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph() {
{ boost::shared_ptr<ExampleNonlinearFactorGraph> fg(
boost::shared_ptr<ExampleNonlinearFactorGraph> fg(new ExampleNonlinearFactorGraph); new ExampleNonlinearFactorGraph);
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<NonlinearFactor1> boost::shared_ptr<NonlinearFactor1> factor(new NonlinearFactor1(z, sigma,
factor(new NonlinearFactor1(z,sigma,&smallOptimize::h,"x",&smallOptimize::H)); &smallOptimize::h, "x", &smallOptimize::H));
fg->push_back(factor); fg->push_back(factor);
return fg; return fg;
} }
ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph() { ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph() {
return *sharedReallyNonlinearFactorGraph(); return *sharedReallyNonlinearFactorGraph();
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<ExampleNonlinearFactorGraph, VectorConfig> createNonlinearSmoother(int T) { pair<ExampleNonlinearFactorGraph, VectorConfig> createNonlinearSmoother(int T) {
// noise on measurements and odometry, respectively // noise on measurements and odometry, respectively
double sigma1 = 1, sigma2 = 1; double sigma1 = 1, sigma2 = 1;
@ -202,7 +212,7 @@ pair<ExampleNonlinearFactorGraph, VectorConfig> createNonlinearSmoother(int T) {
VectorConfig poses; VectorConfig poses;
// prior on x1 // prior on x1
Vector x1 = Vector_(2,1.0,0.0); Vector x1 = Vector_(2, 1.0, 0.0);
string key1 = symbol('x', 1); string key1 = symbol('x', 1);
shared prior(new Point2Prior(x1, sigma1, key1)); shared prior(new Point2Prior(x1, sigma1, key1));
nlfg.push_back(prior); nlfg.push_back(prior);
@ -212,11 +222,12 @@ pair<ExampleNonlinearFactorGraph, VectorConfig> createNonlinearSmoother(int T) {
// odometry between x_t and x_{t-1} // odometry between x_t and x_{t-1}
Vector odo = Vector_(2, 1.0, 0.0); Vector odo = Vector_(2, 1.0, 0.0);
string key = symbol('x', t); string key = symbol('x', t);
shared odometry(new Simulated2DOdometry(odo, sigma2, symbol('x', t - 1), key)); shared odometry(new Simulated2DOdometry(odo, sigma2, symbol('x', t - 1),
key));
nlfg.push_back(odometry); nlfg.push_back(odometry);
// measurement on x_t is like perfect GPS // measurement on x_t is like perfect GPS
Vector xt = Vector_(2, (double)t, 0.0); Vector xt = Vector_(2, (double) t, 0.0);
shared measurement(new Point2Prior(xt, sigma1, key)); shared measurement(new Point2Prior(xt, sigma1, key));
nlfg.push_back(measurement); nlfg.push_back(measurement);
@ -225,20 +236,20 @@ pair<ExampleNonlinearFactorGraph, VectorConfig> createNonlinearSmoother(int T) {
} }
return make_pair(nlfg, poses); return make_pair(nlfg, poses);
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph createSmoother(int T) { GaussianFactorGraph createSmoother(int T) {
ExampleNonlinearFactorGraph nlfg; ExampleNonlinearFactorGraph nlfg;
VectorConfig poses; VectorConfig poses;
boost::tie(nlfg, poses) = createNonlinearSmoother(T); boost::tie(nlfg, poses) = createNonlinearSmoother(T);
GaussianFactorGraph lfg = nlfg.linearize(poses); GaussianFactorGraph lfg = nlfg.linearize(poses);
return lfg; return lfg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph createSimpleConstraintGraph() { GaussianFactorGraph createSimpleConstraintGraph() {
// create unary factor // create unary factor
// prior on "x", mean = [1,-1], sigma=0.1 // prior on "x", mean = [1,-1], sigma=0.1
double sigma = 0.1; double sigma = 0.1;
@ -255,8 +266,8 @@ GaussianFactorGraph 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);
GaussianFactor::shared_ptr f2( GaussianFactor::shared_ptr f2(new GaussianFactor("x", Ax1, "y", Ay1, b2,
new GaussianFactor("x", Ax1, "y", Ay1, b2, 0.0)); 0.0));
// construct the graph // construct the graph
GaussianFactorGraph fg; GaussianFactorGraph fg;
@ -264,19 +275,19 @@ GaussianFactorGraph createSimpleConstraintGraph() {
fg.push_back(f2); fg.push_back(f2);
return fg; return fg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig createSimpleConstraintConfig() { VectorConfig createSimpleConstraintConfig() {
VectorConfig config; VectorConfig config;
Vector v = Vector_(2, 1.0, -1.0); Vector v = Vector_(2, 1.0, -1.0);
config.insert("x", v); config.insert("x", v);
config.insert("y", v); config.insert("y", v);
return config; return config;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph createSingleConstraintGraph() { GaussianFactorGraph createSingleConstraintGraph() {
// create unary factor // create unary factor
// prior on "x", mean = [1,-1], sigma=0.1 // prior on "x", mean = [1,-1], sigma=0.1
double sigma = 0.1; double sigma = 0.1;
@ -291,12 +302,14 @@ GaussianFactorGraph createSingleConstraintGraph() {
// |1 2||x_1| + |10 0||y_1| = |1| // |1 2||x_1| + |10 0||y_1| = |1|
// |2 1||x_2| |0 10||y_2| |2| // |2 1||x_2| |0 10||y_2| |2|
Matrix Ax1(2, 2); Matrix Ax1(2, 2);
Ax1(0, 0) = 1.0; Ax1(0, 1) = 2.0; Ax1(0, 0) = 1.0;
Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0; Ax1(0, 1) = 2.0;
Ax1(1, 0) = 2.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);
GaussianFactor::shared_ptr f2( GaussianFactor::shared_ptr f2(new GaussianFactor("x", Ax1, "y", Ay1, b2,
new GaussianFactor("x", Ax1, "y", Ay1, b2, 0.0)); 0.0));
// construct the graph // construct the graph
GaussianFactorGraph fg; GaussianFactorGraph fg;
@ -304,18 +317,18 @@ GaussianFactorGraph createSingleConstraintGraph() {
fg.push_back(f2); fg.push_back(f2);
return fg; return fg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig createSingleConstraintConfig() { VectorConfig createSingleConstraintConfig() {
VectorConfig config; VectorConfig config;
config.insert("x", Vector_(2, 1.0, -1.0)); config.insert("x", Vector_(2, 1.0, -1.0));
config.insert("y", Vector_(2, 0.2, 0.1)); config.insert("y", Vector_(2, 0.2, 0.1));
return config; return config;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph createMultiConstraintGraph() { GaussianFactorGraph createMultiConstraintGraph() {
// unary factor 1 // unary factor 1
double sigma = 0.1; double sigma = 0.1;
Matrix A = eye(2); Matrix A = eye(2);
@ -323,30 +336,42 @@ GaussianFactorGraph createMultiConstraintGraph() {
GaussianFactor::shared_ptr lf1(new GaussianFactor("x", A, b, sigma)); GaussianFactor::shared_ptr lf1(new GaussianFactor("x", A, b, sigma));
// constraint 1 // constraint 1
Matrix A11(2,2); Matrix A11(2, 2);
A11(0,0) = 1.0 ; A11(0,1) = 2.0; A11(0, 0) = 1.0;
A11(1,0) = 2.0 ; A11(1,1) = 1.0; A11(0, 1) = 2.0;
A11(1, 0) = 2.0;
A11(1, 1) = 1.0;
Matrix A12(2,2); Matrix A12(2, 2);
A12(0,0) = 10.0 ; A12(0,1) = 0.0; A12(0, 0) = 10.0;
A12(1,0) = 0.0 ; A12(1,1) = 10.0; A12(0, 1) = 0.0;
A12(1, 0) = 0.0;
A12(1, 1) = 10.0;
Vector b1(2); Vector b1(2);
b1(0) = 1.0; b1(1) = 2.0; b1(0) = 1.0;
GaussianFactor::shared_ptr lc1(new GaussianFactor("x", A11, "y", A12, b1, 0.0)); b1(1) = 2.0;
GaussianFactor::shared_ptr lc1(new GaussianFactor("x", A11, "y", A12, b1,
0.0));
// constraint 2 // constraint 2
Matrix A21(2,2); Matrix A21(2, 2);
A21(0,0) = 3.0 ; A21(0,1) = 4.0; A21(0, 0) = 3.0;
A21(1,0) = -1.0 ; A21(1,1) = -2.0; A21(0, 1) = 4.0;
A21(1, 0) = -1.0;
A21(1, 1) = -2.0;
Matrix A22(2,2); Matrix A22(2, 2);
A22(0,0) = 1.0 ; A22(0,1) = 1.0; A22(0, 0) = 1.0;
A22(1,0) = 1.0 ; A22(1,1) = 2.0; A22(0, 1) = 1.0;
A22(1, 0) = 1.0;
A22(1, 1) = 2.0;
Vector b2(2); Vector b2(2);
b2(0) = 3.0; b2(1) = 4.0; b2(0) = 3.0;
GaussianFactor::shared_ptr lc2(new GaussianFactor("x", A21, "z", A22, b2, 0.0)); b2(1) = 4.0;
GaussianFactor::shared_ptr lc2(new GaussianFactor("x", A21, "z", A22, b2,
0.0));
// construct the graph // construct the graph
GaussianFactorGraph fg; GaussianFactorGraph fg;
@ -355,117 +380,204 @@ GaussianFactorGraph createMultiConstraintGraph() {
fg.push_back(lc2); fg.push_back(lc2);
return fg; return fg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig createMultiConstraintConfig() { VectorConfig createMultiConstraintConfig() {
VectorConfig config; VectorConfig config;
config.insert("x", Vector_(2, -2.0, 2.0)); config.insert("x", Vector_(2, -2.0, 2.0));
config.insert("y", Vector_(2, -0.1, 0.4)); config.insert("y", Vector_(2, -0.1, 0.4));
config.insert("z", Vector_(2, -4.0, 5.0)); config.insert("z", Vector_(2, -4.0, 5.0));
return config; return config;
} }
/* ************************************************************************* */
//GaussianFactorGraph createConstrainedGaussianFactorGraph()
//{
// GaussianFactorGraph graph;
//
// // add an equality factor
// Vector v1(2); v1(0)=1.;v1(1)=2.;
// GaussianFactor::shared_ptr f1(new GaussianFactor(v1, "x0"));
// graph.push_back_eq(f1);
//
// // add a normal linear factor
// Matrix A21 = -1 * eye(2);
//
// Matrix A22 = eye(2);
//
// Vector b(2);
// b(0) = 2 ; b(1) = 3;
//
// double sigma = 0.1;
// GaussianFactor::shared_ptr f2(new GaussianFactor("x0", A21/sigma, "x1", A22/sigma, b/sigma));
// graph.push_back(f2);
// return graph;
//}
/* ************************************************************************* */
// ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig> , VectorConfig> createConstrainedNonlinearFactorGraph() {
// ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig> , VectorConfig> graph;
// VectorConfig c = createConstrainedConfig();
//
// // equality constraint for initial pose
// GaussianFactor::shared_ptr f1(new GaussianFactor(c["x0"], "x0"));
// graph.push_back_eq(f1);
//
// // odometry between x0 and x1
// double sigma = 0.1;
// shared f2(new Simulated2DOdometry(c["x1"] - c["x0"], sigma, "x0", "x1"));
// graph.push_back(f2); // TODO
// return graph;
// }
/* ************************************************************************* */
//VectorConfig createConstrainedConfig()
//{
// VectorConfig config;
//
// Vector x0(2); x0(0)=1.0; x0(1)=2.0;
// config.insert("x0", x0);
//
// Vector x1(2); x1(0)=3.0; x1(1)=5.0;
// config.insert("x1", x1);
//
// return config;
//}
/* ************************************************************************* */
//VectorConfig createConstrainedLinConfig()
//{
// VectorConfig config;
//
// Vector x0(2); x0(0)=1.0; x0(1)=2.0; // value doesn't actually matter
// config.insert("x0", x0);
//
// Vector x1(2); x1(0)=2.3; x1(1)=5.3;
// config.insert("x1", x1);
//
// return config;
//}
/* ************************************************************************* */
//VectorConfig createConstrainedCorrectDelta()
//{
// VectorConfig config;
//
// Vector x0(2); x0(0)=0.; x0(1)=0.;
// config.insert("x0", x0);
//
// Vector x1(2); x1(0)= 0.7; x1(1)= -0.3;
// config.insert("x1", x1);
//
// return config;
//}
/* ************************************************************************* */
//ConstrainedGaussianBayesNet createConstrainedGaussianBayesNet()
//{
// ConstrainedGaussianBayesNet cbn;
// VectorConfig c = createConstrainedConfig();
//
// // add regular conditional gaussian - no parent
// Matrix R = eye(2);
// Vector d = c["x1"];
// double sigma = 0.1;
// GaussianConditional::shared_ptr f1(new GaussianConditional(d/sigma, R/sigma));
// cbn.insert("x1", f1);
//
// // add a delta function to the cbn
// ConstrainedGaussianConditional::shared_ptr f2(new ConstrainedGaussianConditional); //(c["x0"], "x0"));
// cbn.insert_df("x0", f2);
//
// return cbn;
//}
/* ************************************************************************* */
// Create key for simulated planar graph
string key(int x, int y) {
stringstream ss;
ss << "x" << x << y;
return ss.str();
}
/* ************************************************************************* */
pair<GaussianFactorGraph, VectorConfig> planarGraph(size_t N) {
// create empty graph
NonlinearFactorGraph<VectorConfig> nlfg;
// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
double sigma0 = 1e-3;
shared constraint(new Point2Prior(Vector_(2, 1.0, 1.0), sigma0, "x11"));
nlfg.push_back(constraint);
double sigma = 0.01;
// Create horizontal constraints, 1...N*(N-1)
Vector z1 = Vector_(2, 1.0, 0.0); // move right
for (size_t x = 1; x < N; x++)
for (size_t y = 1; y <= N; y++) {
shared f(new Simulated2DOdometry(z1, sigma, key(x, y), key(x + 1, y)));
nlfg.push_back(f);
}
// Create vertical constraints, N*(N-1)+1..2*N*(N-1)
Vector z2 = Vector_(2, 0.0, 1.0); // move up
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y < N; y++) {
shared f(new Simulated2DOdometry(z2, sigma, key(x, y), key(x, y + 1)));
nlfg.push_back(f);
}
// Create linearization and ground xtrue config
VectorConfig zeros, xtrue;
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++) {
zeros.add(key(x, y), zero(2));
xtrue.add(key(x, y), Vector_(2, (double) x, double(y)));
}
// linearize around zero
GaussianFactorGraph A = nlfg.linearize(zeros);
return make_pair(A, xtrue);
}
/* ************************************************************************* */
Ordering planarOrdering(size_t N) {
Ordering ordering;
for (size_t y = N; y >= 1; y--)
for (size_t x = N; x >= 1; x--)
ordering.push_back(key(x, y));
return ordering;
}
/* ************************************************************************* */
pair<GaussianFactorGraph, GaussianFactorGraph> splitOffPlanarTree(size_t N,
const GaussianFactorGraph& original) {
GaussianFactorGraph 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 make_pair(T, C);
}
/* ************************************************************************* */ /* ************************************************************************* */
//GaussianFactorGraph createConstrainedGaussianFactorGraph()
//{
// GaussianFactorGraph graph;
//
// // add an equality factor
// Vector v1(2); v1(0)=1.;v1(1)=2.;
// GaussianFactor::shared_ptr f1(new GaussianFactor(v1, "x0"));
// graph.push_back_eq(f1);
//
// // add a normal linear factor
// Matrix A21 = -1 * eye(2);
//
// Matrix A22 = eye(2);
//
// Vector b(2);
// b(0) = 2 ; b(1) = 3;
//
// double sigma = 0.1;
// GaussianFactor::shared_ptr f2(new GaussianFactor("x0", A21/sigma, "x1", A22/sigma, b/sigma));
// graph.push_back(f2);
// return graph;
//}
/* ************************************************************************* */
// ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig> , VectorConfig> createConstrainedNonlinearFactorGraph() {
// ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig> , VectorConfig> graph;
// VectorConfig c = createConstrainedConfig();
//
// // equality constraint for initial pose
// GaussianFactor::shared_ptr f1(new GaussianFactor(c["x0"], "x0"));
// graph.push_back_eq(f1);
//
// // odometry between x0 and x1
// double sigma = 0.1;
// shared f2(new Simulated2DOdometry(c["x1"] - c["x0"], sigma, "x0", "x1"));
// graph.push_back(f2); // TODO
// return graph;
// }
/* ************************************************************************* */
//VectorConfig createConstrainedConfig()
//{
// VectorConfig config;
//
// Vector x0(2); x0(0)=1.0; x0(1)=2.0;
// config.insert("x0", x0);
//
// Vector x1(2); x1(0)=3.0; x1(1)=5.0;
// config.insert("x1", x1);
//
// return config;
//}
/* ************************************************************************* */
//VectorConfig createConstrainedLinConfig()
//{
// VectorConfig config;
//
// Vector x0(2); x0(0)=1.0; x0(1)=2.0; // value doesn't actually matter
// config.insert("x0", x0);
//
// Vector x1(2); x1(0)=2.3; x1(1)=5.3;
// config.insert("x1", x1);
//
// return config;
//}
/* ************************************************************************* */
//VectorConfig createConstrainedCorrectDelta()
//{
// VectorConfig config;
//
// Vector x0(2); x0(0)=0.; x0(1)=0.;
// config.insert("x0", x0);
//
// Vector x1(2); x1(0)= 0.7; x1(1)= -0.3;
// config.insert("x1", x1);
//
// return config;
//}
/* ************************************************************************* */
//ConstrainedGaussianBayesNet createConstrainedGaussianBayesNet()
//{
// ConstrainedGaussianBayesNet cbn;
// VectorConfig c = createConstrainedConfig();
//
// // add regular conditional gaussian - no parent
// Matrix R = eye(2);
// Vector d = c["x1"];
// double sigma = 0.1;
// GaussianConditional::shared_ptr f1(new GaussianConditional(d/sigma, R/sigma));
// cbn.insert("x1", f1);
//
// // add a delta function to the cbn
// ConstrainedGaussianConditional::shared_ptr f2(new ConstrainedGaussianConditional); //(c["x0"], "x0"));
// cbn.insert_df("x0", f2);
//
// return cbn;
//}
} // namespace gtsam } // namespace gtsam

36
cpp/smallExample.h
View File

@ -130,4 +130,38 @@ namespace gtsam {
*/ */
// ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig>,VectorConfig> // ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig>,VectorConfig>
// createConstrainedNonlinearFactorGraph(); // createConstrainedNonlinearFactorGraph();
}
/* ******************************************************* */
// 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.
*/
std::pair<GaussianFactorGraph, VectorConfig> 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
*/
Ordering 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<GaussianFactorGraph, GaussianFactorGraph> splitOffPlanarTree(size_t N,
const GaussianFactorGraph& original);
} // gtsam