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

Updated smallExample

release/4.3a0
Richard Roberts 2013-08-05 22:31:21 +00:00
parent ac0f108106
commit 55ce9eed1d
1 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
tests/smallExample.h
View File

@ -24,6 +24,8 @@
#include <tests/simulated2D.h> #include <tests/simulated2D.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
namespace gtsam { namespace gtsam {
@ -56,18 +58,18 @@ Values createNoisyValues();
/** /**
* Zero delta config * Zero delta config
*/ */
VectorValues createZeroDelta(const Ordering& ordering); VectorValues createZeroDelta();
/** /**
* 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();
/** /**
* 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();
/** /**
* create small Chordal Bayes Net x <- y * create small Chordal Bayes Net x <- y
@ -91,7 +93,7 @@ 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<GaussianFactorGraph, Ordering> createSmoother(int T, boost::optional<Ordering> ordering = boost::none); GaussianFactorGraph createSmoother(int T, boost::optional<Ordering> ordering = boost::none);
/* ******************************************************* */ /* ******************************************************* */
// Linear Constrained Examples // Linear Constrained Examples
@ -398,13 +400,12 @@ std::pair<NonlinearFactorGraph, Values> createNonlinearSmoother(int T) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<GaussianFactorGraph, Ordering> createSmoother(int T) { GaussianFactorGraph createSmoother(int T) {
NonlinearFactorGraph nlfg; NonlinearFactorGraph nlfg;
Values poses; Values poses;
boost::tie(nlfg, poses) = createNonlinearSmoother(T); boost::tie(nlfg, poses) = createNonlinearSmoother(T);
if(!ordering) ordering = *poses.orderingArbitrary(); return *nlfg.linearize(poses);
return std::make_pair(*nlfg.linearize(poses, *ordering), *ordering);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -597,14 +598,13 @@ boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
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());
Ordering ordering(planarOrdering(N)); VectorValues xtrue;
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.insert(key(x, y), Point2(x,y).vector());
// linearize around zero // linearize around zero
boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros, ordering); boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros);
return boost::make_tuple(*gfg, xtrue); return boost::make_tuple(*gfg, xtrue);
} }