ba_demoPTAM5.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 <types_g2o_ptam.h>
00015 
00016 #include "tokenizer.h"
00017 
00018 #include <iostream>
00019 #include <fstream>
00020 
00021 //const int W = 640;
00022 //const int H = 480;
00023 
00024 unsigned int CAMARAS = 700;
00025 unsigned int PUNTOS;
00026 
00027 const int ITERACIONES = 200;
00028 
00029 const bool CAL_CORRECTA = false;
00030 
00031 const bool FIJAS = true;
00032 const unsigned int NUM_FIJAS = 1;//1;
00033 const bool RUIDO_CAM = false;
00034 
00035 const bool PTOS_FIJOS = false;
00036 const bool RUIDO_PTOS = false;
00037 
00038 
00039 using namespace Eigen;
00040 using namespace std;
00041 
00042 
00043 class Sample
00044 {
00045 
00046   static tr1::ranlux_base_01 gen_real;
00047   static tr1::mt19937 gen_int;
00048 public:
00049   static int uniform(int from, int to);
00050 
00051   static double uniform();
00052 
00053   static double gaussian(double sigma);
00054 };
00055 
00056 
00057 tr1::ranlux_base_01 Sample::gen_real;
00058 tr1::mt19937 Sample::gen_int;
00059 
00060 int Sample::uniform(int from, int to)
00061 {
00062   tr1::uniform_int<int> unif(from, to);
00063   int sam = unif(gen_int);
00064   return  sam;
00065 }
00066 
00067 double Sample::uniform()
00068 {
00069   std::tr1::uniform_real<double> unif(0.0, 1.0);
00070   double sam = unif(gen_real);
00071   return  sam;
00072 }
00073 
00074 double Sample::gaussian(double sigma)
00075 {
00076   if(sigma < 1e-10) return 0;
00077   std::tr1::normal_distribution<double> gauss(0.0, sigma);
00078   double sam = gauss(gen_real);
00079   return  sam;
00080 }
00081 
00082 
00083 // CARGAR EL GROUND TRUTH
00084 typedef struct
00085 {
00086   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00087   int h, w;
00088   Vector5 cal;
00089   vector < Matrix4, aligned_allocator<Matrix4> > cam;
00090   vector < Vector3, aligned_allocator<Vector3> > pto;
00091 } GroundTruth;
00092 
00093 void GT(GroundTruth &gt)
00094 {
00095    Token t;
00096    if(t.abrirFichero("experimento.mat") != Token::ok)
00097    {
00098       cerr << "FICHERO INVALIDO\n";
00099       exit (-1);
00100    }
00101 
00102    int caso = 0;
00103    while(t.leeLinea() == Token::ok)
00104    {
00105       if(t.tokenizar(" \t") == 0)
00106       {
00107         caso++;
00108         continue;
00109       }
00110 
00111       Matrix4 cam;
00112       cam = Matrix4d::Identity();
00113       Vector3 pto;
00114       cerr << setprecision(10) << fixed;
00115       switch(caso)
00116       {
00117          case 0: // Calibracion
00118             gt.w = atoi(t.token(0));
00119             gt.h = atoi(t.token(1));
00120             for(int i = 0; i < t.getnPalabras()-2; i++)
00121             {
00122                gt.cal(i) = atof(t.token(i+2));
00123             }
00124             break;
00125          case 1: // Camaras
00126             for(int i = 0; i < t.getnPalabras(); i++)
00127             {
00128                int f = i%3, c = i/3;
00129                //cerr << "i: " << i << "  (f,c): (" << f << "," << c << ")\n";
00130                //cam(i) = atof(t.token(i));
00131                cam(f,c) = atof(t.token(i));
00132             }
00133             //cerr << fixed << setprecision(5) << cam << endl;
00134             gt.cam.push_back(cam);
00135             break;
00136          case 2: // Puntos
00137             for(int i = 0; i < t.getnPalabras(); i++)
00138             {
00139                pto(i) = atof(t.token(i));
00140             }
00141             gt.pto.push_back(pto);
00142          default:
00143             ;
00144       }
00145    }
00146    if (CAMARAS > gt.cam.size()) CAMARAS = gt.cam.size();
00147    PUNTOS = gt.pto.size();
00148 }
00150 
00151 
00152 // MATLAB
00153 typedef Matrix<double,2,1> Vector2;
00154 
00155 typedef struct
00156 {
00157    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00158    int n, fija;
00159    //SE3AxisAngle c;
00160    //Vector6 c;
00161    Matrix4 Ecw;
00162 } CamaraMatlab;
00163 
00164 typedef struct
00165 {
00166    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00167    int n;
00168    //VertexPointXYZ p;
00169    Vector3 p;
00170 } PuntoMatlab;
00171 
00172 typedef struct
00173 {
00174    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00175    int cam, pto, espurio;
00176    Vector2d m;
00177 } MedicionMatlab;
00179 
00180 
00181 /*****************************************************************************/
00182 /*****************************************************************************/
00183 /*****************************************************************************/
00184 /*****************************************************************************/
00185 
00186 
00187 int main(int argc, const char* argv[])
00188 {
00189   if (argc<2)
00190   {
00191     cout << "\nPlease type:\n"
00192          << "ba_demo [PIXEL_NOISE] [OUTLIER RATIO] [ROBUST_KERNEL] "
00193          << "[STRUCTURE_ONLY] [DENSE]\n\n"
00194          << "PIXEL_NOISE: noise in image space (E.g.: 1)\n"
00195          << "OUTLIER_RATIO: probability of spuroius observation  "
00196          << "(default: 0.0)\n"
00197          << "ROBUST_KERNEL: use robust kernel (0 or 1; default: 0==false)\n"
00198          << "STRUCTURE_ONLY: performe structure-only BA to get better point "
00199          << "initializations (0 or 1; default: 0==false)\n"
00200          << "DENSE: Use dense solver (0 or 1; default: 0==false)\n\n"
00201          << "Note, if OUTLIER_RATIO is above 0, ROBUST_KERNEL should be set to "
00202          << "1==true.\n\n";
00203     exit(0);
00204   }
00205 
00206 
00207   GroundTruth gt;
00208   GT(gt);
00209 
00210   double PIXEL_NOISE = atof(argv[1]);
00211 
00212   double OUTLIER_RATIO = 0.0;
00213 
00214   if (argc>2)
00215   {
00216     OUTLIER_RATIO = atof(argv[2]);
00217   }
00218 
00219   bool ROBUST_KERNEL = false;
00220   if (argc>3)
00221   {
00222     ROBUST_KERNEL = atof(argv[3]);
00223   }
00224   bool STRUCTURE_ONLY = false;
00225   if (argc>4)
00226   {
00227     STRUCTURE_ONLY = atof(argv[4]);
00228   }
00229 
00230   bool DENSE = false;
00231   if (argc>5)
00232   {
00233     DENSE = atof(argv[5]);
00234   }
00235 
00236   cout << "PIXEL_NOISE: " <<  PIXEL_NOISE << endl
00237        << "OUTLIER_RATIO: " << OUTLIER_RATIO<<  endl
00238        << "ROBUST_KERNEL: " << ROBUST_KERNEL << endl
00239        << "STRUCTURE_ONLY: " << STRUCTURE_ONLY<< endl
00240        << "DENSE: "<<  DENSE << endl;
00241 
00242 
00243 
00244 /******************************************************************************/
00245 // Configuracion del resolvedor
00246 
00247   g2o::SparseOptimizer optimizer;
00248   optimizer.setMethod(g2o::SparseOptimizer::LevenbergMarquardt);
00249   optimizer.setVerbose(true);
00250 
00251   g2o::BlockSolverX::LinearSolverType * linearSolver;
00252   if (DENSE)
00253   {
00254      cerr << "MUY DENSO\n";
00255      linearSolver =
00256        new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
00257   }else
00258   {
00259     linearSolver = 
00260        new g2o::LinearSolverCholmod<g2o::BlockSolverX::PoseMatrixType>();
00261        //new g2o::LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>();
00262        //new g2o::LinearSolverPCG<g2o::BlockSolverX::PoseMatrixType>();
00263   }
00264 
00265 
00266   g2o::BlockSolverX * solver_ptr =
00267      new g2o::BlockSolverX(&optimizer,linearSolver);
00268 
00269 
00270   optimizer.setSolver(solver_ptr);
00271 
00272 /******************************************************************************/
00273 
00274   int vertex_id = 0;
00275 
00276 
00277   // MATLAB
00278   CamaraMatlab*   cM;
00279   PuntoMatlab*    pM;
00280   MedicionMatlab* mM;
00281   vector<CamaraMatlab*>   camGT, cam0, camF;
00282   vector<PuntoMatlab*>    ptoGT, pto0, ptoF;
00283   vector<MedicionMatlab*> med,  medGT;
00285 
00286 
00290 
00291   g2o::VertexKFOV5 *vK0, *vKF, *vKGT;
00292 
00293   vKGT = new g2o::VertexKFOV5(gt.cal[0], gt.cal[1], gt.cal[2], gt.cal[3], gt.cal[4]);
00294   if(CAL_CORRECTA)
00295   {
00296      vK0 = new g2o::VertexKFOV5(gt.cal[0], gt.cal[1], gt.cal[2], gt.cal[3], gt.cal[4]);
00297      vK0->setFixed(true);
00298      vKF = new g2o::VertexKFOV5(gt.cal[0], gt.cal[1], gt.cal[2], gt.cal[3], gt.cal[4]);
00299      vKF->setFixed(true);
00300   }else
00301   {
00302 //     double f = gt.w/(2*tan(60*M_PI/180));
00303 //     vK0 = new g2o::VertexKFOV5(f, f, gt.w>>1, gt.h>>1, 60*M_PI/180);
00304 //     vKF = new g2o::VertexKFOV5(f, f, gt.w>>1, gt.h>>1, 60*M_PI/180);
00305      Vector5 cal;
00306      cal(0) = /*Sample::gaussian(0.05*gt.cal[0]) +*/ 1.05 *gt.cal[0];
00307      cal(1) = /*Sample::gaussian(0.05*gt.cal[1]) +*/ 1.05 * gt.cal[1];
00308      cal(2) = /*Sample::gaussian(0.05*gt.cal[2]) + */ 1.05 *gt.cal[2];
00309      cal(3) = /*Sample::gaussian(0.05*gt.cal[3]) + */ 1.05 *gt.cal[3];
00310      cal(4) = /*Sample::gaussian(0.05*gt.cal[4]) +*/ 1.05 * gt.cal[4];
00311      vK0 = new g2o::VertexKFOV5(cal[0], cal[1], cal[2], cal[3], cal[4]);
00312      vKF = new g2o::VertexKFOV5(cal[0], cal[1], cal[2], cal[3], cal[4]);
00313 
00314      vK0->setFixed(false);
00315      vKF->setFixed(false);
00316   }
00317   vKF->setId(vertex_id);
00318   optimizer.addVertex(vKF);
00319   vertex_id++;
00320 
00321 
00323 
00327   vector<Matrix4, aligned_allocator<Matrix4> > true_cameras;
00328   for (size_t i=0; i<CAMARAS; ++i)
00329   {
00330     Matrix4 Ecw;
00331     Ecw = gt.cam[i];
00332     Vector6 mu;
00333     true_cameras.push_back(Ecw);
00334 
00335     // MATLAB
00336     cM = new CamaraMatlab;
00337     cM->Ecw  = Ecw; 
00339 
00340     if(RUIDO_CAM && i > 1)     // ANIADIR RUIDO A mu
00341     {
00342        mu(0) = Sample::gaussian(0.2);
00343        mu(1) = Sample::gaussian(0.2);
00344        mu(2) = Sample::gaussian(0.2);
00345        mu(3) = Sample::gaussian(0.09);
00346        mu(4) = Sample::gaussian(0.09);
00347        mu(5) = Sample::gaussian(0.09);
00348        Ecw = SE3AxisAngle::exp(mu) * Ecw;
00349        //mu.Zero();
00350     }
00351 
00352     //mu.Zero();
00353     //mu(0) = i*0.04-1.; mu(1) = mu(2) = mu(3) = mu(4) = mu(5) = 0;
00354     //mu(0) = (int(i)-1)*2.5; mu(1) = 0; mu(2) = -5;
00355     //mu(3) = mu(4) = mu(5) = 0;
00356 
00357     g2o::VertexSE3AxisAngle * v_se3 = new g2o::VertexSE3AxisAngle(Ecw);
00358 
00359     v_se3->setId(vertex_id);
00360     //v_se3->setToOrigin();
00361 
00362     if (FIJAS && i<NUM_FIJAS)   v_se3->setFixed(true);
00363 
00364     optimizer.addVertex(v_se3);
00365     vertex_id++;
00366 
00367     // MATLAB
00368     cM->n    = v_se3->id();
00369     cM->fija = v_se3->fixed();
00370     camGT.push_back(cM);
00371 
00372     cM = new CamaraMatlab;
00373     cM->n    = v_se3->id();
00374     cM->fija = v_se3->fixed();
00375     cM->Ecw  = Ecw; 
00376     cam0.push_back(cM);
00378 
00379   }
00380 
00382 
00386   vector<Vector3> true_points;
00387   for (size_t i=0; i < PUNTOS; ++i)   true_points.push_back(gt.pto[i]);
00388 
00389 
00391 
00395 
00396   int point_id = vertex_id;
00397 
00398   int point_num = 0;
00399   double sum_diff2 = 0;
00400 
00401   cout << endl;
00402   tr1::unordered_map<int,int> pointid_2_trueid;
00403   tr1::unordered_set<int> inliers;
00404 
00405   for (size_t i=0; i<true_points.size(); ++i)
00406   {
00407     g2o::VertexPointXYZ * v_p = new g2o::VertexPointXYZ();
00408 
00409     // Estimacion inicial del punto 3 (con error)
00410     // Es lo que hay que optimizar
00411     v_p->setId(point_id);
00412     v_p->setMarginalized(true);
00413     v_p->estimate() = true_points[i];
00414     if(RUIDO_PTOS)
00415       v_p->estimate() += Vector3
00416           (Sample::gaussian(0.5), Sample::gaussian(0.5), Sample::gaussian(0.5));
00417 
00418     v_p->setFixed(PTOS_FIJOS);
00419 
00420     ++point_id;
00421 
00422 
00423     // Comprobacion de que el pto es visto por mas de 2 camaras
00424     int num_obs = 0;
00425     for (size_t j = 0; j < true_cameras.size(); ++j)
00426     {
00427       g2o::VertexSE3AxisAngle *vc = new g2o::VertexSE3AxisAngle(gt.cam[j]);
00428       //vc->setToOrigin();
00429       Vector2d z =
00430          dynamic_cast<g2o::VertexSE3AxisAngle*>
00431             (vc)->map(true_points.at(i), vKGT->estimate());
00432       delete vc;
00433       if (z[0]>=0 && z[1]>=0 && z[0]<gt.w && z[1]<gt.h)   if(++num_obs > 1) break;
00434     }
00435     if (num_obs<2)   continue;
00436 
00437 
00438     // MATLAB
00439     pM    = new PuntoMatlab;
00440     pM->n = v_p->id();
00441     pM->p = true_points[i];
00442     ptoGT.push_back(pM);
00443     
00444     pM    = new PuntoMatlab;
00445     pM->n = v_p->id();
00446     pM->p = v_p->estimate(); //true_points[i]; 
00447     pto0.push_back(pM);
00449 
00450     // Buscar las camaras en las que se ve y calcular su respectiva medicion.
00451     // Si el pto es considerado en una camara como espurio => ya se queda como 
00452     // espurio y en el tratamiento de los errores no se tiene en cuenta pero si
00453     // que cuenta a la hora de repartir el error de los ptos no espurios.
00454     // Creo que esta politica esta MAL. La he modificado para que no se tengan 
00455     // en cuenta en el calculo del error medio :) y aun asi sigue estando mal
00456     // que si un pto en una medicion es espurio lo sea en las demas
00457     bool inlier = true;
00458     for (size_t j = 0; j < true_cameras.size(); ++j)
00459     {
00460       g2o::VertexSE3AxisAngle *vc = new g2o::VertexSE3AxisAngle(gt.cam[j]);
00461       //vc->setToOrigin();
00462       Vector2d z =
00463          dynamic_cast<g2o::VertexSE3AxisAngle*>
00464             (vc)->map(true_points.at(i), vKGT->estimate());
00465       delete vc;
00466 
00467       if (z[0] >= 0 && z[1] >= 0 && z[0] < gt.w && z[1] < gt.h)
00468       {
00469 
00470         // MATLAB
00471         mM = new MedicionMatlab;
00472         mM->cam     = optimizer.vertices().find(j+1)->second->id();
00473         mM->pto     = v_p->id();
00474         mM->espurio = 0;
00475         mM->m       = z;
00476         medGT.push_back(mM);
00478 
00479         double sam = Sample::uniform();
00480         if (sam < OUTLIER_RATIO)
00481         {
00482           z = Vector2d(Sample::uniform(0, gt.w), Sample::uniform(0, gt.h));
00483           inlier= false;
00484         }
00485 
00486         // Medida + Error de medida
00487         z += Vector2d(Sample::gaussian(PIXEL_NOISE),
00488                       Sample::gaussian(PIXEL_NOISE));
00489 
00490 
00491         // MATLAB
00492         mM = new MedicionMatlab;
00493         mM->cam     = optimizer.vertices().find(j+1)->second->id();
00494         mM->pto     = v_p->id();
00495         mM->espurio = inlier?0:1;
00496         mM->m       = z;
00497         med.push_back(mM);
00499 
00500 
00501         g2o::EdgeKProjectXYZ2UV * e = new g2o::EdgeKProjectXYZ2UV();
00502         e->vertices()[0] = dynamic_cast<g2o::OptimizableGraph::Vertex*>(vKF);
00503         e->vertices()[1] = 
00504            dynamic_cast<g2o::OptimizableGraph::Vertex*>
00505               (optimizer.vertices().find(j+1)->second);  //j+1
00506         e->vertices()[2] = dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p);
00507 
00508         e->measurement() = z;
00509         e->inverseMeasurement() = -z;
00510         e->information() = Matrix2d::Identity()/(PIXEL_NOISE*PIXEL_NOISE);
00511         e->setRobustKernel(ROBUST_KERNEL);
00512         e->setHuberWidth(1);
00513         optimizer.addEdge(e);
00514       }
00515     }
00516 
00517     if (inlier)
00518     {
00519       inliers.insert(v_p->id());
00520       Vector3 diff = v_p->estimate() - true_points[i];
00521 
00522       sum_diff2 += diff.dot(diff);
00523       ++point_num;
00524     }
00525 
00526     optimizer.addVertex(v_p);
00527 
00528     pointid_2_trueid.insert(make_pair(v_p->id(),i));
00529   }
00530 
00531 
00532 
00533 
00534 
00535 
00536 
00537 
00539 
00543 
00544   cout << "\n ----------------------------------- \n\n";
00545 
00546   optimizer.initializeOptimization();
00547   optimizer.setVerbose(true);
00548 
00549   if (STRUCTURE_ONLY)
00550   {
00551     // Deja fijas las camaras y solo optimiza los ptos
00552     cout << "Performing structure-only BA:"   << endl;
00553     g2o::StructureOnlySolver<3> structure_only_ba;
00554     structure_only_ba.setVerbose(true);
00555     structure_only_ba.calc(optimizer.vertices(), 10);
00556   }
00557 
00558   cout << "\nPerforming full BA:" << endl;
00559   optimizer.optimize(ITERACIONES);
00560 
00561 
00563 
00567 
00568   cout << "\nPoint error before optimisation (inliers only): " 
00569        << sqrt(sum_diff2/point_num) << endl;
00570 
00571 
00572   // Calculo del error tras las optimizacion
00573   point_num = 0;
00574   sum_diff2 = 0;
00575   for (tr1::unordered_map<int,int>::iterator 
00576        it  = pointid_2_trueid.begin();
00577        it != pointid_2_trueid.end();
00578        ++it)
00579   {
00580     g2o::HyperGraph::VertexIDMap::iterator v_it =
00581        optimizer.vertices().find(it->first);
00582 
00583     if (v_it == optimizer.vertices().end())
00584     {
00585       cerr << "Vertex " << it->first << " not in graph!" << endl;
00586       exit(-1);
00587     }
00588 
00589     g2o::VertexPointXYZ * v_p =
00590        dynamic_cast< g2o::VertexPointXYZ * > (v_it->second);
00591 
00592     if (v_p == 0)
00593     {
00594       cerr << "Vertex " << it->first << "is not a PointXYZ!" << endl;
00595       exit(-1);
00596     }
00597 
00598     // MATLAB
00599     pM    = new PuntoMatlab;
00600     pM->n = v_p->id();
00601     pM->p = v_p->estimate(); //true_points[i]; 
00602     ptoF.push_back(pM);
00604     
00605 
00606     Vector3 diff = v_p->estimate() - true_points[it->second];
00607 
00608     if (inliers.find(it->first)==inliers.end())   continue;
00609 
00610     sum_diff2 += diff.dot(diff);
00611     ++point_num;
00612   }
00613 
00614   cout << "Point error after optimisation (inliers only): " 
00615        << sqrt(sum_diff2/point_num) << endl << endl;
00616 
00617 
00618 
00619   // Ploteo de las camaras optimizadas
00620   for (size_t j=0; j<true_cameras.size(); ++j)
00621   {
00622     g2o::HyperGraph::VertexIDMap::iterator v_it = optimizer.vertices().find(j+1);
00623     g2o::VertexSE3AxisAngle * v_c =
00624        dynamic_cast< g2o::VertexSE3AxisAngle * > (v_it->second);
00625 
00626     // MATLAB
00627     cM = new CamaraMatlab;
00628     cM->n      = v_c->id();
00629     cM->fija   = v_c->fixed();
00630 //    cM->c    = v_c->estimate().getAxisAngle().transpose();
00631     cM->Ecw    = v_c->getCamera();
00632     camF.push_back(cM);
00634   }
00635 
00636 
00637   // Ploteo de la calibracion optimizadas
00638 //  g2o::HyperGraph::VertexIDMap::iterator v_it = optimizer.vertices().find(0);
00639 //  vKF = dynamic_cast< g2o::VertexKFOV5 * > (v_it->second);
00640 
00642 
00643 
00647 
00648   ofstream file ("g2o.m", ofstream::out | ofstream::trunc);
00649 
00650   if (!file) return -1;
00651   
00652   file << setprecision(15) << fixed;
00653 
00654 
00655   file << "kGT = [" << vKGT->estimate().transpose() << "];\n\n\n";
00656 
00657 
00658   file << "camsGT = [\n";
00659   for(size_t c = 0; c < camGT.size(); c++)
00660   {
00661      file << camGT[c]->n << " " << camGT[c]->fija << " "
00662           << camGT[c]->Ecw.col(0).transpose() << " "
00663           << camGT[c]->Ecw.col(1).transpose() << " "
00664           << camGT[c]->Ecw.col(2).transpose() << " "
00665           << camGT[c]->Ecw.col(3).transpose() << endl;
00666   }
00667   file << "];\n\n\n";
00668 
00669 
00670   file << "ptosGT = [\n";
00671   for(size_t p=0; p < ptoGT.size(); p++)
00672   {
00673      file << ptoGT[p]->n << " " << ptoGT[p]->p.transpose() << endl;
00674   }
00675   file << "];\n\n\n";
00676 
00677 
00678 
00679   file << "medsGT = [\n";
00680   for(size_t m=0; m < medGT.size(); m++)
00681   {
00682      file << medGT[m]->cam << " " << medGT[m]->pto << " "
00683           << medGT[m]->espurio << " " << medGT[m]->m.transpose() << endl;
00684   }
00685   file << "];\n\n\n";
00686 
00687 
00688 
00689   file << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n"
00690        << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n"
00691        << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n";
00692 
00693 
00694 
00695   file << "k0 = [" << vK0->estimate().transpose() << "];\n\n\n";
00696 
00697 
00698   file << "cams0 = [\n";
00699   for(size_t c = 0; c < cam0.size(); c++)
00700   {
00701      file << cam0[c]->n << " " << cam0[c]->fija << " "
00702           << cam0[c]->Ecw.col(0).transpose() << " "
00703           << cam0[c]->Ecw.col(1).transpose() << " "
00704           << cam0[c]->Ecw.col(2).transpose() << " "
00705           << cam0[c]->Ecw.col(3).transpose() << endl;
00706   }
00707   file << "];\n\n\n";
00708 
00709 
00710   file << "ptos0 = [\n";
00711   for(size_t p=0; p < pto0.size(); p++)
00712   {
00713      file << pto0[p]->n << " " << pto0[p]->p.transpose() << endl;
00714   }
00715   file << "];\n\n\n";
00716 
00717 
00718   file << "meds0 = [\n";
00719   for(size_t m=0; m < med.size(); m++)
00720   {
00721      file << med[m]->cam << " " << med[m]->pto << " " 
00722           << med[m]->espurio << " " << med[m]->m.transpose() << endl;
00723   }
00724   file << "];\n\n\n";
00725 
00726 
00727 
00728   file << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n"
00729        << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n"
00730        << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n";
00731 
00732 
00733 
00734   file << "kF = [" << vKF->estimate().transpose() << "];\n\n\n";
00735 
00736 
00737   file << "camsF = [\n";
00738   for(size_t c = 0; c < camF.size(); c++)
00739   {
00740      file << camF[c]->n << " " << camF[c]->fija << " "
00741           << camF[c]->Ecw.col(0).transpose() << " "
00742           << camF[c]->Ecw.col(1).transpose() << " "
00743           << camF[c]->Ecw.col(2).transpose() << " "
00744           << camF[c]->Ecw.col(3).transpose() << endl;
00745   }
00746   file << "];\n\n\n";
00747 
00748 
00749   file << "ptosF = [\n";
00750   for(size_t p=0; p < ptoF.size(); p++)
00751   {
00752      file << ptoF[p]->n << " " << ptoF[p]->p.transpose() << endl;
00753   }
00754   file << "];\n\n\n";
00755 
00756   file.close();
00757 
00758   cerr << "kGT: " << vKGT->estimate().transpose() << endl
00759        << "k0 : " << vK0->estimate().transpose() << endl
00760        << "kF : " << vKF->estimate().transpose() << endl
00761        << "-------------------------------------------\n"
00762        << "kF - kGT : "
00763        << vKGT->estimate().transpose() -vKF->estimate().transpose() << endl;
00764 }
00765 
00766 
00767 


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