sba_file_io.cpp
Go to the documentation of this file.
00001 #include "sba/sba_file_io.h"
00002 #include <map>
00003 
00004 using namespace sba;
00005 using namespace Eigen;
00006 using namespace frame_common;
00007 using namespace std;
00008 
00009 int sba::readBundlerFile(const char *filename, SysSBA& sbaout)
00010 { 
00011     // Create vectors to hold the data from the bundler file. 
00012     vector< Vector3d, Eigen::aligned_allocator<Vector3d> > camps;       // cam params <f d1 d2>
00013     vector< Matrix3d, Eigen::aligned_allocator<Matrix3d> > camRs;       // cam rotation matrix
00014     vector< Vector3d, Eigen::aligned_allocator<Vector3d> > camts;       // cam translation
00015     vector< Vector3d, Eigen::aligned_allocator<Vector3d> > ptps;        // point position
00016     vector< Vector3i, Eigen::aligned_allocator<Vector3i> > ptcs;        // point color
00017     vector< vector< Vector4d, Eigen::aligned_allocator<Vector4d> > > ptts; // point tracks - each vector is <camera_index kp_idex u v>
00018 
00019     int ret = ParseBundlerFile(filename, camps, camRs, camts, ptps, ptcs, ptts);
00020     if (ret < 0)
00021         return -1;
00022         
00023     int ncams = camps.size();
00024     int npts  = ptps.size();
00025     int nprjs = 0;
00026     for (int i=0; i<npts; i++)
00027         nprjs += (int)ptts[i].size();
00028     /* cout << "Points: " << npts << "  Tracks: " << ptts.size() 
00029          << "  Projections: " << nprjs << endl; */
00030          
00031     cout << "Setting up nodes..." << flush;
00032     for (int i=0; i<ncams; i++)
00033     {
00034         // camera params
00035         Vector3d &camp = camps[i];
00036         CamParams cpars = {camp[0],camp[0],0,0,0}; // set focal length, no offset
00037 
00038         //
00039         // NOTE: Bundler camera coords are rotated 180 deg around the X axis of
00040         //  the camera, so Z points opposite the camera viewing ray (OpenGL).
00041         // Note quite sure, but I think this gives the camera pose as
00042         //  [-R' -R'*t]
00043 
00044         // rotation matrix
00045         Matrix3d m180x;         // rotate 180 deg around X axis, to convert Bundler frames to SBA frames
00046         m180x << 1, 0, 0, 0, -1, 0, 0, 0, -1;
00047         Matrix3d camR = m180x * camRs[i]; // rotation matrix
00048         Quaternion<double> frq(camR.transpose());       // camera frame rotation, from Bundler docs
00049         if (frq.w() < 0.0)      // w negative, change to positive
00050         {
00051             frq.x() = -frq.x();
00052             frq.y() = -frq.y();
00053             frq.z() = -frq.z();
00054             frq.w() = -frq.w();
00055         }
00056         
00057         frq.normalize();
00058 
00059         // translation
00060         Vector3d &camt = camts[i];
00061         Vector4d frt;
00062         frt.head<3>() = -camRs[i].transpose() * camt; // camera frame translation, from Bundler docs
00063         frt[3] = 1.0;
00064 
00065         Node nd;
00066         
00067         sbaout.addNode(frt, frq, cpars);
00068     }
00069     // cout << "done" << endl;
00070 
00071     // set up points
00072     cout << "Setting up points..." << flush;
00073     for (int i=0; i<npts; i++)
00074     {
00075         // point
00076         Vector3d &ptp = ptps[i];
00077         Point pt;
00078         pt.head<3>() = ptp;
00079         pt[3] = 1.0;
00080         sbaout.addPoint(pt);
00081     }
00082     // cout << "done" << endl;
00083 
00084 
00085     sbaout.useLocalAngles = true;    // use local angles
00086     sbaout.nFixed = 1;
00087 
00088     // set up projections
00089     int ntot = 0;
00090     cout << "Setting up projections..." << flush;
00091     for (int i=0; i<npts; i++)
00092     {
00093       // track
00094       vector<Vector4d, Eigen::aligned_allocator<Vector4d> > &ptt = ptts[i];
00095       int nprjs = ptt.size();
00096       for (int j=0; j<nprjs; j++)
00097         {
00098           // projection
00099           Vector4d &prj = ptt[j];
00100           int cami = (int)prj[0];
00101           Vector2d pt = prj.segment<2>(2);
00102           pt[1] = -pt[1];       // NOTE: Bundler image Y is reversed
00103           if (cami >= ncams)
00104             cout << "*** Cam index exceeds bounds: " << cami << endl;
00105           sbaout.addMonoProj(cami,i,pt); // Monocular projections
00106           ntot++;
00107         }
00108     }
00109     cout << "done" << endl;
00110     
00111     return 0;
00112 }
00113 
00114 int sba::writeBundlerFile(const char *filename, SysSBA& sbain)
00115 {
00116     ofstream outfile(filename, ios_base::trunc);
00117     if (outfile == NULL)
00118     {
00119         cout << "Can't open file " << filename << endl;
00120         return -1;
00121     }
00122     
00123     outfile.precision(10);
00124     outfile.setf(ios_base::scientific);
00125     
00126     unsigned int i = 0;
00127     
00128     outfile << "# Bundle file v0.3" << endl;
00129     // First line is number of cameras and number of points.
00130     outfile << sbain.nodes.size() << ' ' << sbain.tracks.size() << endl;
00131     
00132     // Set up transform matrix for camera parameters
00133     Matrix3d m180x;             // rotate 180 deg around X axis, to convert Bundler frames to SBA frames
00134     m180x << 1, 0, 0, 0, -1, 0, 0, 0, -1;
00135     
00136     
00137     // Then goes information about each camera, in <f> <k1> <k2>\n<R>\n<t> format.
00138     for (i = 0; i < sbain.nodes.size(); i++)
00139     {
00140         // Assuming fx = fy and using fx. Don't use k1 or k2.
00141         outfile << sbain.nodes[i].Kcam(0, 0) << ' ' << 0.0 << ' ' << 0.0 << endl;
00142         
00143         Quaternion<double> quat(sbain.nodes[i].qrot);
00144         /* cout << "\nQuat: [ " << sbain.nodes[i].qrot << " ]\n"; */ 
00145         quat.normalize();
00146         Matrix3d rotmat = m180x * quat.toRotationMatrix().transpose();
00147                        
00148         outfile << rotmat(0, 0) << ' ' << rotmat(0, 1) << ' ' << rotmat(0, 2) << endl;
00149         outfile << rotmat(1, 0) << ' ' << rotmat(1, 1) << ' ' << rotmat(1, 2) << endl;
00150         outfile << rotmat(2, 0) << ' ' << rotmat(2, 1) << ' ' << rotmat(2, 2) << endl;
00151         
00152         Vector3d trans = sbain.nodes[i].trans.head<3>();
00153         trans = -rotmat*trans;
00154         outfile << trans(0) << ' ' << trans(1) << ' ' << trans(2) << endl; 
00155     }
00156     
00157     outfile.setf(ios_base::fixed);
00158     
00159     // Then goes information about each point. <pos>\n<color>\n<viewlist>
00160     for (i = 0; i < sbain.tracks.size(); i++)
00161     {
00162         // World <x y z>
00163         outfile << sbain.tracks[i].point(0) << ' ' << sbain.tracks[i].point(1) 
00164                 << ' ' << sbain.tracks[i].point(2) << endl;
00165         // Color <r g b> (Just say white instead)
00166         outfile << "255 255 255" << endl;
00167         // View list: <list_length><camera_index key u v>\n<camera_index key u v>
00168         // Key is the keypoint # in SIFT, but we just use point number instead.
00169         // We output these as monocular points because the file format does not
00170         // support stereo points.
00171         
00172         ProjMap &prjs = sbain.tracks[i].projections;
00173         
00174         // List length
00175         outfile << prjs.size() << ' ';
00176         
00177         // Output all the tracks as monocular tracks.
00178         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00179         {
00180             Proj &prj = itr->second;
00181             // y is reversed (-y)
00182             Node &node = sbain.nodes[prj.ndi];
00183             
00184             double cx = node.Kcam(0, 2);
00185             double cy = node.Kcam(1, 2);
00186             
00187             outfile << prj.ndi << ' ' << i << ' ' << prj.kp(0)-cx << ' ' 
00188                     << -(prj.kp(1)-cy) << ' ';
00189         }
00190         
00191         outfile << endl;
00192     }
00193 
00194     return 0;
00195 } 
00196 
00197 int  sba::ParseBundlerFile(const char *fin,     // input file
00198                 vector< Vector3d, Eigen::aligned_allocator<Vector3d> > &camp, // cam params <f d1 d2>
00199                 vector< Matrix3d, Eigen::aligned_allocator<Matrix3d> > &camR, // cam rotation matrix
00200                 vector< Vector3d, Eigen::aligned_allocator<Vector3d> > &camt, // cam translation
00201                 vector< Vector3d, Eigen::aligned_allocator<Vector3d> > &ptp, // point position
00202                 vector< Vector3i, Eigen::aligned_allocator<Vector3i> > &ptc, // point color
00203                 vector< vector< Vector4d, Eigen::aligned_allocator<Vector4d> > > &ptt // point tracks - each vector is <camera_index u v>
00204                 )
00205 {
00206     ifstream ifs(fin);
00207     if (ifs == NULL)
00208       {
00209         cout << "Can't open file " << fin << endl;
00210         return -1;
00211       }
00212     ifs.precision(10);
00213 
00214 
00215     // read header
00216     string line;
00217     if (!getline(ifs,line) || line != "# Bundle file v0.3")
00218       {
00219         cout << "Bad header" << endl;
00220         return -1;
00221       }
00222     cout << "Found Bundler 3.0 file" << endl;
00223 
00224     // read number of cameras and points
00225     int ncams, npts;
00226     if (!(ifs >> ncams >> npts))
00227       {
00228         cout << "Bad header" << endl;  
00229         return -1;
00230       }
00231     cout << "Number of cameras: " << ncams << "  Number of points: " << npts << endl;
00232     
00233     cout << "Reading in camera data..." << flush;
00234     for (int i=0; i<ncams; i++)
00235       {
00236         double v1,v2,v3,v4,v5,v6,v7,v8,v9;
00237         if (!(ifs >> v1 >> v2 >> v3))
00238           {
00239             cout << "Bad camera params at number " << i << endl;
00240             return -1;
00241           }
00242         camp.push_back(Vector3d(v1,v2,v3));
00243 
00244         if (!(ifs >> v1 >> v2 >> v3 >> v4 >> v5 >> v6 >> v7 >> v8 >> v9))
00245           {
00246             cout << "Bad camera rotation matrix at number " << i << endl;
00247             return -1;
00248           }
00249         Matrix3d m;
00250         m << v1,v2,v3,v4,v5,v6,v7,v8,v9;
00251         camR.push_back(m);
00252 
00253         if (!(ifs >> v1 >> v2 >> v3))
00254           {
00255             cout << "Bad camera translation at number " << i << endl;
00256             return -1;
00257           }
00258         camt.push_back(Vector3d(v1,v2,v3));
00259       }
00260     cout << "done" << endl;
00261 
00262     ptt.resize(npts);
00263 
00264     cout << "Reading in pts data..." << flush;
00265     for (int i=0; i<npts; i++)
00266       {
00267         double v1,v2,v3;
00268         int i1,i2,i3;
00269 
00270         if (!(ifs >> v1 >> v2 >> v3))
00271           {
00272             cout << "Bad point position at number " << i << endl;
00273             return -1;
00274           }
00275         ptp.push_back(Vector3d(v1,v2,v3));
00276 
00277         if (!(ifs >> i1 >> i2 >> i3))
00278           {
00279             cout << "Bad point color at number " << i << endl;
00280             return -1;
00281           }
00282         ptc.push_back(Vector3i(i1,i2,i3));
00283 
00284 
00285         if (!(ifs >> i1))
00286           {
00287             cout << "Bad track count at number " << i << endl;
00288             return -1;
00289           }
00290         int nprjs = i1;
00291 
00292         vector<Vector4d, Eigen::aligned_allocator<Vector4d> > &prjs = ptt[i];
00293         for (int j=0; j<nprjs; j++)
00294           {
00295             if (!(ifs >> i1 >> i2 >> v1 >> v2))
00296               {
00297                 cout << "Bad track parameter at number " << i << endl;
00298                 return -1;
00299               }
00300             prjs.push_back(Vector4d(i1,i2,v1,v2));
00301           }
00302 
00303       } // end of pts loop
00304     cout << "done" << endl;
00305 
00306     // print some stats
00307     double nprjs = 0;
00308     for (int i=0; i<npts; i++)
00309       nprjs += ptt[i].size();
00310     cout << "Number of projections: " << (int)nprjs << endl;
00311     cout << "Average projections per camera: " << nprjs/(double)ncams << endl;
00312     cout << "Average track length: " << nprjs/(double)npts << endl;
00313     return 0;
00314 }
00315 
00316 
00317 // write out the system in an sba (Lourakis) format
00318 // NOTE: Lourakis FAQ is wrong about coordinate systems
00319 //   Cameras are represented by the w2n transform, converted to
00320 //   a quaternion and translation vector
00321 //
00322 void sba::writeLourakisFile(const char *fname, SysSBA& sba)
00323 {
00324     char name[1024];
00325     sprintf(name,"%s-cams.txt",fname);
00326     FILE *fn = fopen(name,"w");
00327     if (fn == NULL)
00328       {
00329         cout << "[WriteFile] Can't open file " << name << endl;
00330         return;
00331       }
00332     
00333     // write out initial camera poses
00334     int ncams = sba.nodes.size();
00335     for (int i=0; i<ncams; i++)
00336       {
00337         Node &nd = sba.nodes[i];
00338 
00339         // Why not just use the Quaternion???
00340         Quaternion<double> frq(nd.w2n.block<3,3>(0,0)); // rotation matrix of transform
00341         fprintf(fn,"%f %f %f %f ", frq.w(), frq.x(), frq.y(), frq.z());
00342         Vector3d tr = nd.w2n.col(3);
00343         fprintf(fn,"%f %f %f\n", tr[0], tr[1], tr[2]);
00344       }
00345     fclose(fn);
00346 
00347     sprintf(name,"%s-pts.txt",fname);
00348     fn = fopen(name,"w");
00349     if (fn == NULL)
00350       {
00351         cout << "[WriteFile] Can't open file " << name << endl;
00352         return;
00353       }
00354     
00355     fprintf(fn,"# X Y Z  nframes  frame0 x0 y0  frame1 x1 y1 ...\n");
00356 
00357     // write out points
00358     
00359     for(size_t i=0; i<sba.tracks.size(); i++)
00360       {
00361         ProjMap &prjs = sba.tracks[i].projections;
00362         // Write out point
00363         Point &pt = sba.tracks[i].point;
00364         
00365         fprintf(fn,"%f %f %f  ", pt.x(), pt.y(), pt.z());
00366         fprintf(fn,"%d  ",(int)prjs.size());
00367         
00368         // Write out projections
00369         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00370           {
00371             Proj &prj = itr->second;      
00372             if (!prj.isValid) continue;
00373             int cami = itr->first;//prj.ndi;
00374             // NOTE: Lourakis projected y is reversed (???)
00375             fprintf(fn," %d %f %f ",cami,prj.kp[0],prj.kp[1]);
00376           }
00377         fprintf(fn,"\n");
00378       }
00379 
00380     fclose(fn);
00381 
00382     // write camera calibartion
00383     sprintf(name,"%s-calib.txt",fname);
00384     fn = fopen(name,"w");
00385     if (fn == NULL)
00386       {
00387         cout << "[WriteFile] Can't open file " << name << endl;
00388         return;
00389       }
00390     
00391     Matrix3d &K = sba.nodes[0].Kcam;
00392     fprintf(fn,"%f %f %f\n", K(0,0), K(0,1), K(0,2));
00393     fprintf(fn,"%f %f %f\n", K(1,0), K(1,1), K(1,2));
00394     fprintf(fn,"%f %f %f\n", K(2,0), K(2,1), K(2,2));
00395 
00396     fclose(fn);
00397 }
00398 
00399 
00400 // write out the precision matrix
00401 void sba::writeA(const char *fname, SysSBA& sba)
00402 {
00403     ofstream ofs(fname);
00404     if (ofs == NULL)
00405       {
00406         cout << "Can't open file " << fname << endl;
00407         return;
00408       }
00409 
00410     // cameras
00411     Eigen::IOFormat pfmt(16);
00412     ofs << sba.A.format(pfmt) << endl;
00413     ofs.close();
00414 }
00415 
00416 
00417 // write out the precision matrix for CSparse
00418 void sba::writeSparseA(const char *fname, SysSBA& sba)
00419 {
00420     char name[1024];
00421     sprintf(name,"%s-A.tri",fname);
00422 
00423     {
00424       ofstream ofs(name);
00425       if (ofs == NULL)
00426         {
00427           cout << "Can't open file " << fname << endl;
00428           return;
00429         }
00430 
00431       // cameras
00432       Eigen::IOFormat pfmt(16);
00433 
00434       int nrows = sba.A.rows();
00435       int ncols = sba.A.cols();
00436     
00437       cout << "[WriteSparseA] Size: " << nrows << "x" << ncols << endl;
00438 
00439       // find # nonzeros
00440       int nnz = 0;
00441       for (int i=0; i<nrows; i++)
00442         for (int j=i; j<ncols; j++)
00443           {
00444             double a = sba.A(i,j);
00445             if (a != 0.0) nnz++;
00446           }
00447 
00448       ofs << nrows << " " << ncols << " " << nnz << " 1" << endl;
00449 
00450       for (int i=0; i<nrows; i++)
00451         for (int j=i; j<ncols; j++)
00452           {
00453             double a = sba.A(i,j);
00454             if (a != 0.0)
00455               ofs << i << " " << j << " " << setprecision(16) << a << endl;
00456           }
00457 
00458       ofs.close();
00459     }
00460 
00461     sprintf(name,"%s-B.txt",fname);
00462 
00463     {
00464       ofstream ofs(name);
00465       if (ofs == NULL)
00466         {
00467           cout << "Can't open file " << fname << endl;
00468           return;
00469         }
00470 
00471       // cameras
00472       Eigen::IOFormat pfmt(16);
00473 
00474       int nrows = sba.B.rows();
00475     
00476       ofs << nrows << " " << 1 << endl;
00477 
00478       for (int i=0; i<nrows; i++)
00479         {
00480           double a = sba.B(i);
00481           ofs << setprecision(16) << a << endl;
00482         }
00483       ofs.close();
00484     }
00485 }
00486 
00487 
00488 int sba::readGraphFile(const char *filename, SysSBA& sbaout)
00489 { 
00490     // Create vectors to hold the data from the graph file. 
00491     vector< Vector5d, Eigen::aligned_allocator<Vector5d> > camps;       // cam params <f d1 d2>
00492     vector< Vector4d, Eigen::aligned_allocator<Vector4d> > camqs;       // cam rotation matrix
00493     vector< Vector3d, Eigen::aligned_allocator<Vector3d> > camts;       // cam translation
00494     vector< Vector3d, Eigen::aligned_allocator<Vector3d> > ptps;        // point position
00495     vector< vector< Vector11d, Eigen::aligned_allocator<Vector11d> > > ptts; // point tracks - each vector is <camera_index kp_idex u v>
00496 
00497     int ret = ParseGraphFile(filename, camps, camqs, camts, ptps, ptts);
00498     if (ret < 0)
00499         return -1;
00500         
00501     int ncams = camps.size();
00502     int npts  = ptps.size();
00503     int nprjs = 0;
00504     for (int i=0; i<npts; i++)
00505         nprjs += (int)ptts[i].size();
00506     //    cout << "Points: " << npts << "  Tracks: " << ptts.size() 
00507     //         << "  Projections: " << nprjs << endl; 
00508          
00509     // cout << "Setting up nodes..." << flush;
00510     for (int i=0; i<ncams; i++)
00511     {
00512         // camera params
00513         Vector5d &camp = camps[i];
00514         CamParams cpars = {camp[0],camp[1],camp[2],camp[3],camp[4]}; // set focal length and offsets
00515                                                                 // note fy is negative...
00516         //
00517         // NOTE: not sure how graph files parameterize rotations
00518         //
00519 
00520         Quaternion<double> frq(camqs[i]); // quaternion coeffs
00521         if (frq.w() < 0.0)      // w negative, change to positive
00522         {
00523             frq.x() = -frq.x();
00524             frq.y() = -frq.y();
00525             frq.z() = -frq.z();
00526             frq.w() = -frq.w();
00527         }
00528         
00529         frq.normalize();
00530 
00531         // translation
00532         Vector3d &camt = camts[i];
00533         Vector4d frt;
00534         frt.head<3>() = camt;
00535         frt[3] = 1.0;
00536 
00537         Node nd;
00538         
00539         sbaout.addNode(frt, frq, cpars);
00540     }
00541     // cout << "done" << endl;
00542 
00543     // set up points
00544     // cout << "Setting up points..." << flush;
00545     for (int i=0; i<npts; i++)
00546     {
00547         // point
00548         Vector3d &ptp = ptps[i];
00549         Point pt;
00550         pt.head<3>() = ptp;
00551         pt[3] = 1.0;
00552         sbaout.addPoint(pt);
00553     }
00554     // cout << "done" << endl;
00555 
00556 
00557     sbaout.useLocalAngles = true;    // use local angles
00558     sbaout.nFixed = 1;
00559 
00560     // set up projections
00561     int ntot = 0;
00562     // cout << "Setting up projections..." << flush;
00563     for (int i=0; i<npts; i++)
00564     {
00565         // track
00566         vector<Vector11d, Eigen::aligned_allocator<Vector11d> > &ptt = ptts[i];
00567         int nprjs = ptt.size();
00568         for (int j=0; j<nprjs; j++)
00569         {
00570             // projection
00571             Vector11d &prj = ptt[j];
00572             int cami = (int)prj[0];
00573             if (cami >= ncams)
00574               cout << "*** Cam index exceeds bounds: " << cami << endl;
00575             if (prj[4] > 0)     // stereo
00576               {
00577                 Vector3d pt = prj.segment<3>(2);
00578                 sbaout.addStereoProj(cami,i,pt); // Monocular projections
00579               }
00580             else                // mono
00581               {
00582                 Vector2d pt = prj.segment<2>(2);
00583                 sbaout.addMonoProj(cami,i,pt); // Monocular projections
00584               }
00585 
00586             ntot++;
00587         }
00588     }
00589     // cout << "done" << endl;
00590     
00591     return 0;
00592 }
00593 
00594 
00595 // makes a quaternion from fixed Euler RPY angles
00596 // see the Wikipedia article on Euler anlges
00597 static void make_qrot(double rr, double rp, double ry, Vector4d &v)
00598 {
00599   double sr = sin(rr/2.0);
00600   double cr = cos(rr/2.0);
00601   double sp = sin(rp/2.0);
00602   double cp = cos(rp/2.0);
00603   double sy = sin(ry/2.0);
00604   double cy = cos(ry/2.0);
00605   v[0] = sr*cp*cy - cr*sp*sy;   // qx
00606   v[1] = cr*sp*cy + sr*cp*sy;   // qy
00607   v[2] = cr*cp*sy - sr*sp*cy;   // qz
00608   v[3] = cr*cp*cy + sr*sp*sy;   // qw
00609 }
00610 
00611 int  sba::ParseGraphFile(const char *fin,       // input file
00612   vector< Vector5d, Eigen::aligned_allocator<Vector5d> > &camp, // cam params <fx fy cx cy>
00613   vector< Vector4d, Eigen::aligned_allocator<Vector4d> > &camq, // cam rotation quaternion
00614   vector< Vector3d, Eigen::aligned_allocator<Vector3d> > &camt, // cam translation
00615   vector< Vector3d, Eigen::aligned_allocator<Vector3d> > &ptp, // point position
00616   // point tracks - each vector is <camera_index point_index u v d>; 
00617   // point index is redundant, d is 0 for mono, >0 for stereo
00618   vector< vector< Vector11d, Eigen::aligned_allocator<Vector11d> > > &ptts 
00619                 )
00620 {
00621   // input stream
00622   ifstream ifs(fin);
00623   if (ifs == NULL)
00624     {
00625       cout << "Can't open file " << fin << endl;
00626       return -1;
00627     }
00628   ifs.precision(10);
00629 
00630   // map of node and point indices
00631   map<int,int> nodemap;
00632   map<int,int> pointmap;
00633 
00634   // loop over lines
00635   string line;
00636   int nline = 0;
00637   int nid = 0;                  // current node id
00638   int pid = 0;                  // current point id
00639   while (getline(ifs,line))
00640     {
00641       nline++;
00642       stringstream ss(line);    // make a string stream
00643       string type;
00644       ss >> type;
00645       size_t pos = type.find("#");
00646       if (pos != string::npos)
00647         continue;               // comment line
00648 
00649       if (type == "VERTEX_SE3" || 
00650           type == "VERTEX_CAM")    // have a camera node
00651         {
00652           int n;
00653           double tx,ty,tz,qx,qy,qz,qw,fx,fy,cx,cy,bline;
00654           if (!(ss >> n >> tx >> ty >> tz >> qx >> qy >> qz >> qw >> fx >> fy >>
00655                 cx >> cy >> bline))
00656             {
00657               cout << "[ReadSPA] Bad VERTEX_SE3 at line " << nline << endl;
00658               return -1;
00659             }
00660           nodemap.insert(pair<int,int>(n,nid));
00661           nid++;
00662           camt.push_back(Vector3d(tx,ty,tz));
00663           Vector4d v(qx,qy,qz,qw); // use quaternions
00664           //          make_qrot(rr,rp,ry,v);
00665           camq.push_back(v);
00666 
00667           // fx,fy,cx,cy
00668           Vector5d cp;
00669           cp << fx,fy,cx,cy,bline;
00670           camp.push_back(cp);
00671         }
00672 
00673       else if (type == "VERTEX_XYZ")    // have a point
00674         {
00675           int n;
00676           double tx,ty,tz;
00677           if (!(ss >> n >> tx >> ty >> tz))
00678             {
00679               cout << "[ReadSPA] Bad VERTEX_XYZ at line " << nline << endl;
00680               return -1;
00681             }
00682           pointmap.insert(pair<int,int>(n,pid));
00683           pid++;
00684           ptp.push_back(Vector3d(tx,ty,tz));
00685         }
00686 
00687       else if (type == "EDGE_PROJECT_XYZ" ||
00688                type == "EDGE_PROJECT_P2MC" ||
00689                type == "EDGE_PROJECT_P2SC") // have an edge
00690         {
00691           int n1,n2;
00692           double u,v,d;         // projection, including disparity
00693           double cv0, cv1, cv2, cv3, cv4, cv5; // covars of point projection, not used
00694           cv3 = cv4 =cv5 = 0.0;
00695 
00696           // indices and measurement
00697           d = 0;
00698           if (type == "EDGE_PROJECT_P2SC")
00699             {
00700               if (!(ss >> n1 >> n2 >> u >> v >> d >>
00701                     cv0 >> cv1 >> cv2 >> cv3 >> cv4 >> cv5))
00702                 {
00703                   cout << "[ReadSPA] Bad EDGE_PROJECT_XYZ at line " << nline << endl;
00704                   return -1;
00705                 }
00706             }
00707           else
00708             {
00709               if (!(ss >> n1 >> n2 >> u >> v >> cv0 >> cv1 >> cv2))
00710                 {
00711                   cout << "[ReadSPA] Bad EDGE_PROJECT_XYZ at line " << nline << endl;
00712                   return -1;
00713                 }
00714             }
00715 
00716           // get true indices
00717           map<int,int>::iterator it;
00718           it = pointmap.find(n1);
00719           if (it == pointmap.end())
00720             {
00721               cout << "[ReadSPA] Missing point index " << n1 << " at line " << nline << endl;
00722               return -1;
00723             }
00724           int pi = it->second;
00725           if (pi >= (int)ptp.size())
00726             {
00727               cout << "[ReadSPA] Point index " << pi << " too large at line " << nline << endl;
00728               return -1;
00729             }
00730 
00731 
00732           it = nodemap.find(n2);
00733           if (it == nodemap.end())
00734             {
00735               cout << "[ReadSPA] Missing camera index " << n2 << " at line " << nline << endl;
00736               return -1;
00737             }
00738           int ci = it->second;
00739           if (ci >= (int)camp.size())
00740             {
00741               cout << "[ReadSPA] Camera index " << ci << " too large at line " << nline << endl;
00742               return -1;
00743             }
00744 
00745           
00746           // get point track
00747           if (ptts.size() < (size_t)pi+1)
00748             ptts.resize(pi+1);
00749           vector< Vector11d, Eigen::aligned_allocator<Vector11d> > &trk = ptts[pi];
00750           Vector11d tv;
00751           tv << ci,pi,u,v,d,cv0,cv1,cv2,cv3,cv4,cv5;
00752           trk.push_back(tv);
00753         }
00754 
00755       else
00756         {
00757           cout << "[ReadSPA] Undefined type <" << type <<"> at line " << nline << endl;
00758           return -1;
00759         }
00760     }
00761 
00762     // print some stats
00763     double nprjs = 0;
00764     int ncams = camp.size();
00765     int npts = ptp.size();
00766     for (int i=0; i<npts; i++)
00767       nprjs += ptts[i].size();
00768     cout << "Number of cameras: " << ncams << endl;
00769     cout << "Number of points: " << npts << endl;
00770     cout << "Number of projections: " << (int)nprjs << endl;
00771     cout << "Average projections per camera: " << nprjs/(double)ncams << endl;
00772     cout << "Average track length: " << nprjs/(double)npts << endl;
00773     return 0;
00774 }
00775 
00776 
00783 int sba::writeGraphFile(const char *filename, SysSBA& sba, bool mono)
00784 {
00785     ofstream outfile(filename, ios_base::trunc);
00786     if (outfile == NULL)
00787     {
00788         cout << "Can't open file " << filename << endl;
00789         return -1;
00790     }
00791     
00792     outfile.precision(5);
00793     //    outfile.setf(ios_base::scientific);
00794     outfile.setf(ios_base::fixed);
00795     
00796     unsigned int i = 0;
00797     
00798     // header, but we skip for now
00799     //    outfile << "# Bundle file v0.3" << endl;
00800     
00801     // Info about each camera
00802     //   VERTEX_CAM n x y z qx qy qz qw fx fy cx cy baseline
00803     //   <baseline> is 0 for monocular data
00804     //   <n> is the camera index, heading at 0
00805     int ncams = sba.nodes.size();
00806     for (i = 0; i < (unsigned int)ncams; i++)
00807     {
00808       outfile << "VERTEX_CAM" << " ";
00809       outfile << i << " ";      // node number
00810       Vector3d trans = sba.nodes[i].trans.head<3>(); // position
00811       outfile << trans(0) << ' ' << trans(1) << ' ' << trans(2) << ' ';
00812       Vector4d rot = sba.nodes[i].qrot.coeffs(); // rotation
00813       outfile << rot(0) << ' ' << rot(1) << ' ' << rot(2) << ' ' << rot(3) << ' ';
00814       // cam params
00815       outfile << sba.nodes[i].Kcam(0,0) << ' ' << sba.nodes[i].Kcam(1,1) << ' ' << 
00816         sba.nodes[i].Kcam(0,2) << ' ' << sba.nodes[i].Kcam(1,2) << ' ' << sba.nodes[i].baseline << endl;
00817     }
00818     
00819     // Info about each point
00820     //  point indices are sba indices plus ncams (so they're unique in the file)
00821     //    VERTEX_POINT n x y z
00822     //  after each point comes the projections
00823     //    EDGE_PROJECT_P2C pt_ind cam_ind u v
00824     for (i = 0; i < sba.tracks.size(); i++)
00825     {
00826       outfile << "VERTEX_XYZ" << ' ' << ncams+i << ' '; // index
00827       // World <x y z>
00828       outfile << sba.tracks[i].point(0) << ' ' << sba.tracks[i].point(1) 
00829               << ' ' << sba.tracks[i].point(2) << endl;
00830         
00831       ProjMap &prjs = sba.tracks[i].projections;
00832         
00833       // Output all projections
00834       //   Mono projections have 0 for the disparity
00835       for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00836         {
00837           // TODO: output real covariance, if available
00838           Proj &prj = itr->second;
00839           if (prj.stereo && !mono)
00840             {
00841               outfile << "EDGE_PROJECT_P2SC "; // stereo edge
00842               outfile << ncams+i << ' ' << prj.ndi << ' ' << prj.kp(0) << ' ' 
00843                       << prj.kp(1) << ' ' << prj.kp(2) << ' ';
00844               outfile << "1 0 0 0 1 1" << endl; // covariance
00845             }
00846           else
00847             {
00848               outfile << "EDGE_PROJECT_P2MC "; // mono edge
00849               outfile << ncams+i << ' ' << prj.ndi << ' ' << prj.kp(0) << ' ' 
00850                       << prj.kp(1) << ' ';
00851               outfile << "1 0 1" << endl; // covariance
00852             }
00853         }
00854     }
00855 
00856     return 0;
00857 } 
00858 
00859 
00860 //
00861 // SPA (3D pose graphs)
00862 //
00863 
00864 
00865 //
00866 // add a single node to the graph, in the position given by the VERTEX2 entry in the file
00867 //
00868 
00869 void 
00870 addnode(SysSPA &spa, int n, 
00871  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ntrans,      // node translation
00872  std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > nqrot,       // node rotation
00873  std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > cind,        // constraint indices
00874  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ctrans,      // constraint local translation 
00875  std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > cqrot,       // constraint local rotation as quaternion
00876  std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > prec) // constraint covariance
00877 {
00878   Node nd;
00879 
00880   // rotation
00881   Quaternion<double> frq;
00882   frq.coeffs() = nqrot[n];
00883 #if 0
00884   frq.normalize();
00885   if (frq.w() <= 0.0) frq.coeffs() = -frq.coeffs();
00886   nd.qrot = frq.coeffs();
00887 #endif
00888 
00889   // translation
00890   Vector4d v;
00891   v.head(3) = ntrans[n];
00892   v(3) = 1.0;
00893 
00894   spa.addNode(v,frq);
00895 
00896 #if 0
00897   nd.trans = v;
00898   nd.setTransform();        // set up world2node transform
00899   nd.setDr(true);
00900 
00901   // add to system
00902   spa.nodes.push_back(nd);
00903 #endif
00904 
00905   // add in constraints
00906   for (int i=0; i<(int)ctrans.size(); i++)
00907     {
00908       ConP2 con;
00909       con.ndr = cind[i].x();
00910       con.nd1 = cind[i].y();
00911 
00912       if ((con.ndr == n && con.nd1 <= n-1) ||
00913           (con.nd1 == n && con.ndr <= n-1))
00914         {
00915           con.tmean = ctrans[i];
00916           Quaternion<double> qr;
00917           qr.coeffs() = cqrot[i];
00918           qr.normalize();
00919           con.qpmean = qr.inverse(); // inverse of the rotation measurement
00920           con.prec = prec[i];       // ??? should this be inverted ???
00921 
00922           // need a boost for noise-offset system
00923           //con.prec.block<3,3>(3,3) *= 10.0;
00924           spa.p2cons.push_back(con);
00925         }
00926     }
00927 }
00928 
00929 int sba::readSPAGraphFile(const char *filename, SysSPA& spaout)
00930 { 
00931   // Create vectors to hold the data from the graph file. 
00932   std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ntrans;     // node translation
00933   std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > nqrot;      // node rotation
00934   std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > cind;       // constraint indices
00935   std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ctrans;     // constraint local translation 
00936   std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > cqrot;      // constraint local rotation as quaternion
00937   std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > prec; // constraint covariance
00938 
00939   int ret = ParseSPAGraphFile(filename, ntrans, nqrot, cind, ctrans, cqrot, prec);
00940   if (ret < 0)
00941     return -1;
00942         
00943   cout << "# [ReadSPAFile] Found " << (int)ntrans.size() << " nodes and " 
00944        << (int)cind.size() << " constraints" << endl;
00945 
00946   int nnodes = ntrans.size();
00947 
00948   // add in nodes
00949   for (int i=0; i<nnodes; i++)
00950     addnode(spaout, i, ntrans, nqrot, cind, ctrans, cqrot, prec);
00951     
00952   return 0;
00953 }
00954 
00955 
00956 // cv is upper triangular
00957 void make_covar(double *cv, Matrix<double,6,6> &m)
00958 {
00959   m.setZero();
00960 
00961   int i = 0;
00962   m(0,0) = cv[i++];
00963   m(0,1) = cv[i++];
00964   m(0,2) = cv[i++];
00965   m(0,3) = cv[i++];
00966   m(0,4) = cv[i++];
00967   m(0,5) = cv[i++];
00968 
00969   m(1,1) = cv[i++];
00970   m(1,2) = cv[i++];
00971   m(1,3) = cv[i++];
00972   m(1,4) = cv[i++];
00973   m(1,5) = cv[i++];
00974 
00975   m(2,2) = cv[i++];
00976   m(2,3) = cv[i++];
00977   m(2,4) = cv[i++];
00978   m(2,5) = cv[i++];
00979 
00980   m(3,3) = cv[i++];
00981   m(3,4) = cv[i++];
00982   m(3,5) = cv[i++];
00983 
00984   m(4,4) = cv[i++];
00985   m(4,5) = cv[i++];
00986 
00987   m(5,5) = cv[i++];
00988 
00989   // make symmetric
00990   Matrix<double,6,6> mt = m.transpose();
00991   mt.diagonal().setZero();
00992   m = m+mt;
00993 }
00994 
00995 int
00996 sba::ParseSPAGraphFile(const char *fin, // input file
00997    std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &ntrans, // node translation
00998    std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > &nqrot,  // node rotation
00999    std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > &cind,   // constraint indices
01000    std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &ctrans, // constraint local translation 
01001    std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > &cqrot,  // constraint local rotation as quaternion
01002    std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > &prec) // constraint covariance
01003 {
01004   ifstream ifs(fin);
01005   if (ifs == NULL)
01006     {
01007       cout << "Can't open file " << fin << endl;
01008       return -1;
01009     }
01010   ifs.precision(10);
01011 
01012   // loop over lines
01013   string line;
01014   int nline = 0;
01015   bool first = true;
01016   while (getline(ifs,line))
01017     {
01018       nline++;
01019       stringstream ss(line);    // make a string stream
01020       string type;
01021       ss >> type;
01022       size_t pos = type.find("#");
01023       if (pos != string::npos)
01024         continue;               // comment line
01025 
01026       if (type == "VERTEX_SE3")    // have a vertex
01027         {
01028           int n;
01029           double tx,ty,tz,rr,rp,ry;
01030           if (!(ss >> n >> tx >> ty >> tz >> rr >> rp >> ry))
01031             {
01032               cout << "[ReadSPA] Bad VERTEX_SE3 at line " << nline << endl;
01033               return -1;
01034             }
01035           ntrans.push_back(Vector3d(tx,ty,tz));
01036           Vector4d v;
01037           make_qrot(rr,rp,ry,v);
01038           nqrot.push_back(v);
01039         }
01040 
01041       if (type == "EDGE_SE3_SE3")      // have an edge
01042         {
01043           int n1,n2;
01044           double tx,ty,tz,rr,rp,ry;
01045           double cv[21];
01046 
01047           // indices and measurement
01048           if (!(ss >> n1 >> n2 >> tx >> ty >> tz >> rr >> rp >> ry))
01049             {
01050               cout << "[ReadSPA] Bad EDGE_SE3_SE3 at line " << nline << endl;
01051               return -1;
01052             }
01053           cind.push_back(Vector2i(n1,n2));
01054           ctrans.push_back(Vector3d(tx,ty,tz));
01055           Vector4d v;
01056           make_qrot(rr,rp,ry,v);
01057           cqrot.push_back(v);
01058 
01059           // covar
01060           if (!(ss >> cv[0] >> cv[1] >> cv[2] >> cv[3] >> cv[4] 
01061                 >> cv[5] >> cv[6] >> cv[7] >> cv[8] >> cv[9] 
01062                 >> cv[10] >> cv[11] >> cv[12] >> cv[13] >> cv[14] 
01063                 >> cv[15] >> cv[16] >> cv[17] >> cv[18] >> cv[19] >> cv[20]))
01064             {
01065               cout << "[ReadSPA] Bad EDGE_SE3_SE3 at line " << nline << endl;
01066               return -1;
01067             }
01068           Matrix<double,6,6> m;
01069           make_covar(cv,m);
01070           if (first)
01071             {
01072               //cout << endl;
01073               //for (int j=0; j<21; j++);
01074                 //cout << cv[j] << " ";
01075               //cout << endl << endl << << m << endl;
01076               first = false;
01077             }
01078           prec.push_back(m);
01079         }
01080 
01081     }
01082   return 0;
01083 }


sba
Author(s): Kurt Konolige, Helen Oleynikova
autogenerated on Thu Jan 2 2014 12:12:09