NonLinearLS.h
Go to the documentation of this file.
00001 #ifndef _TYPES_G2O_PTAM_
00002 #define _TYPES_G2O_PTAM_
00003 
00004 #include <Eigen/StdVector>
00005 #include <tr1/random>
00006 #include <iostream>
00007 #include <stdint.h>
00008 #include <tr1/unordered_set>
00009 
00010 #include "g2o/core/graph_optimizer_sparse.h"
00011 #include "g2o/core/block_solver.h"
00012 #include "g2o/core/solver.h"
00013 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
00014 #include "g2o/solvers/dense/linear_solver_dense.h"
00015 #include "g2o/core/structure_only_solver.h"
00016 
00017 #include "g2o/core/base_vertex.h"
00018 #include "g2o/core/base_binary_edge.h"
00019 #include "g2o/core/base_multi_edge.h"
00020 
00021 #include <Eigen/Eigen>
00022 #include <Eigen/Geometry>
00023 
00024 #include <cstdio>
00025 #include <iostream>
00026 #include <fstream>
00027 
00028 using namespace std;
00029 //using namespace Eigen;
00030 
00031 
00032 typedef Eigen::Matrix<double, 2, 1> Vector2;
00033 typedef Eigen::Matrix<double, 3, 1> Vector3;
00034 typedef Eigen::Matrix<double, 4, 1> Vector4;
00035 typedef Eigen::Matrix<double, 5, 1> Vector5;
00036 typedef Eigen::Matrix<double, 6, 1> Vector6;
00037 
00038 typedef Eigen::Matrix<double, 3, 3> Matrix3;
00039 typedef Eigen::Matrix<double, 4, 4> Matrix4;
00040 typedef Eigen::Matrix<double, 6, 6> Matrix6;
00041 
00042 
00043 
00044 class SE3AxisAngle
00045 {
00046 public:
00047 //  enum { Dim = 6 };
00048 
00049   SE3AxisAngle()
00050   {
00051      t.fill(0);
00052      axis.fill(0);
00053   }
00054 
00056 
00057   inline SE3AxisAngle(const Vector6 mu)
00058   {
00059      t = mu.segment<3>(0);
00060      axis = mu.segment<3>(3);
00061   }
00062 
00064 
00065   inline void setAxisAngle(const Vector6  mu)
00066   {
00067      t = mu.segment<3>(0);
00068      axis = mu.segment<3>(3);
00069   }
00070 
00072 
00073   SE3AxisAngle& operator=(const Matrix4& m)
00074   {
00075      Vector6 v6;
00076      v6 = log(m);
00077      t = v6.segment<3>(0);
00078      axis = v6.segment<3>(3);
00079      return *this;
00080   }
00081 
00083 
00084   Vector3 map(const Vector3& pto_xyz)
00085   {
00086      Matrix4 T;
00087      T = exp(getAxisAngle());
00088      return (T.block<3,3>(0,0) *  pto_xyz) + T.block<3,1>(0,3);
00089   }
00090 
00092 
00093   Vector3 mapInv(const Vector3& pto_xyz)
00094   {
00095      Matrix4 T;
00096      T = exp(-getAxisAngle());
00097      return (T.block<3,3>(0,0) *  pto_xyz) + T.block<3,1>(0,3);
00098   }
00099 
00101 
00102   Vector6 getAxisAngle(void)
00103   {
00104      Vector6 mu;
00105      mu.segment<3>(0) = t;
00106      mu.segment<3>(3) = axis;
00107      return(mu);
00108   }
00109 
00111 
00112   SE3AxisAngle inverse() { return SE3AxisAngle(log(exp(-getAxisAngle()))); }
00113 
00114 
00115   static Vector6 log(const Matrix4& m);
00116   static Matrix4 exp(const Vector6 & mu);
00117 //  static Matrix4 expInv(const Vector6 & mu);
00118 
00119 private:
00120   Vector3 t, axis;
00121 };
00122 
00123 
00124 
00129 
00130 
00131 
00132 namespace g2o
00133 {
00134 
00135   using namespace Eigen;
00136 
00140   class VertexPointXYZ : public BaseVertex<3, Vector3>
00141   {
00142     public:
00143       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00144 
00145       VertexPointXYZ()  {}
00146 
00148 
00149       virtual bool read(std::istream& is)  { return true; }
00150       virtual bool write(std::ostream& os) const  { return true; }
00151 
00153 
00154       virtual void setToOrigin() { _estimate.fill(0.); }
00155 
00157 
00158       virtual void oplus(double* update_)
00159       {
00160          //Vector3 update;
00161          for (int i=0; i<3; i++)   _estimate[i] += update_[i];
00162          //_estimate += update;
00163       }
00164   };
00165 
00166 
00169 
00172   class VertexSE3AxisAngle : public BaseVertex<6, SE3AxisAngle>
00173   {
00174     public:
00175       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00176 
00177       VertexSE3AxisAngle(Vector6 calibracion): K(calibracion)
00178       {
00179          Rcw = MatrixXd::Identity(3, 3);
00180          tcw = MatrixXd::Zero(3, 1);
00181       }
00182 
00184 
00185       VertexSE3AxisAngle(Vector6 calibracion, Vector6 mu, bool Twc = false)
00186       : K(calibracion)
00187       {
00188          Matrix4 upd;
00189          upd = SE3AxisAngle::exp(mu);
00190 
00191          this->Twc = Twc;
00192          if(Twc)
00193          {
00194             Rcw = (upd.block<3,3>(0,0)).transpose();
00195             tcw = -Rcw * upd.block<3,1>(0,3);
00196          }else
00197          {
00198             Rcw = upd.block<3,3>(0,0);
00199             tcw = upd.block<3,1>(0,3);
00200          }
00201       }
00202 
00204 
00205       VertexSE3AxisAngle(Vector6 calibracion, Matrix4 T, bool Twc = false)
00206       : K(calibracion)
00207       {
00208          this->Twc = Twc;
00209          if(Twc)
00210          {
00211             Rcw = (T.block<3,3>(0,0)).transpose();
00212             tcw = -Rcw * T.block<3,1>(0,3);
00213          }else
00214          {
00215             Rcw = T.block<3,3>(0,0);
00216             tcw = T.block<3,1>(0,3);
00217          }
00218       }
00220 
00221       bool read(std::istream& is)  { return true; }
00222       bool write(std::ostream& os) const  { return true; }
00223 
00225 
00226       virtual void setToOrigin()  { _estimate = SE3AxisAngle(); }
00227 
00229 
00230       virtual void oplus(double* update_)
00231       {
00232          Vector6 update;
00233          Matrix4 upd;
00234          for (int i=0; i<6; i++)   update[i] = update_[i];
00235 
00236          // Tcw = exp(update) * Tcw
00237          upd = SE3AxisAngle::exp(update);
00238          Rcw = upd.block<3,3>(0,0) * Rcw;
00239          tcw = upd.block<3,3>(0,0) * tcw + upd.block<3,1>(0,3);
00240       }
00241 
00243 
00244       Vector2d map(const Vector3 & Pw)
00245       {
00246         double fx, fy, cx, cy, k1, k2;
00247         cx = K(0);
00248         cy = K(1);
00249         fx = K(2);
00250         fy = K(3);
00251         k1 = K(4);
00252         k2 = K(5);
00253 
00254         double r2, r4, L;
00255         // Pc = Tcw * Pw
00256         Vector3 Pc =Rcw * Pw + tcw;
00257         Vector2d proj;    proj(0) = Pc(0)/Pc(2);      proj(1) = Pc(1)/Pc(2);
00258         r2 = proj[0]*proj[0] + proj[1]*proj[1];
00259         r4 = r2*r2;
00260         L  = 1 + k1*r2 + k2*r4;
00261 
00262         Vector2d res;
00263         res[0] = cx + fx * L * proj[0];
00264         res[1] = cy + fy * L * proj[1];
00265         return res;
00266       }
00267 
00269 
00270       void push()
00271       {
00272          RtMatrix *upd;
00273          upd = new RtMatrix();
00274          upd->block<3,3>(0,0) = Rcw;
00275          upd->block<3,1>(0,3) = tcw;
00276          backups.push(upd);
00277          BaseVertex<6, SE3AxisAngle>::push();
00278       }
00279 
00281 
00282       void pop()
00283       {
00284          RtMatrix *upd;
00285          assert(!backups.empty());
00286          upd = backups.top();
00287          Rcw = upd->block<3,3>(0,0);
00288          tcw = upd->block<3,1>(0,3);
00289          backups.pop();
00290          delete upd;
00291          BaseVertex<6, SE3AxisAngle>::pop();
00292       }
00293 
00295 
00296       Matrix4 getCamera(bool Twc=false)
00297       {
00298          Vector6 cam;
00299          Matrix4 upd;
00300          upd.setIdentity();
00301          if(Twc)
00302          {
00303             upd.block<3,3>(0,0) = Rcw.transpose();
00304             upd.block<3,1>(0,3) = -upd.block<3,3>(0,0) * tcw;
00305          }else
00306          {
00307             upd.block<3,3>(0,0) = Rcw;
00308             upd.block<3,1>(0,3) = tcw;
00309          }
00310          //cam = SE3AxisAngle::log(upd);
00311          //return cam;
00312          return upd;
00313       }
00314 
00315     private:
00316       typedef Matrix<double, 3, 4> RtMatrix;
00317 
00318       std::stack<RtMatrix*> backups;
00319 
00320       // Tcw = [Rcw | tcw]
00321       bool Twc;
00322       Matrix3 Rcw;
00323       Vector3 tcw;
00324       Vector6 K;
00325   };
00326 
00329 
00330   // Edge declaration
00331   class EdgeProjectXYZ2UV : public  BaseMultiEdge<2, Vector2d>
00332   {
00333     public:
00334       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00335 
00336       EdgeProjectXYZ2UV(){resize(2);};
00337 
00339 
00340       bool read(std::istream& is){return true;};
00341       bool write(std::ostream& os) const{return true;};
00342 
00344 
00345       void computeError()
00346       {
00347         VertexSE3AxisAngle*   cam =
00348            static_cast<VertexSE3AxisAngle*>(_vertices[0]);
00349 //           static_cast<const VertexSE3AxisAngle*>(_vertices[0]);
00350         VertexPointXYZ* pto =
00351            static_cast<VertexPointXYZ*>(_vertices[1]);
00352 //           static_cast<const VertexPointXYZ*>(_vertices[1]);
00353 
00354         Vector2d obs(_measurement);
00355         _error = obs - cam->map(pto->estimate());
00356       }
00357 
00359 
00360 //      virtual void linearizeOplus(); // Tengo que meter la jacobiana del error
00361                                        // de forma analitica. Ahora es numerica
00362   };
00363 
00364 
00365 } // end namespace
00366 
00367 
00368 
00369 
00370 
00371 // OPTIMIZATION
00372 typedef struct
00373 {
00374   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00375 
00376   Vector6 cal;
00377   Matrix4 cam;
00378   vector < Vector2, Eigen::aligned_allocator<Vector2> > pto2D;
00379   vector < Vector3, Eigen::aligned_allocator<Vector3> > pto3D;
00380   //Matrix4 solMatlab;
00381 } OptimizationData;
00382 
00383 
00384 void poseOptimization(OptimizationData &dat);
00385 
00386 #endif


re_vision
Author(s): Dorian Galvez-Lopez
autogenerated on Sun Jan 5 2014 11:32:01