iSAM
slam3d.h
Go to the documentation of this file.
00001 
00028 #pragma once
00029 
00030 #include <Eigen/Dense>
00031 #include "Node.h"
00032 #include "Factor.h"
00033 #include "Pose3d.h"
00034 #include "Point3d.h"
00035 #include "Anchor.h"
00036 
00037 namespace isam {
00038 
00039 typedef NodeT<Pose3d> Pose3d_Node;
00040 
00041 template<class T>
00042 class Point3dT_Node : public NodeT<T>
00043 {
00044   Pose3d_Node* _base;
00045 public:
00046   Point3dT_Node() : _base(NULL) {}
00047   Point3dT_Node(const char* name) : NodeT<T>(name), _base(NULL) {}
00048 
00049   ~Point3dT_Node() {}
00050 
00051   void set_base(Pose3d_Node* base) { _base = base; }
00052   Pose3d_Node* base() { return _base; }
00053 };
00054 
00055 typedef Point3dT_Node<Point3d> Point3d_Node;
00056 typedef Point3dT_Node<Point3dh> Point3dh_Node;
00057 
00058 class Pose3d_Factor : public FactorT<Pose3d> {
00059   Pose3d_Node* _pose;
00060 
00061 public:
00062 
00069   Pose3d_Factor(Pose3d_Node* pose, const Pose3d& prior, const Noise& noise)
00070     : FactorT<Pose3d>("Pose3d_Factor", 6, noise, prior), _pose(pose) {
00071     _nodes.resize(1);
00072     _nodes[0] = pose;
00073   }
00074 
00075   void initialize() {
00076     if (!_pose->initialized()) {
00077       Pose3d predict = _measure;
00078       _pose->init(predict);
00079     }
00080   }
00081 
00082   Eigen::VectorXd basic_error(Selector s = ESTIMATE) const {
00083     Eigen::VectorXd err = _nodes[0]->vector(s) - _measure.vector();
00084     err(3) = standardRad(err(3));
00085     err(4) = standardRad(err(4));
00086     err(5) = standardRad(err(5));
00087     return err;
00088   }
00089 };
00090 
00091 class Pose3d_Pose3d_Factor : public FactorT<Pose3d> {
00092   Pose3d_Node* _pose1;
00093   Pose3d_Node* _pose2;
00094 
00095 public:
00096 
00106   Pose3d_Pose3d_Factor(Pose3d_Node* pose1, Pose3d_Node* pose2,
00107       const Pose3d& measure, const Noise& noise,
00108       Anchor3d_Node* anchor1 = NULL, Anchor3d_Node* anchor2 = NULL)
00109     : FactorT<Pose3d>("Pose3d_Pose3d_Factor", 6, noise, measure), _pose1(pose1), _pose2(pose2) {
00110     require((anchor1==NULL && anchor2==NULL) || (anchor1!=NULL && anchor2!=NULL),
00111         "slam3d: Pose3d_Pose3d_Factor requires either 0 or 2 anchor nodes");
00112     if (anchor1) { // offset between two relative pose graphs
00113       _nodes.resize(4);
00114       _nodes[2] = anchor1;
00115       _nodes[3] = anchor2;
00116     } else {
00117       _nodes.resize(2);
00118     }
00119     _nodes[0] = pose1;
00120     _nodes[1] = pose2;
00121   }
00122 
00123   void initialize() {
00124     require(_pose1->initialized() || _pose2->initialized(),
00125         "slam3d: Pose3d_Pose3d_Factor requires pose1 or pose2 to be initialized");
00126 
00127     if (!_pose1->initialized() && _pose2->initialized()) {
00128       // Reverse constraint 
00129       Pose3d p2 = _pose2->value();
00130       Pose3d z;
00131       Pose3d predict = p2.oplus(z.ominus(_measure));
00132       _pose1->init(predict);
00133     } else if (_pose1->initialized() && !_pose2->initialized()) {
00134       Pose3d p1 = _pose1->value();
00135       Pose3d predict = p1.oplus(_measure);
00136       _pose2->init(predict);
00137     }
00138     if (_nodes.size()==4) {
00139       Anchor3d_Node* anchor1 = dynamic_cast<Anchor3d_Node*>(_nodes[2]);
00140       Anchor3d_Node* anchor2 = dynamic_cast<Anchor3d_Node*>(_nodes[3]);
00141       if (!anchor1->initialized()) {
00142         anchor1->set_prior();
00143         anchor1->init(Pose3d());
00144       }
00145       if (!anchor2->initialized()) {
00146         Pose3d p1 = _pose1->value();
00147         Pose3d p2 = _pose2->value();
00148         Pose3d a1 = anchor1->value();
00149         // see notes in slam2d.h
00150         Pose3d zero;
00151         Pose3d d = a1.oplus(p1).oplus(_measure).oplus(zero.ominus(p2));
00152         anchor2->init(d);
00153       }
00154       if (  (anchor1->parent() != NULL && anchor2->parent() != NULL && anchor1->parent() != anchor2->parent())
00155               || (anchor1->parent() == NULL && anchor2->parent() == NULL)
00156               || (anchor1->parent() == NULL && anchor2->parent() != anchor1)
00157               || (anchor1->parent() != anchor2 && anchor2->parent() == NULL)) {
00158         Pose3d p1 = _pose1->value();
00159         Pose3d p2 = _pose2->value();
00160         Pose3d a1 = anchor1->value();
00161         Pose3d a2 = anchor2->value();
00162         // Compute the transformation from anchor1 and anchor2 frames
00163         //Pose2d d = (a2.oplus(p2)).ominus(a1.oplus(p1).oplus(_measure));
00164 
00165         Pose3d zero;
00166         Pose3d a2_p2 = a2.oplus(p2);
00167         Pose3d d = a1.oplus(p1).oplus(_measure).oplus(zero.ominus(a2_p2));
00168 
00169         anchor1->merge(anchor2, d);
00170       }
00171     }
00172   }
00173 
00174   Eigen::VectorXd basic_error(Selector s = ESTIMATE) const {
00175     const Pose3d& p1 = _pose1->value(s);
00176     const Pose3d& p2 = _pose2->value(s);
00177     Pose3d predicted;
00178     if (_nodes.size()==4) {
00179       const Pose3d& a1 = dynamic_cast<Pose3d_Node*>(_nodes[2])->value(s);
00180       const Pose3d& a2 = dynamic_cast<Pose3d_Node*>(_nodes[3])->value(s);
00181       // see notes in slam2d.h
00182       predicted = (a2.oplus(p2)).ominus(a1.oplus(p1));
00183     } else {
00184       predicted = p2.ominus(p1);
00185     }
00186     Eigen::VectorXd err = predicted.vector() - _measure.vector();
00187     err(3) = standardRad(err(3));
00188     err(4) = standardRad(err(4));
00189     err(5) = standardRad(err(5));
00190     return err;
00191   }
00192 
00193 };
00194 
00195 class Pose3d_Point3d_Factor : public FactorT<Point3d> {
00196   Pose3d_Node* _pose;
00197   Point3d_Node* _point;
00198 
00199 public:
00200 
00208   Pose3d_Point3d_Factor(Pose3d_Node* pose, Point3d_Node* point,
00209       const Point3d& measure, const Noise& noise)
00210     : FactorT<Point3d>("Pose3d_Point3d_Factor", 3, noise, measure), _pose(pose), _point(point) {
00211     _nodes.resize(2);
00212     _nodes[0] = pose;
00213     _nodes[1] = point;
00214   }
00215 
00216   void initialize() {
00217     require(_pose->initialized(), "slam3d: Pose3d_Point3d_Factor requires pose to be initialized");
00218     if (!_point->initialized()) {
00219       Pose3d p = _pose->value();
00220       Point3d predict = p.transform_from(_measure);
00221       _point->init(predict);
00222     }
00223   }
00224 
00225   Eigen::VectorXd basic_error(Selector s = ESTIMATE) const {
00226     const Pose3d& po = _pose->value(s);
00227     const Point3d& pt = _point->value(s);
00228     Point3d p = po.transform_to(pt);
00229     Eigen::VectorXd predicted = p.vector();
00230     return (predicted - _measure.vector());
00231   }
00232 };
00233 
00234 }
 All Classes Files Functions Variables