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


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