From cde9a41acca4880d01d74ed8db523fae7c2e44e1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 27 Sep 2014 15:48:07 +0200 Subject: [PATCH] Formatting only --- gtsam_unstable/base/tests/testBAD.cpp | 154 ++++++++++++++------------ 1 file changed, 86 insertions(+), 68 deletions(-) diff --git a/gtsam_unstable/base/tests/testBAD.cpp b/gtsam_unstable/base/tests/testBAD.cpp index 18d5b7f54..b736148ea 100644 --- a/gtsam_unstable/base/tests/testBAD.cpp +++ b/gtsam_unstable/base/tests/testBAD.cpp @@ -38,12 +38,14 @@ namespace gtsam { /// http://loki-lib.sourceforge.net/html/a00652.html template class ExpressionNode { - public: - ExpressionNode(){} - virtual ~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; + boost::optional&> = boost::none) const = 0; }; template @@ -51,23 +53,25 @@ class Expression; /// Constant Expression template -class ConstantExpression : public ExpressionNode { +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() { } - virtual ~ConstantExpression(){} - virtual void getKeys(std::set& /* keys */) const {} + virtual void getKeys(std::set& /* keys */) const { + } virtual T value(const Values& values, - boost::optional&> jacobians = boost::none) const { + boost::optional&> jacobians = boost::none) const { return value_; } }; @@ -75,30 +79,34 @@ class ConstantExpression : public ExpressionNode { //----------------------------------------------------------------------------- /// Leaf Expression template -class LeafExpression : public ExpressionNode { +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() { } - virtual ~LeafExpression(){} - virtual void getKeys(std::set& keys) const { keys.insert(key_); } + virtual void getKeys(std::set& keys) const { + keys.insert(key_); + } virtual T value(const Values& values, - boost::optional&> jacobians = boost::none) const { + boost::optional&> jacobians = boost::none) const { const T& value = values.at(key_); - if( jacobians ) { + if (jacobians) { std::map::iterator it = jacobians->find(key_); - if(it != jacobians->end()) { + if (it != jacobians->end()) { it->second += Eigen::MatrixXd::Identity(value.dim(), value.dim()); } else { - (*jacobians)[key_] = Eigen::MatrixXd::Identity(value.dim(), value.dim()); + (*jacobians)[key_] = Eigen::MatrixXd::Identity(value.dim(), + value.dim()); } } return value; @@ -109,37 +117,40 @@ class LeafExpression : public ExpressionNode { //----------------------------------------------------------------------------- /// Unary Expression template -class UnaryExpression : public ExpressionNode { +class UnaryExpression: public ExpressionNode { - public: +public: typedef T (*function)(const E&, boost::optional); - private: +private: - boost::shared_ptr< ExpressionNode > expression_; + boost::shared_ptr > expression_; function f_; - public: +public: typedef T type; /// Constructor with a single key UnaryExpression(function f, const Expression& expression) : - expression_(expression.root()), f_(f) { + expression_(expression.root()), f_(f) { + } + virtual ~UnaryExpression() { } - virtual ~UnaryExpression(){} - virtual void getKeys(std::set& keys) const{ expression_->getKeys(keys); } + virtual void getKeys(std::set& keys) const { + expression_->getKeys(keys); + } virtual T value(const Values& values, - boost::optional&> jacobians = boost::none) const { + boost::optional&> jacobians = boost::none) const { T value; - if(jacobians) { + if (jacobians) { Eigen::MatrixXd H; value = f_(expression_->value(values, jacobians), H); std::map::iterator it = jacobians->begin(); - for( ; it != jacobians->end(); ++it) { + for (; it != jacobians->end(); ++it) { it->second = H * it->second; } } else { @@ -154,47 +165,50 @@ class UnaryExpression : public ExpressionNode { /// Binary Expression template -class BinaryExpression : public ExpressionNode { +class BinaryExpression: public ExpressionNode { - public: +public: - typedef T (*function)(const E1&, const E2&, - boost::optional, boost::optional); + typedef T (*function)(const E1&, const E2&, boost::optional, + boost::optional); - private: +private: - boost::shared_ptr< ExpressionNode > expression1_; - boost::shared_ptr< ExpressionNode > expression2_; + boost::shared_ptr > expression1_; + boost::shared_ptr > expression2_; function f_; - public: +public: typedef T type; /// Constructor with a single key - BinaryExpression(function f, const Expression& expression1, const Expression& expression2) : - expression1_(expression1.root()), expression2_(expression2.root()), f_(f) { + BinaryExpression(function f, const Expression& expression1, + const Expression& expression2) : + expression1_(expression1.root()), expression2_(expression2.root()), f_(f) { + } + virtual ~BinaryExpression() { } - virtual ~BinaryExpression(){} - virtual void getKeys(std::set& keys) const{ + 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 { + boost::optional&> jacobians = boost::none) const { T val; - if(jacobians) { + if (jacobians) { std::map terms1; std::map terms2; Matrix H1, H2; - val = f_(expression1_->value(values, terms1), expression2_->value(values, terms2), 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()) { + if (it != jacobians->end()) { it->second += H1 * term.second; } else { (*jacobians)[term.first] = H1 * term.second; @@ -202,7 +216,7 @@ class BinaryExpression : public ExpressionNode { } BOOST_FOREACH(const Pair& term, terms2) { std::map::iterator it = jacobians->find(term.first); - if(it != jacobians->end()) { + if (it != jacobians->end()) { it->second += H2 * term.second; } else { (*jacobians)[term.first] = H2 * term.second; @@ -210,7 +224,7 @@ class BinaryExpression : public ExpressionNode { } } else { val = f_(expression1_->value(values), expression2_->value(values), - boost::none, boost::none); + boost::none, boost::none); } return val; } @@ -219,42 +233,46 @@ class BinaryExpression : public ExpressionNode { template class Expression { - public: +public: // Initialize a constant expression Expression(const T& value) : - root_(new ConstantExpression(value)){ } + root_(new ConstantExpression(value)) { + } // Initialize a leaf expression Expression(const Key& key) : - root_(new LeafExpression(key)) {} + root_(new LeafExpression(key)) { + } /// Initialize a unary expression template - Expression(typename UnaryExpression::function f, - const Expression& expression) { + Expression(typename UnaryExpression::function f, + const Expression& expression) { // TODO Assert that root of expression is not null. - root_.reset(new UnaryExpression(f, expression)); + root_.reset(new UnaryExpression(f, expression)); } /// Initialize a binary expression template - Expression(typename BinaryExpression::function f, - const Expression& expression1, - const Expression& expression2) { + 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, - expression2)); + root_.reset(new BinaryExpression(f, expression1, expression2)); } - void getKeys(std::set& keys) const { root_->getKeys(keys); } + void getKeys(std::set& keys) const { + root_->getKeys(keys); + } T value(const Values& values, - boost::optional&> jacobians = boost::none) const { + boost::optional&> jacobians = boost::none) const { return root_->value(values, jacobians); } - const boost::shared_ptr >& root() const{ return root_; } - private: + const boost::shared_ptr >& root() const { + return root_; + } +private: boost::shared_ptr > root_; }; //----------------------------------------------------------------------------- @@ -278,15 +296,15 @@ class BADFactor: NonlinearFactor { return value.localCoordinates(measurement_); } - public: +public: /// Constructor BADFactor(const T& measurement, const Expression& expression) : - measurement_(measurement), expression_(expression) { + measurement_(measurement), expression_(expression) { } /// Constructor BADFactor(const T& measurement, const ExpressionNode& expression) : - measurement_(measurement), expression_(expression) { + measurement_(measurement), expression_(expression) { } /** * Calculate the error of the factor. @@ -330,7 +348,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); } @@ -340,7 +358,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); }