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

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

@ -38,12 +38,14 @@ namespace gtsam {
/// http://loki-lib.sourceforge.net/html/a00652.html /// http://loki-lib.sourceforge.net/html/a00652.html
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;
}; };
template<typename T> template<typename T>
@ -51,23 +53,25 @@ class Expression;
/// Constant Expression /// Constant Expression
template<class T> template<class T>
class ConstantExpression : public ExpressionNode<T> { class ConstantExpression: public ExpressionNode<T> {
T value_; T value_;
public: public:
typedef T type; typedef T type;
/// Constructor with a value, yielding a constant /// Constructor with a value, yielding a constant
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_;
} }
}; };
@ -75,30 +79,34 @@ class ConstantExpression : public ExpressionNode<T> {
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/// Leaf Expression /// Leaf Expression
template<class T> template<class T>
class LeafExpression : public ExpressionNode<T> { class LeafExpression: public ExpressionNode<T> {
Key key_; Key key_;
public: public:
typedef T type; typedef T type;
/// Constructor with a single key /// Constructor with a single key
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_);
if( jacobians ) { if (jacobians) {
std::map<Key, Matrix>::iterator it = jacobians->find(key_); std::map<Key, Matrix>::iterator it = jacobians->find(key_);
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;
@ -109,37 +117,40 @@ class LeafExpression : public ExpressionNode<T> {
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/// Unary Expression /// Unary Expression
template<class T, class E> template<class T, class E>
class UnaryExpression : public ExpressionNode<T> { class UnaryExpression: public ExpressionNode<T> {
public: public:
typedef T (*function)(const E&, boost::optional<Matrix&>); typedef T (*function)(const E&, boost::optional<Matrix&>);
private: private:
boost::shared_ptr< ExpressionNode<E> > expression_; boost::shared_ptr<ExpressionNode<E> > expression_;
function f_; function f_;
public: public:
typedef T type; typedef T type;
/// Constructor with a single key /// Constructor with a single key
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 {
T value; T value;
if(jacobians) { if (jacobians) {
Eigen::MatrixXd H; Eigen::MatrixXd H;
value = f_(expression_->value(values, jacobians), H); value = f_(expression_->value(values, jacobians), H);
std::map<Key, Matrix>::iterator it = jacobians->begin(); std::map<Key, Matrix>::iterator it = jacobians->begin();
for( ; it != jacobians->end(); ++it) { for (; it != jacobians->end(); ++it) {
it->second = H * it->second; it->second = H * it->second;
} }
} else { } else {
@ -154,47 +165,50 @@ class UnaryExpression : public ExpressionNode<T> {
/// Binary Expression /// Binary Expression
template<class T, class E1, class E2> template<class T, class E1, class E2>
class BinaryExpression : public ExpressionNode<T> { 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:
boost::shared_ptr< ExpressionNode<E1> > expression1_; boost::shared_ptr<ExpressionNode<E1> > expression1_;
boost::shared_ptr< ExpressionNode<E2> > expression2_; boost::shared_ptr<ExpressionNode<E2> > expression2_;
function f_; function f_;
public: public:
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,
expression1_(expression1.root()), expression2_(expression2.root()), f_(f) { const Expression<E2>& expression2) :
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);
expression2_->getKeys(keys); expression2_->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 {
T val; T val;
if(jacobians) { if (jacobians) {
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;
BOOST_FOREACH(const Pair& term, terms1) { BOOST_FOREACH(const Pair& term, terms1) {
std::map<Key, Matrix>::iterator it = jacobians->find(term.first); std::map<Key, Matrix>::iterator it = jacobians->find(term.first);
if(it != jacobians->end()) { if (it != jacobians->end()) {
it->second += H1 * term.second; it->second += H1 * term.second;
} else { } else {
(*jacobians)[term.first] = H1 * term.second; (*jacobians)[term.first] = H1 * term.second;
@ -202,7 +216,7 @@ class BinaryExpression : public ExpressionNode<T> {
} }
BOOST_FOREACH(const Pair& term, terms2) { BOOST_FOREACH(const Pair& term, terms2) {
std::map<Key, Matrix>::iterator it = jacobians->find(term.first); std::map<Key, Matrix>::iterator it = jacobians->find(term.first);
if(it != jacobians->end()) { if (it != jacobians->end()) {
it->second += H2 * term.second; it->second += H2 * term.second;
} else { } else {
(*jacobians)[term.first] = H2 * term.second; (*jacobians)[term.first] = H2 * term.second;
@ -210,7 +224,7 @@ class BinaryExpression : public ExpressionNode<T> {
} }
} else { } else {
val = f_(expression1_->value(values), expression2_->value(values), val = f_(expression1_->value(values), expression2_->value(values),
boost::none, boost::none); boost::none, boost::none);
} }
return val; return val;
} }
@ -219,42 +233,46 @@ class BinaryExpression : public ExpressionNode<T> {
template<typename T> template<typename T>
class Expression { class Expression {
public: public:
// 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>
Expression(typename UnaryExpression<T,E>::function f, Expression(typename UnaryExpression<T, E>::function f,
const Expression<E>& expression) { const Expression<E>& expression) {
// TODO Assert that root of expression is not null. // TODO Assert that root of expression is not null.
root_.reset(new UnaryExpression<T,E>(f, expression)); root_.reset(new UnaryExpression<T, E>(f, 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 {
private: return root_;
}
private:
boost::shared_ptr<ExpressionNode<T> > root_; boost::shared_ptr<ExpressionNode<T> > root_;
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
@ -278,15 +296,15 @@ class BADFactor: NonlinearFactor {
return value.localCoordinates(measurement_); return value.localCoordinates(measurement_);
} }
public: public:
/// Constructor /// Constructor
BADFactor(const T& measurement, const Expression<T>& expression) : BADFactor(const T& measurement, const Expression<T>& expression) :
measurement_(measurement), expression_(expression) { measurement_(measurement), expression_(expression) {
} }
/// Constructor /// Constructor
BADFactor(const T& measurement, const ExpressionNode<T>& expression) : BADFactor(const T& measurement, const ExpressionNode<T>& expression) :
measurement_(measurement), expression_(expression) { measurement_(measurement), expression_(expression) {
} }
/** /**
* Calculate the error of the factor. * Calculate the error of the factor.
@ -330,7 +348,7 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
Point3 transformTo(const Pose3& x, const Point3& p, Point3 transformTo(const Pose3& x, const Point3& p,
boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) { boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) {
return x.transform_to(p, Dpose, Dpoint); return x.transform_to(p, Dpose, Dpoint);
} }
@ -340,7 +358,7 @@ Point2 project(const Point3& p, boost::optional<Matrix&> Dpoint) {
template<class CAL> template<class CAL>
Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix&> Dcal, Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) { boost::optional<Matrix&> Dp) {
return K.uncalibrate(p, Dcal, Dp); return K.uncalibrate(p, Dcal, Dp);
} }