ba_demoPTAM.cpp
Go to the documentation of this file.
00001 #include <Eigen/StdVector>
00002 #include <tr1/random>
00003 #include <iostream>
00004 #include <stdint.h>
00005 #include <tr1/unordered_set>
00006 
00007 #include "g2o/core/graph_optimizer_sparse.h"
00008 #include "g2o/core/block_solver.h"
00009 #include "g2o/core/solver.h"
00010 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
00011 #include "g2o/solvers/dense/linear_solver_dense.h"
00012 #include "g2o/core/structure_only_solver.h"
00013 
00014 #include "NonLinearLS.h"
00015 
00016 #include "tokenizer.h"
00017 
00018 #include <iostream>
00019 #include <fstream>
00020 
00021 
00022 const int ITERACIONES = 200;
00023 
00024 
00025 using namespace Eigen;
00026 using namespace std;
00027 
00028 
00029 // CARGAR EL GROUND TRUTH
00030 /*
00031 typedef struct
00032 {
00033   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00034 
00035   Vector6 cal;
00036   Matrix4 cam;
00037   vector < Vector2, aligned_allocator<Vector2> > pto2D;
00038   vector < Vector3, aligned_allocator<Vector3> > pto3D;
00039   Matrix4 solMatlab;
00040 } Datos;
00041 */
00042 
00043 void loadData(Data &dat)
00044 {
00045    Token t;
00046    if(t.abrirFichero("deDorian.mat") != Token::ok)
00047    {
00048       cerr << "INVALID FILE\n";
00049       exit (-1);
00050    }
00051 
00052    int caso = 0;
00053    while(t.leeLinea() == Token::ok)
00054    {
00055       if(t.tokenizar(" \t") == 0)
00056       {
00057         caso++;
00058         continue;
00059       }
00060 
00061       Vector2 pto2;
00062       Vector3 pto3;
00063 
00064       cerr << setprecision(10) << fixed;
00065       switch(caso)
00066       {
00067          case 0: // Calibracion
00068             for(int i = 0; i < t.getnPalabras(); i++)
00069             {
00070                dat.cal(i) = atof(t.token(i));
00071             }
00072             //cerr << "Calibracion: " << dat.cal.transpose() << endl;
00073             break;
00074          case 1: // Camara
00075             dat.cam = Matrix4d::Identity();
00076             for(int i = 0; i < t.getnPalabras(); i++)
00077             {
00078                int f = i%3, c = i/3;
00079                dat.cam(f,c) = atof(t.token(i));
00080             }
00081             cerr << "cam: " << dat.cam << endl;
00082             break;
00083          case 2: // Puntos 2D
00084             //cerr << "ptos2D:\n";
00085             for(int i = 0; i < t.getnPalabras(); i++)
00086             {
00087                pto2(i) = atof(t.token(i));
00088             }
00089             //cerr << pto2 << endl;
00090             dat.pto2D.push_back(pto2);
00091             break;
00092          case 3: // Puntos 3D
00093             //cerr << "ptos3D:\n";
00094             for(int i = 0; i < t.getnPalabras(); i++)
00095             {
00096                pto3(i) = atof(t.token(i));
00097             }
00098             //cerr << pto3 << endl;
00099             dat.pto3D.push_back(pto3);
00100             break;
00101          case 4: // sol Matlab
00102             dat.solMatlab = Matrix4d::Identity();
00103             for(int i = 0; i < t.getnPalabras(); i++)
00104             {
00105                int f = i%3, c = i/3;
00106                dat.solMatlab(f,c) = atof(t.token(i));
00107             }
00108             //cerr << "mat: " << dat.solMatlab << endl;
00109          default:
00110             ;
00111       }
00112    }
00113 }
00114 
00115 
00116 /*****************************************************************************/
00117 /*****************************************************************************/
00118 /*****************************************************************************/
00119 /*****************************************************************************/
00120 
00121 
00122 int main(int argc, const char* argv[])
00123 {
00124 
00125   Data dat;
00126   loadData(dat);
00127 
00128   poseOptimization(dat);
00129   
00130 
00131 #if 0
00132 /******************************************************************************/
00133 // Configuracion del resolvedor
00134 
00135   g2o::SparseOptimizer optimizer;
00136   optimizer.setMethod(g2o::SparseOptimizer::LevenbergMarquardt);
00137   optimizer.setVerbose(true);
00138 
00139   g2o::BlockSolverX::LinearSolverType * linearSolver;
00140 //  if (DENSE)
00141 //  {
00142 //     cerr << "MUY DENSO\n";
00143      linearSolver =
00144        new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
00145 //  }else
00146   /*{
00147     linearSolver =
00148        new g2o::LinearSolverCholmod<g2o::BlockSolverX::PoseMatrixType>();
00149        //new g2o::LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>();
00150        //new g2o::LinearSolverPCG<g2o::BlockSolverX::PoseMatrixType>();
00151 //  }*/
00152 
00153 
00154   g2o::BlockSolverX * solver_ptr =
00155      new g2o::BlockSolverX(&optimizer,linearSolver);
00156 
00157 
00158   optimizer.setSolver(solver_ptr);
00159 
00160 /******************************************************************************/
00161 
00162   int vertex_id = 0;
00163 
00164 
00168   g2o::VertexSE3AxisAngle * v_se3 = new g2o::VertexSE3AxisAngle(dat.cal, dat.cam);
00169 
00170   v_se3->setId(vertex_id);
00171 
00172   optimizer.addVertex(v_se3);
00173   vertex_id++;
00174 
00175 
00179   for (size_t i=0; i < dat.pto3D.size(); ++i)
00180   {
00181     g2o::VertexPointXYZ * v_p = new g2o::VertexPointXYZ();
00182 
00183     v_p->setId(vertex_id);
00184     v_p->setMarginalized(true);
00185     v_p->estimate() = dat.pto3D[i];
00186 
00187     v_p->setFixed(true);
00188 
00189     g2o::EdgeProjectXYZ2UV * e = new g2o::EdgeProjectXYZ2UV();
00190     e->vertices()[0] = dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_se3);
00191     e->vertices()[1] = dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p);
00192     e->measurement() = dat.pto2D[i];
00193     e->inverseMeasurement() = -dat.pto2D[i];
00194 
00195     e->information() = Matrix2d::Identity();
00196     e->setRobustKernel(false);
00197     e->setHuberWidth(1);
00198 
00199     optimizer.addEdge(e);
00200     optimizer.addVertex(v_p);
00201     vertex_id ++;
00202   }
00203 
00204 
00208 
00209   cout << "\n ----------------------------------- \n\n";
00210 
00211   optimizer.initializeOptimization();
00212   optimizer.setVerbose(true);
00213 
00214   optimizer.optimize(ITERACIONES);
00215 
00216   cerr << "Camara final: " << v_se3->getCamera() << endl;
00217   cerr << "Camara Matlab: " << dat.solMatlab << endl;
00218   cerr << "AAAA " << dat.cam << endl;
00219 #endif
00220 }
00221 
00222 
00223 


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