diff --git a/tests/smallExample.h b/tests/smallExample.h index 05405f651..5e0006030 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -24,6 +24,8 @@ #include #include #include +#include +#include #include namespace gtsam { @@ -56,18 +58,18 @@ Values createNoisyValues(); /** * Zero delta config */ -VectorValues createZeroDelta(const Ordering& ordering); +VectorValues createZeroDelta(); /** * Delta config that, when added to noisyValues, returns the ground truth */ -VectorValues createCorrectDelta(const Ordering& ordering); +VectorValues createCorrectDelta(); /** * create a linear factor graph * The non-linear graph above evaluated at NoisyValues */ -GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering); +GaussianFactorGraph createGaussianFactorGraph(); /** * create small Chordal Bayes Net x <- y @@ -91,7 +93,7 @@ std::pair createNonlinearSmoother(int T); * Create a Kalman smoother by linearizing a non-linear factor graph * @param T number of time-steps */ -std::pair createSmoother(int T, boost::optional ordering = boost::none); +GaussianFactorGraph createSmoother(int T, boost::optional ordering = boost::none); /* ******************************************************* */ // Linear Constrained Examples @@ -398,13 +400,12 @@ std::pair createNonlinearSmoother(int T) { } /* ************************************************************************* */ -std::pair createSmoother(int T) { +GaussianFactorGraph createSmoother(int T) { NonlinearFactorGraph nlfg; Values poses; boost::tie(nlfg, poses) = createNonlinearSmoother(T); - if(!ordering) ordering = *poses.orderingArbitrary(); - return std::make_pair(*nlfg.linearize(poses, *ordering), *ordering); + return *nlfg.linearize(poses); } /* ************************************************************************* */ @@ -597,14 +598,13 @@ boost::tuple planarGraph(size_t N) { for (size_t x = 1; x <= N; x++) for (size_t y = 1; y <= N; y++) zeros.insert(key(x, y), Point2()); - Ordering ordering(planarOrdering(N)); - VectorValues xtrue(zeros.dims(ordering)); + VectorValues xtrue; 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(); + xtrue.insert(key(x, y), Point2(x,y).vector()); // linearize around zero - boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); + boost::shared_ptr gfg = nlfg.linearize(zeros); return boost::make_tuple(*gfg, xtrue); }