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

Formatting only

release/4.3a0
dellaert 2014-09-27 15:48:07 +02:00
parent 768c7f00e1
commit cde9a41acc
1 changed files with 86 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
gtsam_unstable/base/tests/testBAD.cpp
View File

@ -39,8 +39,10 @@ namespace gtsam {
template<class T> template<class T>
class ExpressionNode { class ExpressionNode {
public: public:
ExpressionNode(){} ExpressionNode() {
virtual ~ExpressionNode(){} }
virtual ~ExpressionNode() {
}
virtual void getKeys(std::set<Key>& keys) const = 0; virtual void getKeys(std::set<Key>& keys) const = 0;
virtual T value(const Values& values, virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> = boost::none) const = 0; boost::optional<std::map<Key, Matrix>&> = boost::none) const = 0;
@ -63,9 +65,11 @@ class ConstantExpression : public ExpressionNode<T> {
ConstantExpression(const T& value) : ConstantExpression(const T& value) :
value_(value) { value_(value) {
} }
virtual ~ConstantExpression(){} virtual ~ConstantExpression() {
}
virtual void getKeys(std::set<Key>& /* keys */) const {} virtual void getKeys(std::set<Key>& /* keys */) const {
}
virtual T value(const Values& values, virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const { boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
return value_; return value_;
@ -87,9 +91,12 @@ class LeafExpression : public ExpressionNode<T> {
LeafExpression(Key key) : LeafExpression(Key key) :
key_(key) { key_(key) {
} }
virtual ~LeafExpression(){} virtual ~LeafExpression() {
}
virtual void getKeys(std::set<Key>& keys) const { keys.insert(key_); } virtual void getKeys(std::set<Key>& keys) const {
keys.insert(key_);
}
virtual T value(const Values& values, virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const { boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
const T& value = values.at<T>(key_); const T& value = values.at<T>(key_);
@ -98,7 +105,8 @@ class LeafExpression : public ExpressionNode<T> {
if (it != jacobians->end()) { if (it != jacobians->end()) {
it->second += Eigen::MatrixXd::Identity(value.dim(), value.dim()); it->second += Eigen::MatrixXd::Identity(value.dim(), value.dim());
} else { } else {
(*jacobians)[key_] = Eigen::MatrixXd::Identity(value.dim(), value.dim()); (*jacobians)[key_] = Eigen::MatrixXd::Identity(value.dim(),
value.dim());
} }
} }
return value; return value;
@ -128,9 +136,12 @@ class UnaryExpression : public ExpressionNode<T> {
UnaryExpression(function f, const Expression<E>& expression) : UnaryExpression(function f, const Expression<E>& expression) :
expression_(expression.root()), f_(f) { expression_(expression.root()), f_(f) {
} }
virtual ~UnaryExpression(){} virtual ~UnaryExpression() {
}
virtual void getKeys(std::set<Key>& keys) const{ expression_->getKeys(keys); } virtual void getKeys(std::set<Key>& keys) const {
expression_->getKeys(keys);
}
virtual T value(const Values& values, virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const { boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
@ -158,8 +169,8 @@ class BinaryExpression : public ExpressionNode<T> {
public: public:
typedef T (*function)(const E1&, const E2&, typedef T (*function)(const E1&, const E2&, boost::optional<Matrix&>,
boost::optional<Matrix&>, boost::optional<Matrix&>); boost::optional<Matrix&>);
private: private:
@ -172,10 +183,12 @@ class BinaryExpression : public ExpressionNode<T> {
typedef T type; typedef T type;
/// Constructor with a single key /// Constructor with a single key
BinaryExpression(function f, const Expression<E1>& expression1, const Expression<E2>& expression2) : BinaryExpression(function f, const Expression<E1>& expression1,
const Expression<E2>& expression2) :
expression1_(expression1.root()), expression2_(expression2.root()), f_(f) { expression1_(expression1.root()), expression2_(expression2.root()), f_(f) {
} }
virtual ~BinaryExpression(){} virtual ~BinaryExpression() {
}
virtual void getKeys(std::set<Key>& keys) const { virtual void getKeys(std::set<Key>& keys) const {
expression1_->getKeys(keys); expression1_->getKeys(keys);
@ -188,7 +201,8 @@ class BinaryExpression : public ExpressionNode<T> {
std::map<Key, Matrix> terms1; std::map<Key, Matrix> terms1;
std::map<Key, Matrix> terms2; std::map<Key, Matrix> terms2;
Matrix H1, H2; 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 // TODO: both Jacobians and terms are sorted. There must be a simple
// but fast algorithm that does this. // but fast algorithm that does this.
typedef std::pair<Key, Matrix> Pair; typedef std::pair<Key, Matrix> Pair;
@ -223,11 +237,13 @@ class Expression {
// Initialize a constant expression // Initialize a constant expression
Expression(const T& value) : Expression(const T& value) :
root_(new ConstantExpression<T>(value)){ } root_(new ConstantExpression<T>(value)) {
}
// Initialize a leaf expression // Initialize a leaf expression
Expression(const Key& key) : Expression(const Key& key) :
root_(new LeafExpression<T>(key)) {} root_(new LeafExpression<T>(key)) {
}
/// Initialize a unary expression /// Initialize a unary expression
template<typename E> template<typename E>
@ -240,20 +256,22 @@ class Expression {
/// Initialize a binary expression /// Initialize a binary expression
template<typename E1, typename E2> template<typename E1, typename E2>
Expression(typename BinaryExpression<T, E1, E2>::function f, Expression(typename BinaryExpression<T, E1, E2>::function f,
const Expression<E1>& expression1, const Expression<E1>& expression1, const Expression<E2>& expression2) {
const Expression<E2>& expression2) {
// TODO Assert that root of expressions 1 and 2 are not null. // TODO Assert that root of expressions 1 and 2 are not null.
root_.reset(new BinaryExpression<T,E1,E2>(f, expression1, root_.reset(new BinaryExpression<T, E1, E2>(f, expression1, expression2));
expression2));
} }
void getKeys(std::set<Key>& keys) const { root_->getKeys(keys); } void getKeys(std::set<Key>& keys) const {
root_->getKeys(keys);
}
T value(const Values& values, T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const { boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
return root_->value(values, jacobians); return root_->value(values, jacobians);
} }
const boost::shared_ptr<ExpressionNode<T> >& root() const{ return root_; } const boost::shared_ptr<ExpressionNode<T> >& root() const {
return root_;
}
private: private:
boost::shared_ptr<ExpressionNode<T> > root_; boost::shared_ptr<ExpressionNode<T> > root_;
}; };