diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 7556ea629..5eca4bf84 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -181,5 +181,15 @@ Expression operator*(const Expression& expression1, expression1, expression2); } +/// Construct an array of leaves +template +std::vector > createUnknowns(size_t n, char c, size_t start = 0) { + std::vector > unknowns; + unknowns.reserve(n); + for (size_t i = start; i < start + n; i++) + unknowns.push_back(Expression(c, i)); + return unknowns; +} + } diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index ad738d50c..db6dcf367 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -34,7 +34,6 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ - template Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional Dcal, boost::optional Dp) { @@ -57,7 +56,7 @@ TEST(Expression, constant) { /* ************************************************************************* */ // Leaf -TEST(Expression, leaf) { +TEST(Expression, Leaf) { Expression R(100); Values values; values.insert(100, someR); @@ -74,8 +73,18 @@ TEST(Expression, leaf) { } /* ************************************************************************* */ +// Many Leaves +TEST(Expression, Leaves) { + Values values; + Point3 somePoint(1, 2, 3); + values.insert(Symbol('p', 10), somePoint); + std::vector > points = createUnknowns(10, 'p', 1); + EXPECT(assert_equal(somePoint,points.back().value(values))); +} -//TEST(Expression, nullaryMethod) { +/* ************************************************************************* */ + +//TEST(Expression, NullaryMethod) { // Expression p(67); // Expression norm(p, &Point3::norm); // Values values;