iSAM
Anchor.h
Go to the documentation of this file.
00001 
00029 #pragma once
00030 
00031 #include "Node.h"
00032 #include "Factor.h"
00033 #include "Pose2d.h"
00034 #include "Point2d.h"
00035 #include "Pose3d.h"
00036 #include "Slam.h"
00037 
00038 namespace isam {
00039 
00040 typedef NodeT<Pose2d> Pose2d_Node;
00041 typedef NodeT<Pose3d> Pose3d_Node;
00042 
00046 class Anchor2d_Node : public Pose2d_Node {
00047 public:
00048   Anchor2d_Node(Slam* slam);
00049   ~Anchor2d_Node();
00050 
00055   void set_prior();
00056 
00060   void add_anchor(Anchor2d_Node* a);
00061 
00073   void merge(Anchor2d_Node* a, Pose2d old_origin);
00074 
00075   Anchor2d_Node* parent() { return _parent; }
00076 
00077 private:
00078   Anchor2d_Node* _parent;
00079   std::vector<Anchor2d_Node*> _childs;
00080   Factor* _prior;
00081   Slam* _slam;
00082 };
00083 
00087 class Anchor3d_Node : public Pose3d_Node {
00088 public:
00089   Anchor3d_Node(Slam* slam);
00090   ~Anchor3d_Node();
00091 
00096   void set_prior();
00097 
00101   void add_anchor(Anchor3d_Node* a);
00102 
00114   void merge(Anchor3d_Node* a, Pose3d old_origin);
00115 
00116   Anchor3d_Node* parent() { return _parent; }
00117 
00118 private:
00119   Anchor3d_Node* _parent;
00120   std::vector<Anchor3d_Node*> _childs;
00121   Factor* _prior;
00122   Slam* _slam;
00123 };
00124 
00125 }
 All Classes Files Functions Variables