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

View File

@ -22,118 +22,106 @@
#pragma once
#include <tests/simulated2D.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#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 example {
namespace example {
typedef NonlinearFactorGraph Graph;
typedef NonlinearFactorGraph Graph;
/**
/**
* Create small example for non-linear factor graph
*/
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph();
Graph createNonlinearFactorGraph();
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();
Values createValues();
/** Vector Values equivalent */
VectorValues createVectorValues();
/** Vector Values equivalent */
VectorValues createVectorValues();
/**
/**
* create a noisy values structure for a nonlinear factor graph
*/
boost::shared_ptr<const Values> sharedNoisyValues();
Values createNoisyValues();
boost::shared_ptr<const Values> sharedNoisyValues();
Values createNoisyValues();
/**
/**
* Zero delta config
*/
VectorValues createZeroDelta(const Ordering& ordering);
VectorValues createZeroDelta(const Ordering& ordering);
/**
/**
* 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
* 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
*/
GaussianBayesNet createSmallGaussianBayesNet();
GaussianBayesNet createSmallGaussianBayesNet();
/**
/**
* Create really non-linear factor graph (cos/sin)
*/
boost::shared_ptr<const Graph>
sharedReallyNonlinearFactorGraph();
Graph createReallyNonlinearFactorGraph();
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);
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<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
* one binary equality constraint that sets x = y
*/
GaussianFactorGraph createSimpleConstraintGraph();
VectorValues createSimpleConstraintValues();
GaussianFactorGraph createSimpleConstraintGraph();
VectorValues createSimpleConstraintValues();
/**
/**
* Creates a simple constrained graph with one linear factor and
* one binary constraint.
*/
GaussianFactorGraph createSingleConstraintGraph();
VectorValues createSingleConstraintValues();
GaussianFactorGraph createSingleConstraintGraph();
VectorValues createSingleConstraintValues();
/**
/**
* Creates a constrained graph with a linear factor and two
* binary constraints that share a node
*/
GaussianFactorGraph createMultiConstraintGraph();
VectorValues createMultiConstraintValues();
GaussianFactorGraph createMultiConstraintGraph();
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
* x13-x23-x33
* | | |
@ -142,16 +130,16 @@ namespace gtsam {
* -x11-x21-x31
* 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
* With x11 the root, e.g. for N=3
* 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
* x13-x23-x33
* |
@ -159,10 +147,10 @@ namespace gtsam {
* |
* -x11-x21-x31
*/
std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(
std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(
size_t N, const GaussianFactorGraph& original);
} // example
} // example
} // gtsam
@ -173,20 +161,20 @@ namespace example {
// using namespace gtsam::noiseModel;
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 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 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;
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() {
/* ************************************************************************* */
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -215,15 +203,15 @@ namespace impl {
nlfg->push_back(f4);
return nlfg;
}
}
/* ************************************************************************* */
Graph createNonlinearFactorGraph() {
/* ************************************************************************* */
Graph createNonlinearFactorGraph() {
return *sharedNonlinearFactorGraph();
}
}
/* ************************************************************************* */
Values createValues() {
/* ************************************************************************* */
Values createValues() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -232,10 +220,10 @@ namespace impl {
c.insert(X(2), Point2(1.5, 0.0));
c.insert(L(1), Point2(0.0, -1.0));
return c;
}
}
/* ************************************************************************* */
VectorValues createVectorValues() {
/* ************************************************************************* */
VectorValues createVectorValues() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -244,10 +232,10 @@ namespace impl {
c[_x1_] = Vector_(2, 0.0, 0.0);
c[_x2_] = Vector_(2, 1.5, 0.0);
return c;
}
}
/* ************************************************************************* */
boost::shared_ptr<const Values> sharedNoisyValues() {
/* ************************************************************************* */
boost::shared_ptr<const Values> sharedNoisyValues() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -256,15 +244,15 @@ namespace impl {
c->insert(X(2), Point2(1.4, 0.2));
c->insert(L(1), Point2(0.1, -1.1));
return c;
}
}
/* ************************************************************************* */
Values createNoisyValues() {
/* ************************************************************************* */
Values createNoisyValues() {
return *sharedNoisyValues();
}
}
/* ************************************************************************* */
VectorValues createCorrectDelta(const Ordering& ordering) {
/* ************************************************************************* */
VectorValues createCorrectDelta(const Ordering& ordering) {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -273,10 +261,10 @@ namespace impl {
c[ordering[X(1)]] = Vector_(2, -0.1, -0.1);
c[ordering[X(2)]] = Vector_(2, 0.1, -0.2);
return c;
}
}
/* ************************************************************************* */
VectorValues createZeroDelta(const Ordering& ordering) {
/* ************************************************************************* */
VectorValues createZeroDelta(const Ordering& ordering) {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -285,10 +273,10 @@ namespace impl {
c[ordering[X(1)]] = zero(2);
c[ordering[X(2)]] = zero(2);
return c;
}
}
/* ************************************************************************* */
GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
/* ************************************************************************* */
GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
using namespace impl;
using symbol_shorthand::X;
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));
return fg;
}
}
/* ************************************************************************* */
/** create small Chordal Bayes Net x <- y
/* ************************************************************************* */
/** create small Chordal Bayes Net x <- y
* x y d
* 1 1 9
* 1 5
*/
GaussianBayesNet createSmallGaussianBayesNet() {
GaussianBayesNet createSmallGaussianBayesNet() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -338,24 +326,24 @@ namespace impl {
cbn.push_back(Py);
return cbn;
}
}
/* ************************************************************************* */
// Some nonlinear functions to optimize
/* ************************************************************************* */
namespace smallOptimize {
/* ************************************************************************* */
// Some nonlinear functions to optimize
/* ************************************************************************* */
namespace smallOptimize {
Point2 h(const Point2& v) {
Point2 h(const Point2& v) {
return Point2(cos(v.x()), sin(v.y()));
}
}
Matrix H(const Point2& v) {
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> {
struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> {
Point2 z_;
@ -368,12 +356,12 @@ namespace impl {
return (h(x) - z_).vector();
}
};
};
}
}
/* ************************************************************************* */
boost::shared_ptr<const Graph> sharedReallyNonlinearFactorGraph() {
/* ************************************************************************* */
boost::shared_ptr<const Graph> sharedReallyNonlinearFactorGraph() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -384,14 +372,14 @@ namespace impl {
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
fg->push_back(factor);
return fg;
}
}
Graph createReallyNonlinearFactorGraph() {
Graph createReallyNonlinearFactorGraph() {
return *sharedReallyNonlinearFactorGraph();
}
}
/* ************************************************************************* */
std::pair<Graph, Values> createNonlinearSmoother(int T) {
/* ************************************************************************* */
std::pair<Graph, Values> createNonlinearSmoother(int T) {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -422,20 +410,20 @@ namespace impl {
}
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;
Values poses;
boost::tie(nlfg, poses) = createNonlinearSmoother(T);
if(!ordering) ordering = *poses.orderingArbitrary();
return std::make_pair(*nlfg.linearize(poses, *ordering), *ordering);
}
}
/* ************************************************************************* */
GaussianFactorGraph createSimpleConstraintGraph() {
/* ************************************************************************* */
GaussianFactorGraph createSimpleConstraintGraph() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -463,10 +451,10 @@ namespace impl {
fg.push_back(f2);
return fg;
}
}
/* ************************************************************************* */
VectorValues createSimpleConstraintValues() {
/* ************************************************************************* */
VectorValues createSimpleConstraintValues() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -475,10 +463,10 @@ namespace impl {
config[_x_] = v;
config[_y_] = v;
return config;
}
}
/* ************************************************************************* */
GaussianFactorGraph createSingleConstraintGraph() {
/* ************************************************************************* */
GaussianFactorGraph createSingleConstraintGraph() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -511,10 +499,10 @@ namespace impl {
fg.push_back(f2);
return fg;
}
}
/* ************************************************************************* */
VectorValues createSingleConstraintValues() {
/* ************************************************************************* */
VectorValues createSingleConstraintValues() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -522,10 +510,10 @@ namespace impl {
config[_x_] = Vector_(2, 1.0, -1.0);
config[_y_] = Vector_(2, 0.2, 0.1);
return config;
}
}
/* ************************************************************************* */
GaussianFactorGraph createMultiConstraintGraph() {
/* ************************************************************************* */
GaussianFactorGraph createMultiConstraintGraph() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -579,10 +567,10 @@ namespace impl {
fg.push_back(lc2);
return fg;
}
}
/* ************************************************************************* */
VectorValues createMultiConstraintValues() {
/* ************************************************************************* */
VectorValues createMultiConstraintValues() {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -591,19 +579,19 @@ namespace impl {
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) {
/* ************************************************************************* */
// 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
}
} // \namespace impl
/* ************************************************************************* */
boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
/* ************************************************************************* */
boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
using namespace impl;
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -645,19 +633,19 @@ namespace impl {
// linearize around zero
boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros, ordering);
return boost::make_tuple(*gfg, xtrue);
}
}
/* ************************************************************************* */
Ordering planarOrdering(size_t N) {
/* ************************************************************************* */
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(impl::key(x, y));
return ordering;
}
}
/* ************************************************************************* */
std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N,
/* ************************************************************************* */
std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N,
const GaussianFactorGraph& original) {
GaussianFactorGraph T, C;
@ -679,7 +667,7 @@ namespace impl {
C.push_back(original[i]);
return std::make_pair(T, C);
}
}
/* ************************************************************************* */