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>
class ExpressionNode {
public:
ExpressionNode(){}
virtual ~ExpressionNode(){}
ExpressionNode() {
}
virtual ~ExpressionNode() {
}
virtual void getKeys(std::set<Key>& keys) const = 0;
virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> = boost::none) const = 0;
@ -63,9 +65,11 @@ class ConstantExpression : public ExpressionNode<T> {
ConstantExpression(const T& 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,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
return value_;
@ -87,9 +91,12 @@ class LeafExpression : public ExpressionNode<T> {
LeafExpression(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,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
const T& value = values.at<T>(key_);
@ -98,7 +105,8 @@ class LeafExpression : public ExpressionNode<T> {
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;
@ -128,9 +136,12 @@ class UnaryExpression : public ExpressionNode<T> {
UnaryExpression(function f, const Expression<E>& expression) :
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,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
@ -158,8 +169,8 @@ class BinaryExpression : public ExpressionNode<T> {
public:
typedef T (*function)(const E1&, const E2&,
boost::optional<Matrix&>, boost::optional<Matrix&>);
typedef T (*function)(const E1&, const E2&, boost::optional<Matrix&>,
boost::optional<Matrix&>);
private:
@ -172,10 +183,12 @@ class BinaryExpression : public ExpressionNode<T> {
typedef T type;
/// 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) {
}
virtual ~BinaryExpression(){}
virtual ~BinaryExpression() {
}
virtual void getKeys(std::set<Key>& keys) const {
expression1_->getKeys(keys);
@ -188,7 +201,8 @@ class BinaryExpression : public ExpressionNode<T> {
std::map<Key, Matrix> terms1;
std::map<Key, Matrix> 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<Key, Matrix> Pair;
@ -223,11 +237,13 @@ class Expression {
// Initialize a constant expression
Expression(const T& value) :
root_(new ConstantExpression<T>(value)){ }
root_(new ConstantExpression<T>(value)) {
}
// Initialize a leaf expression
Expression(const Key& key) :
root_(new LeafExpression<T>(key)) {}
root_(new LeafExpression<T>(key)) {
}
/// Initialize a unary expression
template<typename E>
@ -240,20 +256,22 @@ class Expression {
/// Initialize a binary expression
template<typename E1, typename E2>
Expression(typename BinaryExpression<T, E1, E2>::function f,
const Expression<E1>& expression1,
const Expression<E2>& expression2) {
const Expression<E1>& expression1, const Expression<E2>& expression2) {
// TODO Assert that root of expressions 1 and 2 are not null.
root_.reset(new BinaryExpression<T,E1,E2>(f, expression1,
expression2));
root_.reset(new BinaryExpression<T, E1, E2>(f, expression1, 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,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
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:
boost::shared_ptr<ExpressionNode<T> > root_;
};