slam_monocular.h
Go to the documentation of this file.
00001 
00028 // WARNING: The monocular factor has not been tested extensively...
00029 
00030 #pragma once
00031 
00032 #include <string>
00033 #include <sstream>
00034 #include <math.h>
00035 #include <Eigen/Dense>
00036 
00037 #include "Node.h"
00038 #include "Factor.h"
00039 #include "Pose3d.h"
00040 #include "Point3dh.h"
00041 
00042 namespace isam {
00043 
00044 class MonocularMeasurement {
00045   friend std::ostream& operator<<(std::ostream& out, const MonocularMeasurement& t) {
00046     t.write(out);
00047     return out;
00048   }
00049 
00050 public:
00051   double u;
00052   double v;
00053   bool valid;
00054 
00055   MonocularMeasurement(double u, double v) : u(u), v(v), valid(true) {}
00056   MonocularMeasurement(double u, double v, bool valid) : u(u), v(v), valid(valid) {}
00057 
00058   Eigen::Vector3d vector() const {
00059     Eigen::Vector3d tmp(u, v);
00060     return tmp;
00061   }
00062 
00063   void write(std::ostream &out) const {
00064     out << "(" << u << ", " << v << ")";
00065   }
00066 };
00067 
00068 class MonocularCamera { // for now, camera and robot are identical
00069   double _f;
00070   Eigen::Vector2d _pp;
00071   double _b;
00072 
00073 public:
00074 
00075   MonocularCamera() : _f(1), _pp(Eigen::Vector2d(0.5,0.5)) {}
00076   MonocularCamera(double f, const Eigen::Vector2d& pp) : _f(f), _pp(pp) {}
00077 
00078   inline double focalLength() const {return _f;}
00079 
00080   inline Eigen::Vector2d principalPoint() const {return _pp;}
00081 
00082   MonocularMeasurement project(const Pose3d& pose, const Point3dh& Xw) const {
00083     Point3dh X = pose.transform_to(Xw);
00084     // camera system has z pointing forward, instead of x
00085     double x = X.y();
00086     double y = X.z();
00087     double z = X.x();
00088     double w = X.w();
00089     double fz = _f / z;
00090     double u = x * fz + _pp(0);
00091     double v = y * fz + _pp(1);
00092     bool valid = ((w==0&&z>0) || (z/w) > 0.); // infront of camera?
00093     // Setting a point valid can make the system (hj 25/10/2012)
00094     // ill conditioned, i.e. if point goes behind all cameras that see it
00095     //valid = true;
00096 
00097     return MonocularMeasurement(u, v, valid);
00098   }
00099 
00100   Point3dh backproject(const Pose3d& pose, const MonocularMeasurement& measure,
00101       double distance = 5.) const {
00102     double lx = (measure.u-_pp(0)) * distance / _f;
00103     double ly = (measure.v-_pp(1)) * distance / _f;
00104     double lz = distance;
00105     double lw = 1;
00106     if (lz<0.) {
00107       std::cout << "Warning: MonocularCamera.backproject called with negative distance\n";
00108     }
00109     Point3dh X(lz, lx, ly, lw);
00110 
00111     return pose.transform_from(X);
00112   }
00113 
00114 };
00115 
00116 //typedef NodeT<Point3dh> Point3dh_Node;
00117 
00122 class Monocular_Factor : public FactorT<MonocularMeasurement> {
00123   Pose3d_Node* _pose;
00124   Point3d_Node* _point;
00125   Point3dh_Node* _point_h;
00126   MonocularCamera* _camera;
00127 
00128 public:
00129 
00130   // constructor for projective geometry
00131   Monocular_Factor(Pose3d_Node* pose, Point3dh_Node* point, MonocularCamera* camera,
00132                    const MonocularMeasurement& measure, const isam::Noise& noise)
00133     : FactorT<MonocularMeasurement>("Monocular_Factor", 2, noise, measure),
00134       _pose(pose), _point(NULL), _point_h(point), _camera(camera) {
00135     // MonocularCamera could also be a node later (either with 0 variables,
00136     // or with calibration as variables)
00137     _nodes.resize(2);
00138     _nodes[0] = pose;
00139     _nodes[1] = point;
00140   }
00141 
00142   // constructor for Euclidean geometry - WARNING: only use for points at short range
00143   Monocular_Factor(Pose3d_Node* pose, Point3d_Node* point, MonocularCamera* camera,
00144                    const MonocularMeasurement& measure, const isam::Noise& noise)
00145     : FactorT<MonocularMeasurement>("Monocular_Factor", 2, noise, measure),
00146       _pose(pose), _point(point), _point_h(NULL), _camera(camera) {
00147     _nodes.resize(2);
00148     _nodes[0] = pose;
00149     _nodes[1] = point;
00150   }
00151 
00152   void initialize() {
00153     require(_pose->initialized(), "Monocular_Factor requires pose to be initialized");
00154     bool initialized = (_point_h!=NULL) ? _point_h->initialized() : _point->initialized();
00155     if (!initialized) {
00156       Point3dh predict = _camera->backproject(_pose->value(), _measure);
00157       // normalize homogeneous vector
00158       predict = Point3dh(predict.vector()).normalize();
00159       if (_point_h!=NULL) {
00160         _point_h->init(predict);
00161       } else {
00162         _point->init(predict.to_point3d());
00163       }
00164     }
00165   }
00166 
00167   Eigen::VectorXd basic_error(Selector s = ESTIMATE) const {
00168     Point3dh point = (_point_h!=NULL) ? _point_h->value(s) : _point->value(s);
00169     MonocularMeasurement predicted = _camera->project(_pose->value(s), point);
00170     if (predicted.valid == true) {
00171       return (predicted.vector() - _measure.vector());
00172     } else {
00173       // effectively disables points behind the camera
00174       return Eigen::Vector2d::Zero();
00175     }
00176   }
00177 
00178 };
00179 
00180 }


demo_lidar
Author(s): Ji Zhang
autogenerated on Sun Oct 5 2014 23:22:43