slam2d.h
Go to the documentation of this file.
```00001
00029 #pragma once
00030
00031 #include <string>
00032 #include <sstream>
00033 #include <Eigen/Dense>
00034
00035 #include "Node.h"
00036 #include "Factor.h"
00037 #include "Pose2d.h"
00038 #include "Point2d.h"
00039 #include "Anchor.h"
00040
00041 namespace Eigen {
00042 typedef Matrix< double , 1 , 1> Vector1d;
00043 }
00044
00045 namespace isam {
00046
00047 typedef NodeT<Pose2d> Pose2d_Node;
00048 typedef NodeT<Point2d> Point2d_Node;
00049
00050
00054 class Point2d_Factor : public FactorT<Point2d> {
00055   Point2d_Node* _point;
00056
00057 public:
00058
00065   Point2d_Factor(Point2d_Node* point, const Point2d& prior, const Noise& noise)
00066     : FactorT<Point2d>("Point2d_Factor", 2, noise, prior), _point(point) {
00067     _nodes.resize(1);
00068     _nodes[0] = point;
00069   }
00070
00071   void initialize() {
00072     if (!_point->initialized()) {
00073       Point2d predict = _measure;
00074       _point->init(predict);
00075     }
00076   }
00077
00078   Eigen::VectorXd basic_error(Selector s = LINPOINT) const {
00079     return (_point->vector(s) - _measure.vector());
00080   }
00081
00082 };
00083
00087 class Pose2d_Factor : public FactorT<Pose2d> {
00088   Pose2d_Node* _pose;
00089
00090 public:
00091
00098   Pose2d_Factor(Pose2d_Node* pose, const Pose2d& prior, const Noise& noise)
00099     : FactorT<Pose2d>("Pose2d_Factor", 3, noise, prior), _pose(pose) {
00100     _nodes.resize(1);
00101     _nodes[0] = pose;
00102   }
00103
00104   void initialize() {
00105     if (!_pose->initialized()) {
00106       Pose2d predict = _measure;
00107       _pose->init(predict);
00108     }
00109   }
00110
00111   Eigen::VectorXd basic_error(Selector s = LINPOINT) const {
00112     Eigen::VectorXd err = _pose->vector(s) - _measure.vector();
00114     return err;
00115   }
00116
00117   Jacobian jacobian() {
00118     Eigen::MatrixXd M = sqrtinf(); // derivatives are all 1 (eye)
00119     Eigen::VectorXd r = sqrtinf() * basic_error();
00120     Jacobian jac(r);
00122     return jac;
00123   }
00124
00125 };
00126
00130 class Pose2d_Pose2d_Factor : public FactorT<Pose2d> {
00131   Pose2d_Node* _pose1;
00132   Pose2d_Node* _pose2;
00133
00134 public:
00135
00145   Pose2d_Pose2d_Factor(Pose2d_Node* pose1, Pose2d_Node* pose2,
00146       const Pose2d& measure, const Noise& noise,
00147       Anchor2d_Node* anchor1 = NULL, Anchor2d_Node* anchor2 = NULL)
00148     : FactorT<Pose2d>("Pose2d_Pose2d_Factor", 3, noise, measure),
00149     _pose1(pose1), _pose2(pose2) {
00150     require((anchor1==NULL && anchor2==NULL) || (anchor1!=NULL && anchor2!=NULL),
00151         "slam2d: Pose2d_Pose2d_Factor requires either 0 or 2 anchor nodes");
00152
00153     require((anchor1==NULL && anchor2==NULL) || (anchor1!=anchor2),
00154         "slam2d: Pose2d_Pose2d_Factor requires anchors to be different");
00155
00156     if (anchor1) { // offset between two relative pose graphs
00157       _nodes.resize(4);
00158       _nodes[2] = anchor1;
00159       _nodes[3] = anchor2;
00160     } else {
00161       _nodes.resize(2);
00162     }
00163     _nodes[0] = pose1;
00164     _nodes[1] = pose2;
00165   }
00166
00167   void initialize() {
00168     Pose2d_Node* pose1 = _pose1;
00169     Pose2d_Node* pose2 = _pose2;
00170     require(pose1->initialized() || pose2->initialized(),
00171         "slam2d: Pose2d_Pose2d_Factor requires pose1 or pose2 to be initialized");
00172
00173     if (!pose1->initialized() && pose2->initialized()) {
00174       // reverse constraint
00175       Pose2d p2 = pose2->value();
00176       Pose2d z;
00177       Pose2d predict = p2.oplus(z.ominus(_measure));
00178       pose1->init(predict);
00179     } else if (pose1->initialized() && !pose2->initialized()) {
00180       Pose2d p1 = pose1->value();
00181       Pose2d predict = p1.oplus(_measure);
00182       pose2->init(predict);
00183     }
00184
00185     if (_nodes.size()==4) {
00186       Anchor2d_Node* anchor1 = dynamic_cast<Anchor2d_Node*>(_nodes[2]);
00187       Anchor2d_Node* anchor2 = dynamic_cast<Anchor2d_Node*>(_nodes[3]);
00188
00189       if (!anchor1->initialized()) {
00190         anchor1->set_prior();
00191         anchor1->init(Pose2d());
00192       }
00193
00194       if (!anchor2->initialized()) {
00195         Pose2d p1 = pose1->value();
00196         Pose2d p2 = pose2->value();
00197         Pose2d a1 = anchor1->value();
00198         // note 1: ominus is tricky: a.ominus(b)=B^(-1)*A
00199         // note 2: unlike vectors, the order of composition matters
00200         //         for poses! (think in terms of matrix muliplication)
00201         Pose2d zero;
00202         Pose2d d = a1.oplus(p1).oplus(_measure).oplus(zero.ominus(p2));
00203         anchor2->init(d);
00204       }
00205       if (  (anchor1->parent() != NULL && anchor2->parent() != NULL && anchor1->parent() != anchor2->parent())
00206               || (anchor1->parent() == NULL && anchor2->parent() == NULL)
00207               || (anchor1->parent() == NULL && anchor2->parent() != anchor1)
00208               || (anchor1->parent() != anchor2 && anchor2->parent() == NULL)) {
00209         Pose2d p1 = pose1->value();
00210         Pose2d p2 = pose2->value();
00211         Pose2d a1 = anchor1->value();
00212         Pose2d a2 = anchor2->value();
00213         // Compute the transformation from anchor1 and anchor2 frames
00214         //Pose2d d = (a2.oplus(p2)).ominus(a1.oplus(p1).oplus(_measure));
00215
00216         Pose2d zero;
00217         Pose2d a2_p2 = a2.oplus(p2);
00218         Pose2d d = a1.oplus(p1).oplus(_measure).oplus(zero.ominus(a2_p2));
00219
00220         anchor1->merge(anchor2, d);
00221       }
00222     }
00223   }
00224
00225   Eigen::VectorXd basic_error(Selector s = LINPOINT) const {
00226     Pose2d p1(_nodes[0]->vector(s));
00227     Pose2d p2(_nodes[1]->vector(s));
00228     Pose2d predicted;
00229     if (_nodes.size()==4) {
00230       Pose2d a1(_nodes[2]->vector(s));
00231       Pose2d a2(_nodes[3]->vector(s));
00232       // see notes above
00233       predicted = (a2.oplus(p2)).ominus(a1.oplus(p1));
00234     } else {
00235       predicted = p2.ominus(p1);
00236     }
00237     Eigen::VectorXd err = predicted.vector() - _measure.vector();
00239     return err;
00240   }
00241
00242   Jacobian jacobian() {
00243     if (_nodes.size()==4) { // symbolic available only without anchor nodes
00244       return Factor::jacobian();
00245     } else {
00246       const Pose2d& p1 = _pose1->value0();
00247       const Pose2d& p2 = _pose2->value0();
00248       Pose2d p = p2.ominus(p1);
00249       double c = cos(p1.t());
00250       double s = sin(p1.t());
00251
00252       double dx = p.x() - _measure.x();
00253       double dy = p.y() - _measure.y();
00254       double dt = standardRad(p.t() - _measure.t());
00255
00256       Eigen::MatrixXd M1(3,3);
00257       M1 <<
00258         -c, -s,  p.y(),
00259         s,  -c,  -p.x(),
00260         0.,  0., -1.;
00261       M1 = sqrtinf() * M1;
00262
00263       Eigen::MatrixXd M2(3,3);
00264       M2 <<
00265         c,   s,   0.,
00266         -s,  c,   0.,
00267         0.,  0.,  1.;
00268       M2 = sqrtinf() * M2;
00269
00270       Pose2d pp(dx, dy, dt);
00271
00272       Eigen::VectorXd r = sqrtinf() * pp.vector();
00273
00274       Jacobian jac(r);
00277
00278       return jac;
00279     }
00280   }
00281 };
00282
00286 class Pose2d_Point2d_Factor : public FactorT<Point2d> {
00287   Pose2d_Node* _pose;
00288   Point2d_Node* _point;
00289
00290 public:
00291
00299   Pose2d_Point2d_Factor(Pose2d_Node* pose, Point2d_Node* point,
00300       const Point2d& measure, const Noise& noise)
00301     : FactorT<Point2d>("Pose2d_Point2d_Factor", 2, noise, measure), _pose(pose), _point(point) {
00302     _nodes.resize(2);
00303     _nodes[0] = pose;
00304     _nodes[1] = point;
00305   }
00306
00307   void initialize() {
00308     require(_pose->initialized(),
00309         "slam2d: Pose2d_Point2d_Factor requires pose to be initialized");
00310     if (!_point->initialized()) {
00311       Pose2d p = _pose->value();
00312       Point2d predict = p.transform_from(_measure);
00313       _point->init(predict);
00314     }
00315   }
00316
00317   Eigen::VectorXd basic_error(Selector s = LINPOINT) const {
00318     Pose2d po(_nodes[0]->vector(s));
00319     Point2d pt(_nodes[1]->vector(s));
00320     Point2d p = po.transform_to(pt);
00321     Eigen::VectorXd predicted = p.vector();
00322     return (predicted - _measure.vector());
00323   }
00324
00325   Jacobian jacobian() {
00326     Pose2d po = _pose->value0();
00327     Point2d pt = _point->value0();
00328     double c = cos(po.t());
00329     double s = sin(po.t());
00330     double dx = pt.x() - po.x();
00331     double dy = pt.y() - po.y();
00332     // f(x)
00333     double x =  c*dx + s*dy; // relative forward position of landmark point from pose
00334     double y = -s*dx + c*dy; // relative position to the left
00335     Eigen::MatrixXd M1(2,3);
00336     M1 <<
00337       -c, -s,  y,
00338       s,  -c, -x;
00339     M1 = sqrtinf() * M1;
00340     Eigen::MatrixXd M2(2,2);
00341     M2 <<
00342       c,   s,
00343       -s,  c;
00344     M2 = sqrtinf() * M2;
00345     Point2d p(x, y);
00346     Eigen::VectorXd r = sqrtinf() * (p.vector() - _measure.vector());
00347     Jacobian jac(r);