slam_stereo.h
Go to the documentation of this file.
00001 
00028 #pragma once
00029 
00030 #include <string>
00031 #include <sstream>
00032 #include <math.h>
00033 #include <Eigen/Dense>
00034 
00035 #include "Node.h"
00036 #include "Factor.h"
00037 #include "Pose3d.h"
00038 #include "Point3dh.h"
00039 
00040 namespace isam {
00041 
00042 class StereoMeasurement {
00043   friend std::ostream& operator<<(std::ostream& out, const StereoMeasurement& t) {
00044     t.write(out);
00045     return out;
00046   }
00047 
00048 public:
00049   double u;
00050   double v;
00051   double u2;
00052   bool valid;
00053 
00054   StereoMeasurement(double u, double v, double u2)
00055     : u(u), v(v), u2(u2), valid(true) {
00056   }
00057   StereoMeasurement(double u, double v, double u2, bool valid)
00058     : u(u), v(v), u2(u2), valid(valid) {
00059   }
00060 
00061   Eigen::Vector2d left_pixel() const {Eigen::Vector2d V(2); V << u, v; return V;}
00062 
00063   Eigen::Vector2d right_pixel() const {Eigen::Vector2d V(2); V << u2, v; return V;}
00064 
00065   double disparity() const {return u-u2;}
00066 
00067   Eigen::Vector3d vector() const {
00068     Eigen::Vector3d tmp(u, v, u2);
00069     return tmp;
00070   }
00071 
00072   void write(std::ostream &out) const {
00073     out << "(" << u << ", " << v << ", " << u2 << ")";
00074   }
00075 };
00076 
00077 class StereoCamera { // for now, camera and robot are identical
00078   double _f;
00079   Eigen::Vector2d _pp;
00080   double _b;
00081 
00082 public:
00083   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00084 
00085   StereoCamera() : _f(1), _pp(Eigen::Vector2d(0.5,0.5)), _b(0.1) {}
00086   StereoCamera(double f, const Eigen::Vector2d& pp, double b) : _f(f), _pp(pp), _b(b) {}
00087 
00088   inline double focalLength() const {return _f;}
00089 
00090   inline Eigen::Vector2d principalPoint() const {return _pp;}
00091 
00092   inline double baseline() const {return _b;}
00093 
00094   StereoMeasurement project(const Pose3d& pose, const Point3dh& Xw) const {
00095     Point3dh X = pose.transform_to(Xw);
00096     // camera system has z pointing forward, instead of x
00097     double x = X.y();
00098     double y = X.z();
00099     double z = X.x();
00100     double w = X.w();
00101     // left camera
00102     double fz = _f / z;
00103     double u = x * fz + _pp(0);
00104     double v = y * fz + _pp(1);
00105     // right camera
00106     double u2 = (x - w*_b) * fz + _pp(0);
00107     bool valid = ((w==0&&z>0) || (z/w) > 0.); // infront of camera?
00108 
00109     // Setting a point valid can make the system (hj 25/10/2012)
00110     // ill conditioned, i.e. if point goes behind all cameras that see it
00111     //valid = true;
00112 
00113     return StereoMeasurement(u, v, u2, valid);
00114   }
00115 
00116 
00117   Point3dh backproject(const Pose3d& pose, const StereoMeasurement& measure) const {
00118     double lx = (measure.u-_pp(0))*_b;
00119     double ly = (measure.v-_pp(1))*_b;
00120     double lz = _f*_b;
00121     double lw = measure.u - measure.u2;
00122     if (lw<0.) {
00123       std::cout << "Warning: StereoCamera.backproject called with negative disparity\n";
00124     }
00125     Point3dh X(lz, lx, ly, lw);
00126 
00127     return pose.transform_from(X);
00128   }
00129 
00130 };
00131 
00136 class Stereo_Factor : public FactorT<StereoMeasurement> {
00137   Pose3d_Node* _pose;
00138   Point3d_Node* _point;
00139   Point3dh_Node* _point_h;
00140   StereoCamera* _camera;
00141   bool _relative;
00142   Pose3d_Node* _base;
00143 
00144 public:
00145 
00146   // constructor for projective geometry
00147   Stereo_Factor(Pose3d_Node* pose, Point3dh_Node* point, StereoCamera* camera,
00148                          const StereoMeasurement& measure, const Noise& noise, bool relative = false)
00149     : FactorT<StereoMeasurement>("Stereo_Factor", 3, noise, measure),
00150       _pose(pose), _point(NULL), _point_h(point), _camera(camera), _relative(relative), _base(NULL) {
00151     // StereoCamera could also be a node later (either with 0 variables,
00152     // or with calibration as variables)
00153     _nodes.resize(2);
00154     _nodes[0] = pose;
00155     _nodes[1] = point;
00156     // for relative parameterization recover base pose
00157     if (_relative) {
00158       if (!point->factors().empty()) {
00159         _nodes.resize(3);
00160         _nodes[2] = point->factors().front()->nodes()[0];
00161         // todo: first factor might refer to a prior or other type of node...
00162         _base = dynamic_cast<Pose3d_Node*>(_nodes[2]);
00163       } else {
00164         _base = _pose;
00165       }
00166       point->set_base(_base);
00167     }
00168   }
00169 
00170   // constructor for Euclidean geometry
00171   // WARNING: only use for points at short range
00172   Stereo_Factor(Pose3d_Node* pose, Point3d_Node* point, StereoCamera* camera,
00173                          const StereoMeasurement& measure, const Noise& noise, bool relative = false)
00174     : FactorT<StereoMeasurement>("Stereo_Factor", 3, noise, measure),
00175       _pose(pose), _point(point), _point_h(NULL), _camera(camera), _relative(relative), _base(NULL) {
00176     _nodes.resize(2);
00177     _nodes[0] = pose;
00178     _nodes[1] = point;
00179     // for relative parameterization recover base pose
00180     if (_relative) {
00181       if (!point->factors().empty()) {
00182         _nodes.resize(3);
00183         _nodes[2] = point->factors().front()->nodes()[0];
00184         // todo: first factor might refer to a prior or other type of node...
00185         _base = dynamic_cast<Pose3d_Node*>(_nodes[2]);
00186       } else {
00187         _base = _pose;
00188       }
00189       point->set_base(_base);
00190     }
00191   }
00192 
00193   void initialize() {
00194     require(_pose->initialized(), "Stereo_Factor requires pose to be initialized");
00195     bool initialized = (_point_h!=NULL) ? _point_h->initialized() : _point->initialized();
00196     if (!initialized) {
00197       Point3dh predict;
00198       if (_relative) {
00199         predict = _camera->backproject(Pose3d(), _measure);
00200       } else {
00201         predict = _camera->backproject(_pose->value(), _measure);
00202       }
00203       // normalize homogeneous vector
00204       predict = Point3dh(predict.vector()).normalize();
00205       if (_point_h!=NULL) {
00206         _point_h->init(predict);
00207       } else {
00208         _point->init(predict.to_point3d());
00209       }
00210     }
00211   }
00212 
00213   Eigen::VectorXd basic_error(Selector s = ESTIMATE) const {
00214     Point3dh point = (_point_h!=NULL) ? _point_h->value(s) : _point->value(s);
00215     Pose3d pose = _pose->value(s);
00216     if (_base) {
00217       // pose of camera relative to base camera (which might be this one!)
00218       pose = pose.ominus(_base->value(s));
00219     }
00220     StereoMeasurement predicted = _camera->project(pose, point);
00221     if (_point_h!=NULL || predicted.valid == true) {
00222       return (predicted.vector() - _measure.vector());
00223     } else {
00224       //std::cout << "Warning - StereoFactor.basic_error: point behind camera, dropping term.\n";
00225       std::cout << "Warning - StereoFactor.basic_error: " << this
00226           << " #: " << _nodes[1]->factors().size() << " " << _nodes[0]->factors().size()
00227           << " \npoint: " << point << " ptr: " << _point
00228           << " pose: " << pose << " ptr: " << _pose
00229           << " \npredicted: " << predicted.vector().transpose()
00230           << " measure: " << _measure.vector().transpose() << std::endl;
00231       return Eigen::Vector3d::Zero();
00232     }
00233   }
00234 
00235 };
00236 
00237 }


demo_rgbd
Author(s): Ji Zhang
autogenerated on Tue Mar 3 2015 18:01:08