iSAM
Node.h
Go to the documentation of this file.
00001 
00028 #pragma once
00029 
00030 #include <list>
00031 #include <Eigen/Dense>
00032 
00033 #include "Element.h"
00034 
00035 namespace Eigen {
00036   typedef Matrix<bool, Dynamic, 1> VectorXb;
00037 }
00038 
00039 namespace isam {
00040 
00041 enum Selector {LINPOINT, ESTIMATE};
00042 
00043 class Factor; // Factor.h not included here to avoid circular dependency
00044 
00045 // Node of the graph also containing measurements (Factor).
00046 class Node : public Element {
00047   friend std::ostream& operator<<(std::ostream& output, const Node& n) {
00048     n.write(output);
00049     return output;
00050   }
00051 
00052   static int _next_id;
00053   bool _deleted;
00054 
00055 protected:
00056 
00057   std::list<Factor*> _factors; // list of adjacent factors
00058 
00059 public:
00060 
00061   Node(const char* name, int dim) : Element(name, dim), _deleted(false) {
00062     _id = _next_id++;
00063   }
00064 
00065   virtual ~Node() {};
00066 
00067   virtual bool initialized() const = 0;
00068 
00069   virtual Eigen::VectorXd vector(Selector s = ESTIMATE) const = 0;
00070 
00071   virtual Eigen::VectorXd vector0() const = 0;
00072   virtual Eigen::VectorXb is_angle() const {return Eigen::VectorXb::Zero(_dim);}
00073 
00074   virtual void update(const Eigen::VectorXd& v) = 0;
00075   virtual void update0(const Eigen::VectorXd& v) = 0;
00076 
00077   virtual void linpoint_to_estimate() = 0;
00078   virtual void estimate_to_linpoint() = 0;
00079   virtual void swap_estimates() = 0;
00080 
00081   virtual void apply_exmap(const Eigen::VectorXd& v) = 0;
00082   virtual void self_exmap(const Eigen::VectorXd& v) = 0;
00083 
00084   void add_factor(Factor* e) {_factors.push_back(e);}
00085   void remove_factor(Factor* e) {_factors.remove(e);}
00086 
00087   const std::list<Factor*>& factors() {return _factors;}
00088 
00089   bool deleted() const { return _deleted; }
00090 
00091   void mark_deleted();
00092   void erase_marked_factors();
00093 
00094   virtual void write(std::ostream &out) const = 0;
00095 };
00096 
00097 // Generic template for easy instantiation of the multiple node types.
00098 template <class T>
00099 class NodeT : public Node {
00100 
00101  protected:
00102   T* _value;  // current estimate
00103   T* _value0; // linearization point
00104 
00105 public:
00106 
00107   NodeT() : Node(T::name(), T::dim) {
00108     _value = NULL;
00109     _value0 = NULL;
00110   }
00111 
00112   NodeT(const char* name) : Node(name, T::dim) {
00113     _value = NULL;
00114     _value0 = NULL;
00115   }
00116 
00117   virtual ~NodeT() {
00118     delete _value;
00119     delete _value0;
00120   }
00121 
00122   void init(const T& t) {
00123     delete _value; delete _value0;
00124     _value = new T(t); _value0 = new T(t);
00125   }
00126 
00127   bool initialized() const {return _value != NULL;}
00128 
00129   T value(Selector s = ESTIMATE) const {return (s==ESTIMATE)?*_value:*_value0;}
00130   T value0() const {return *_value0;}
00131 
00132   Eigen::VectorXd vector(Selector s = ESTIMATE) const {return (s==ESTIMATE)?_value->vector():_value0->vector();}
00133   Eigen::VectorXd vector0() const {return _value0->vector();}
00134   Eigen::VectorXb is_angle() const {return _value->is_angle();}
00135 
00136   void update(const Eigen::VectorXd& v) {_value->set(v);}
00137   void update0(const Eigen::VectorXd& v) {_value0->set(v);}
00138 
00139   void linpoint_to_estimate() {*_value = *_value0;}
00140   void estimate_to_linpoint() {*_value0 = *_value;}
00141   void swap_estimates() {T tmp = *_value; *_value = *_value0; *_value0 = tmp;}
00142 
00143   void apply_exmap(const Eigen::VectorXd& v) {*_value = _value0->exmap(v);}
00144   void self_exmap(const Eigen::VectorXd& v) {*_value0 = _value0->exmap(v);}
00145 
00146   void write(std::ostream &out) const {
00147     out << name() << "_Node " << _id;
00148     if (_value != NULL) {
00149       out << " " << value();
00150     }
00151   }
00152 };
00153 
00154 }
 All Classes Files Functions Variables