From 1a00d7e3d9ae510b43cc0a013a99620f045e3da6 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Sat, 27 Sep 2014 11:39:46 +0200 Subject: [PATCH] Second draft of the BAD implementation --- gtsam_unstable/base/tests/testBAD.cpp | 284 +++++++++++++++++--------- 1 file changed, 185 insertions(+), 99 deletions(-) diff --git a/gtsam_unstable/base/tests/testBAD.cpp b/gtsam_unstable/base/tests/testBAD.cpp index 4515660a8..24cfdacea 100644 --- a/gtsam_unstable/base/tests/testBAD.cpp +++ b/gtsam_unstable/base/tests/testBAD.cpp @@ -30,154 +30,238 @@ namespace gtsam { -//----------------------------------------------------------------------------- +///----------------------------------------------------------------------------- +/// Expression node. The superclass for objects that do the heavy lifting +/// An Expression has a pointer to an ExpressionNode underneath +/// allowing Expressions to have polymorphic behaviour even though they +/// are passed by value. This is the same way boost::function works. +/// http://loki-lib.sourceforge.net/html/a00652.html +template +class ExpressionNode { + public: + ExpressionNode(){} + virtual ~ExpressionNode(){} + virtual void getKeys(std::set& keys) const = 0; + virtual T value(const Values& values, + boost::optional&> = boost::none) const = 0; + virtual ExpressionNode* clone() const = 0; +}; + /// Constant Expression template -class ConstantExpression { +class ConstantExpression : public ExpressionNode { T value_; -public: + public: typedef T type; /// Constructor with a value, yielding a constant ConstantExpression(const T& value) : - value_(value) { + value_(value) { } + virtual ~ConstantExpression(){} - /// The value is just the stored constant - T value(const Values& values) const { + virtual void getKeys(std::set& /* keys */) const {} + virtual T value(const Values& values, + boost::optional&> jacobians = boost::none) const { return value_; } - - /// A constant does not have a Jacobian - std::map jacobians(const Values& values) const { - std::map terms; - return terms; - } + virtual ExpressionNode* clone() const { return new ConstantExpression(*this); } }; //----------------------------------------------------------------------------- /// Leaf Expression template -class LeafExpression { +class LeafExpression : public ExpressionNode { Key key_; -public: + public: typedef T type; /// Constructor with a single key LeafExpression(Key key) : - key_(key) { + key_(key) { } + virtual ~LeafExpression(){} - /// The value of a leaf is just a lookup in values - T value(const Values& values) const { - return values.at(key_); - } - - /// There is only a single identity Jacobian in a leaf - std::map jacobians(const Values& values) const { - std::map terms; + virtual void getKeys(std::set& keys) const { keys.insert(key_); } + virtual T value(const Values& values, + boost::optional&> jacobians = boost::none) const { const T& value = values.at(key_); - terms[key_] = eye(value.dim()); - return terms; + if( jacobians ) { + std::map::iterator it = jacobians->find(key_); + if(it != jacobians->end()) { + it->second += Eigen::MatrixXd::Identity(value.dim(), value.dim()); + } else { + (*jacobians)[key_] = Eigen::MatrixXd::Identity(value.dim(), value.dim()); + } + } + return value; } + + virtual ExpressionNode* clone() const { return new LeafExpression(*this); } }; //----------------------------------------------------------------------------- /// Unary Expression template -class UnaryExpression { +class UnaryExpression : public ExpressionNode { -public: + public: - typedef T (*function)(const typename E::type&, boost::optional); + typedef T (*function)(const E&, boost::optional); -private: + private: - const E expression_; + boost::shared_ptr< ExpressionNode > expression_; function f_; -public: + public: typedef T type; /// Constructor with a single key - UnaryExpression(function f, const E& expression) : - expression_(expression), f_(f) { + UnaryExpression(function f, const ExpressionNode& expression) : + expression_(expression.clone()), f_(f) { + } + virtual ~UnaryExpression(){} + + virtual void getKeys(std::set& keys) const{ expression_->getKeys(keys); } + virtual T value(const Values& values, + boost::optional&> jacobians = boost::none) const { + + T value; + if(jacobians) { + Eigen::MatrixXd H; + value = f_(expression_->value(values, jacobians), H); + std::map::iterator it = jacobians->begin(); + for( ; it != jacobians->end(); ++it) { + it->second = H * it->second; + } + } else { + value = f_(expression_->value(values), boost::none); + } + return value; } - /// Evaluate the values of the subtree and call unary function on result - T value(const Values& values) const { - return f_(expression_.value(values), boost::none); - } - - /// Get the Jacobians from the subtree and apply the chain rule - std::map jacobians(const Values& values) const { - std::map terms = expression_.jacobians(values); - Matrix H; - // TODO, wasted value calculation, create a combined call - f_(expression_.value(values), H); - typedef std::pair Pair; - BOOST_FOREACH(const Pair& term, terms) - terms[term.first] = H * term.second; - return terms; - } + virtual ExpressionNode* clone() const { return new UnaryExpression(*this); } }; //----------------------------------------------------------------------------- /// Binary Expression + template -class BinaryExpression { +class BinaryExpression : public ExpressionNode { -public: + public: - typedef T (*function)(const typename E1::type&, const typename E2::type&, + typedef T (*function)(const E1&, const E2&, boost::optional, boost::optional); -private: + private: - const E1 expression1_; - const E2 expression2_; + boost::shared_ptr< ExpressionNode > expression1_; + boost::shared_ptr< ExpressionNode > expression2_; function f_; -public: + public: typedef T type; /// Constructor with a single key - BinaryExpression(function f, const E1& expression1, const E2& expression2) : - expression1_(expression1), expression2_(expression2), f_(f) { + BinaryExpression(function f, const ExpressionNode& expression1, const ExpressionNode& expression2) : + expression1_(expression1.clone()), expression2_(expression2.clone()), f_(f) { + } + virtual ~BinaryExpression(){} + + virtual void getKeys(std::set& keys) const{ + expression1_->getKeys(keys); + expression2_->getKeys(keys); + } + virtual T value(const Values& values, + boost::optional&> jacobians = boost::none) const { + T val; + if(jacobians) { + std::map terms1; + std::map terms2; + Matrix H1, H2; + val = f_(expression1_->value(values, terms1), expression2_->value(values, terms2), H1, H2); + // TODO: both Jacobians and terms are sorted. There must be a simple + // but fast algorithm that does this. + typedef std::pair Pair; + BOOST_FOREACH(const Pair& term, terms1) { + std::map::iterator it = jacobians->find(term.first); + if(it != jacobians->end()) { + it->second += H1 * term.second; + } else { + (*jacobians)[term.first] = H1 * term.second; + } + } + BOOST_FOREACH(const Pair& term, terms2) { + std::map::iterator it = jacobians->find(term.first); + if(it != jacobians->end()) { + it->second += H2 * term.second; + } else { + (*jacobians)[term.first] = H2 * term.second; + } + } + } else { + val = f_(expression1_->value(values), expression2_->value(values), + boost::none, boost::none); + } + return val; } - /// Evaluate the values of the subtrees and call binary function on result - T value(const Values& values) const { - return f_(expression1_.value(values), expression2_.value(values), - boost::none, boost::none); - } - - /// Get the Jacobians from the subtrees and apply the chain rule - std::map jacobians(const Values& values) const { - std::map terms1 = expression1_.jacobians(values); - std::map terms2 = expression2_.jacobians(values); - Matrix H1, H2; - // TODO, wasted value calculation, create a combined call - f_(expression1_.value(values), expression2_.value(values), H1, H2); - std::map terms; - // TODO if Key already exists, add ! - typedef std::pair Pair; - BOOST_FOREACH(const Pair& term, terms1) - terms[term.first] = H1 * term.second; - BOOST_FOREACH(const Pair& term, terms2) - terms[term.first] = H2 * term.second; - return terms; - } + virtual ExpressionNode* clone() const { return new BinaryExpression(*this); } }; +template +class Expression { + public: + Expression(const ExpressionNode& root) { + root_.reset(root.clone()); + } + + // Initialize a constant expression + Expression(const T& value) : + root_(new ConstantExpression(value)){ } + + // Initialize a leaf expression + Expression(const Key& key) : + root_(new LeafExpression(key)) {} + + /// Initialize a unary expression + template + Expression(typename UnaryExpression::function f, + const Expression& expression) { + // TODO Assert that root of expression is not null. + root_.reset(new UnaryExpression(f, *expression.root())); + } + + /// Initialize a binary expression + template + Expression(typename BinaryExpression::function f, + const Expression& expression1, + const Expression& expression2) { + // TODO Assert that root of expressions 1 and 2 are not null. + root_.reset(new BinaryExpression(f, *expression1.root(), + *expression2.root())); + } + + void getKeys(std::set& keys) const { root_->getKeys(); } + T value(const Values& values, + boost::optional&> jacobians = boost::none) const { + return root_->value(values, jacobians); + } + + const boost::shared_ptr >& root() const{ return root_; } + private: + boost::shared_ptr > root_; +}; //----------------------------------------------------------------------------- void printPair(std::pair pair) { @@ -187,11 +271,11 @@ void printPair(std::pair pair) { //----------------------------------------------------------------------------- /// AD Factor -template +template class BADFactor: NonlinearFactor { const T measurement_; - const E expression_; + const Expression expression_; /// get value from expression and calculate error with respect to measurement Vector unwhitenedError(const Values& values) const { @@ -199,13 +283,16 @@ class BADFactor: NonlinearFactor { return value.localCoordinates(measurement_); } -public: + public: /// Constructor - BADFactor(const T& measurement, const E& expression) : - measurement_(measurement), expression_(expression) { + BADFactor(const T& measurement, const Expression& expression) : + measurement_(measurement), expression_(expression) { + } + /// Constructor + BADFactor(const T& measurement, const ExpressionNode& expression) : + measurement_(measurement), expression_(expression) { } - /** * Calculate the error of the factor. * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. @@ -231,7 +318,8 @@ public: // We will construct an n-ary factor below, where terms is a container whose // value type is std::pair, specifying the // collection of keys and matrices making up the factor. - std::map terms = expression_.jacobians(values); + std::map terms; + expression_.value(values, terms); Vector b = unwhitenedError(values); SharedDiagonal model = SharedDiagonal(); return boost::shared_ptr( @@ -247,7 +335,7 @@ using namespace gtsam; /* ************************************************************************* */ Point3 transformTo(const Pose3& x, const Point3& p, - boost::optional Dpose, boost::optional Dpoint) { + boost::optional Dpose, boost::optional Dpoint) { return x.transform_to(p, Dpose, Dpoint); } @@ -257,7 +345,7 @@ Point2 project(const Point3& p, boost::optional Dpoint) { template Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional Dcal, - boost::optional Dp) { + boost::optional Dp) { return K.uncalibrate(p, Dcal, Dp); } @@ -279,22 +367,19 @@ TEST(BAD, test) { GaussianFactor::shared_ptr expected = old.linearize(values); // Create leaves - LeafExpression x(1); - LeafExpression p(2); - LeafExpression K(3); + Expression x(1); + Expression p(2); + Expression K(3); // Create expression tree - typedef BinaryExpression, LeafExpression > Binary1; - Binary1 p_cam(transformTo, x, p); + Expression p_cam(transformTo, x, p); - typedef UnaryExpression Unary1; - Unary1 projection(project, p_cam); + Expression projection(project, p_cam); - typedef BinaryExpression, Unary1> Binary2; - Binary2 uv_hat(uncalibrate, K, projection); + Expression uv_hat(uncalibrate, K, projection); // Create factor - BADFactor f(measured, uv_hat); + BADFactor f(measured, uv_hat); // Check value EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); @@ -305,6 +390,7 @@ TEST(BAD, test) { // Check linearization boost::shared_ptr gf = f.linearize(values); EXPECT( assert_equal(*expected, *gf, 1e-9)); + } /* ************************************************************************* */