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

Cleanup, whitespace

release/4.3a0
Alex Cunningham 2013-06-11 14:36:50 +00:00
parent 7b79cfc38c
commit 37f936d41c
1 changed files with 567 additions and 579 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

300
tests/smallExample.h
View File

@ -22,118 +22,106 @@
#pragma once #pragma once
#include <tests/simulated2D.h> #include <tests/simulated2D.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <gtsam/nonlinear/Symbol.h>
//#include <gtsam/nonlinear/Ordering.h>
//#include <gtsam/nonlinear/NonlinearFactorGraph.h>
//#include <gtsam/nonlinear/NonlinearFactor.h>
//#include <gtsam/inference/FactorGraph.h>
//#include <gtsam/base/Matrix.h>
//
//#include <boost/optional.hpp>
//#include <boost/shared_ptr.hpp>
//
//#include <iostream>
//#include <string>
namespace gtsam { namespace gtsam {
namespace example { namespace example {
typedef NonlinearFactorGraph Graph; typedef NonlinearFactorGraph Graph;
/** /**
* 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 Graph> sharedNonlinearFactorGraph();
Graph createNonlinearFactorGraph(); Graph createNonlinearFactorGraph();
/** /**
* Create values structure to go with it * Create values structure to go with it
* The ground truth values structure for the example above * The ground truth values structure for the example above
*/ */
Values createValues(); Values createValues();
/** Vector Values equivalent */ /** Vector Values equivalent */
VectorValues createVectorValues(); VectorValues createVectorValues();
/** /**
* create a noisy values structure for a nonlinear factor graph * create a noisy values structure for a nonlinear factor graph
*/ */
boost::shared_ptr<const Values> sharedNoisyValues(); boost::shared_ptr<const Values> sharedNoisyValues();
Values createNoisyValues(); Values createNoisyValues();
/** /**
* Zero delta config * Zero delta config
*/ */
VectorValues createZeroDelta(const Ordering& 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
*/ */
VectorValues createCorrectDelta(const Ordering& 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
*/ */
GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering); GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering);
/** /**
* create small Chordal Bayes Net x <- y * create small Chordal Bayes Net x <- y
*/ */
GaussianBayesNet 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 Graph>
sharedReallyNonlinearFactorGraph(); sharedReallyNonlinearFactorGraph();
Graph createReallyNonlinearFactorGraph(); Graph 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<Graph, 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<FactorGraph<GaussianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering = boost::none); std::pair<FactorGraph<GaussianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering = boost::none);
/* ******************************************************* */ /* ******************************************************* */
// Linear Constrained Examples // Linear Constrained Examples
/* ******************************************************* */ /* ******************************************************* */
/** /**
* 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
*/ */
GaussianFactorGraph createSimpleConstraintGraph(); GaussianFactorGraph createSimpleConstraintGraph();
VectorValues 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.
*/ */
GaussianFactorGraph createSingleConstraintGraph(); GaussianFactorGraph createSingleConstraintGraph();
VectorValues 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
*/ */
GaussianFactorGraph createMultiConstraintGraph(); GaussianFactorGraph createMultiConstraintGraph();
VectorValues createMultiConstraintValues(); VectorValues createMultiConstraintValues();
/* ******************************************************* */ /* ******************************************************* */
// Planar graph with easy subtree for SubgraphPreconditioner // Planar graph with easy subtree for SubgraphPreconditioner
/* ******************************************************* */ /* ******************************************************* */
/* /*
* Create factor graph with N^2 nodes, for example for N=3 * Create factor graph with N^2 nodes, for example for N=3
* x13-x23-x33 * x13-x23-x33
* | | | * | | |
@ -142,16 +130,16 @@ namespace gtsam {
* -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<GaussianFactorGraph, VectorValues> 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
*/ */
Ordering 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
* x13-x23-x33 * x13-x23-x33
* | * |
@ -159,10 +147,10 @@ namespace gtsam {
* | * |
* -x11-x21-x31 * -x11-x21-x31
*/ */
std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree( std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(
size_t N, const GaussianFactorGraph& original); size_t N, const GaussianFactorGraph& original);
} // example } // example
} // gtsam } // gtsam
@ -173,20 +161,20 @@ namespace example {
// using namespace gtsam::noiseModel; // using namespace gtsam::noiseModel;
namespace impl { namespace impl {
typedef boost::shared_ptr<NonlinearFactor> shared_nlf; typedef boost::shared_ptr<NonlinearFactor> shared_nlf;
static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); 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 Index _l1_=0, _x1_=1, _x2_=2;
static const Index _x_=0, _y_=1, _z_=2; static const Index _x_=0, _y_=1, _z_=2;
} // \namespace impl } // \namespace impl
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() { boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -215,15 +203,15 @@ namespace impl {
nlfg->push_back(f4); nlfg->push_back(f4);
return nlfg; return nlfg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Graph createNonlinearFactorGraph() { Graph createNonlinearFactorGraph() {
return *sharedNonlinearFactorGraph(); return *sharedNonlinearFactorGraph();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Values createValues() { Values createValues() {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -232,10 +220,10 @@ namespace impl {
c.insert(X(2), Point2(1.5, 0.0)); c.insert(X(2), Point2(1.5, 0.0));
c.insert(L(1), Point2(0.0, -1.0)); c.insert(L(1), Point2(0.0, -1.0));
return c; return c;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues createVectorValues() { VectorValues createVectorValues() {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -244,10 +232,10 @@ namespace impl {
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);
return c; return c;
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const Values> sharedNoisyValues() { boost::shared_ptr<const Values> sharedNoisyValues() {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -256,15 +244,15 @@ namespace impl {
c->insert(X(2), Point2(1.4, 0.2)); c->insert(X(2), Point2(1.4, 0.2));
c->insert(L(1), Point2(0.1, -1.1)); c->insert(L(1), Point2(0.1, -1.1));
return c; return c;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Values createNoisyValues() { Values createNoisyValues() {
return *sharedNoisyValues(); return *sharedNoisyValues();
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues createCorrectDelta(const Ordering& ordering) { VectorValues createCorrectDelta(const Ordering& ordering) {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -273,10 +261,10 @@ namespace impl {
c[ordering[X(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); c[ordering[X(2)]] = Vector_(2, 0.1, -0.2);
return c; return c;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues createZeroDelta(const Ordering& ordering) { VectorValues createZeroDelta(const Ordering& ordering) {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -285,10 +273,10 @@ namespace impl {
c[ordering[X(1)]] = zero(2); c[ordering[X(1)]] = zero(2);
c[ordering[X(2)]] = zero(2); c[ordering[X(2)]] = zero(2);
return c; return c;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -310,15 +298,15 @@ namespace impl {
fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2)); fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2));
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() {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -338,24 +326,24 @@ namespace impl {
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 {
Point2 h(const Point2& v) { Point2 h(const Point2& v) {
return Point2(cos(v.x()), sin(v.y())); return Point2(cos(v.x()), sin(v.y()));
} }
Matrix H(const Point2& v) { Matrix H(const Point2& v) {
return Matrix_(2, 2, return Matrix_(2, 2,
-sin(v.x()), 0.0, -sin(v.x()), 0.0,
0.0, cos(v.y())); 0.0, cos(v.y()));
} }
struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> { struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> {
Point2 z_; Point2 z_;
@ -368,12 +356,12 @@ namespace impl {
return (h(x) - z_).vector(); return (h(x) - z_).vector();
} }
}; };
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const Graph> sharedReallyNonlinearFactorGraph() { boost::shared_ptr<const Graph> sharedReallyNonlinearFactorGraph() {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -384,14 +372,14 @@ namespace impl {
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
fg->push_back(factor); fg->push_back(factor);
return fg; return fg;
} }
Graph createReallyNonlinearFactorGraph() { Graph createReallyNonlinearFactorGraph() {
return *sharedReallyNonlinearFactorGraph(); return *sharedReallyNonlinearFactorGraph();
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<Graph, Values> createNonlinearSmoother(int T) { std::pair<Graph, 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;
@ -422,20 +410,20 @@ namespace impl {
} }
return std::make_pair(nlfg, poses); return std::make_pair(nlfg, poses);
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<FactorGraph<GaussianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering) { std::pair<FactorGraph<GaussianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering) {
Graph nlfg; Graph nlfg;
Values poses; Values poses;
boost::tie(nlfg, poses) = createNonlinearSmoother(T); boost::tie(nlfg, poses) = createNonlinearSmoother(T);
if(!ordering) ordering = *poses.orderingArbitrary(); if(!ordering) ordering = *poses.orderingArbitrary();
return std::make_pair(*nlfg.linearize(poses, *ordering), *ordering); return std::make_pair(*nlfg.linearize(poses, *ordering), *ordering);
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph createSimpleConstraintGraph() { GaussianFactorGraph createSimpleConstraintGraph() {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -463,10 +451,10 @@ namespace impl {
fg.push_back(f2); fg.push_back(f2);
return fg; return fg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues 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;
@ -475,10 +463,10 @@ namespace impl {
config[_x_] = v; config[_x_] = v;
config[_y_] = v; config[_y_] = v;
return config; return config;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph createSingleConstraintGraph() { GaussianFactorGraph createSingleConstraintGraph() {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -511,10 +499,10 @@ namespace impl {
fg.push_back(f2); fg.push_back(f2);
return fg; return fg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues createSingleConstraintValues() { VectorValues createSingleConstraintValues() {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -522,10 +510,10 @@ namespace impl {
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;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph createMultiConstraintGraph() { GaussianFactorGraph createMultiConstraintGraph() {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -579,10 +567,10 @@ namespace impl {
fg.push_back(lc2); fg.push_back(lc2);
return fg; return fg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues createMultiConstraintValues() { VectorValues createMultiConstraintValues() {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -591,19 +579,19 @@ namespace impl {
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);
return config; return config;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Create key for simulated planar graph // Create key for simulated planar graph
namespace impl { namespace impl {
Symbol key(int x, int y) { Symbol key(int x, int y) {
using symbol_shorthand::X; using symbol_shorthand::X;
return X(1000*x+y); return X(1000*x+y);
} }
} // \namespace impl } // \namespace impl
/* ************************************************************************* */ /* ************************************************************************* */
boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) { boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
using namespace impl; using namespace impl;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -645,19 +633,19 @@ namespace impl {
// linearize around zero // linearize around zero
boost::shared_ptr<GaussianFactorGraph> 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);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Ordering planarOrdering(size_t N) { Ordering planarOrdering(size_t N) {
Ordering 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));
return ordering; return ordering;
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N, std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N,
const GaussianFactorGraph& original) { const GaussianFactorGraph& original) {
GaussianFactorGraph T, C; GaussianFactorGraph T, C;
@ -679,7 +667,7 @@ namespace impl {
C.push_back(original[i]); C.push_back(original[i]);
return std::make_pair(T, C); return std::make_pair(T, C);
} }
/* ************************************************************************* */ /* ************************************************************************* */