iSAM
Anchor.cpp
Go to the documentation of this file.
00001 
00029 #include <isam/Anchor.h>
00030 #include <isam/slam2d.h>
00031 #include <isam/slam3d.h>
00032 
00033 namespace isam
00034 {
00035 
00036 Anchor2d_Node::Anchor2d_Node(Slam* slam) : Pose2d_Node("Anchor2d"), _parent(NULL), _prior(NULL), _slam(slam)
00037 {
00038 }
00039 
00040 Anchor2d_Node::~Anchor2d_Node() {
00041   if (_prior) {
00042     _slam->remove_factor(_prior);
00043     delete _prior;
00044   }
00045 }
00046 
00047 void Anchor2d_Node::set_prior() {
00048   Noise noise = SqrtInformation(10000. * eye(3));
00049   Pose2d prior_origin(0., 0., 0.);
00050   _prior = new Pose2d_Factor(this, prior_origin, noise);
00051   _slam->add_factor(_prior);
00052 }
00053 
00054 void Anchor2d_Node::add_anchor(Anchor2d_Node* a) {
00055   if (a->_prior) {
00056     _slam->remove_factor(a->_prior);
00057     delete a->_prior;
00058     a->_prior = NULL;
00059   }
00060   a->_parent = this;
00061   _childs.push_back(a);
00062 }
00063 
00064 void Anchor2d_Node::merge(Anchor2d_Node* a, Pose2d old_origin) {
00065   if (_parent != NULL) {
00066     _parent->merge(a, old_origin);
00067   } else if (a->_parent != NULL) {
00068     merge(a->parent(), old_origin);
00069   } else {
00070     // this and a are root anchors.
00071 
00072     // Add a to the child list
00073     Pose2d old_anchor_pose = a->value();
00074     a->init(old_origin.oplus(old_anchor_pose));
00075     add_anchor(a);
00076 
00077     // Move the children of a to this
00078     for (size_t i = 0; i < a->_childs.size(); i++) {
00079       Anchor2d_Node* child = a->_childs[i];
00080       
00081       old_anchor_pose = child->value();
00082       child->init(old_origin.oplus(old_anchor_pose));
00083       add_anchor(child);
00084     }
00085     a->_childs.clear();
00086   }
00087 }
00088 
00089 Anchor3d_Node::Anchor3d_Node(Slam* slam) : Pose3d_Node("Anchor3d"), _parent(NULL), _prior(NULL), _slam(slam)
00090 {
00091 }
00092 
00093 Anchor3d_Node::~Anchor3d_Node() {
00094   if (_prior) {
00095     _slam->remove_factor(_prior);
00096     delete _prior;
00097   }
00098 }
00099 
00100 void Anchor3d_Node::set_prior() {
00101   Noise noise = SqrtInformation(10000. * eye(6));
00102   Pose3d prior_origin;
00103   _prior = new Pose3d_Factor(this, prior_origin, noise);
00104   _slam->add_factor(_prior);
00105 }
00106 
00107 void Anchor3d_Node::add_anchor(Anchor3d_Node* a) {
00108   if (a->_prior) {
00109     _slam->remove_factor(a->_prior);
00110     delete a->_prior;
00111     a->_prior = NULL;
00112   }
00113   a->_parent = this;
00114   _childs.push_back(a);
00115 }
00116 
00117 void Anchor3d_Node::merge(Anchor3d_Node* a, Pose3d old_origin) {
00118   if (_parent != NULL) {
00119     _parent->merge(a, old_origin);
00120   } else if (a->_parent != NULL) {
00121     merge(a->parent(), old_origin);
00122   } else {
00123     // this and a are root anchors.
00124 
00125     // Add a to the child list
00126     Pose3d old_anchor_pose = a->value();
00127     a->init(old_origin.oplus(old_anchor_pose));
00128     add_anchor(a);
00129 
00130     // Move the children of a to this
00131     for (size_t i = 0; i < a->_childs.size(); i++) {
00132       Anchor3d_Node* child = a->_childs[i];
00133 
00134       old_anchor_pose = child->value();
00135       child->init(old_origin.oplus(old_anchor_pose));
00136       add_anchor(child);
00137     }
00138     a->_childs.clear();
00139   }
00140 }
00141 
00142 }
 All Classes Files Functions Variables