sba_file_io.cpp
Go to the documentation of this file.
2 #include <map>
3 
4 using namespace sba;
5 using namespace Eigen;
6 using namespace frame_common;
7 using namespace std;
8 
9 int sba::readBundlerFile(const char *filename, SysSBA& sbaout)
10 {
11  // Create vectors to hold the data from the bundler file.
12  vector< Vector3d, Eigen::aligned_allocator<Vector3d> > camps; // cam params <f d1 d2>
13  vector< Matrix3d, Eigen::aligned_allocator<Matrix3d> > camRs; // cam rotation matrix
14  vector< Vector3d, Eigen::aligned_allocator<Vector3d> > camts; // cam translation
15  vector< Vector3d, Eigen::aligned_allocator<Vector3d> > ptps; // point position
16  vector< Vector3i, Eigen::aligned_allocator<Vector3i> > ptcs; // point color
17  vector< vector< Vector4d, Eigen::aligned_allocator<Vector4d> > > ptts; // point tracks - each vector is <camera_index kp_idex u v>
18 
19  int ret = ParseBundlerFile(filename, camps, camRs, camts, ptps, ptcs, ptts);
20  if (ret < 0)
21  return -1;
22 
23  int ncams = camps.size();
24  int npts = ptps.size();
25  int nprjs = 0;
26  for (int i=0; i<npts; i++)
27  nprjs += (int)ptts[i].size();
28  /* cout << "Points: " << npts << " Tracks: " << ptts.size()
29  << " Projections: " << nprjs << endl; */
30 
31  cout << "Setting up nodes..." << flush;
32  for (int i=0; i<ncams; i++)
33  {
34  // camera params
35  Vector3d &camp = camps[i];
36  CamParams cpars = {camp[0],camp[0],0,0,0}; // set focal length, no offset
37 
38  //
39  // NOTE: Bundler camera coords are rotated 180 deg around the X axis of
40  // the camera, so Z points opposite the camera viewing ray (OpenGL).
41  // Note quite sure, but I think this gives the camera pose as
42  // [-R' -R'*t]
43 
44  // rotation matrix
45  Matrix3d m180x; // rotate 180 deg around X axis, to convert Bundler frames to SBA frames
46  m180x << 1, 0, 0, 0, -1, 0, 0, 0, -1;
47  Matrix3d camR = m180x * camRs[i]; // rotation matrix
48  Quaternion<double> frq(camR.transpose()); // camera frame rotation, from Bundler docs
49  if (frq.w() < 0.0) // w negative, change to positive
50  {
51  frq.x() = -frq.x();
52  frq.y() = -frq.y();
53  frq.z() = -frq.z();
54  frq.w() = -frq.w();
55  }
56 
57  frq.normalize();
58 
59  // translation
60  Vector3d &camt = camts[i];
61  Vector4d frt;
62  frt.head<3>() = -camRs[i].transpose() * camt; // camera frame translation, from Bundler docs
63  frt[3] = 1.0;
64 
65  Node nd;
66 
67  sbaout.addNode(frt, frq, cpars);
68  }
69  // cout << "done" << endl;
70 
71  // set up points
72  cout << "Setting up points..." << flush;
73  for (int i=0; i<npts; i++)
74  {
75  // point
76  Vector3d &ptp = ptps[i];
77  Point pt;
78  pt.head<3>() = ptp;
79  pt[3] = 1.0;
80  sbaout.addPoint(pt);
81  }
82  // cout << "done" << endl;
83 
84 
85  sbaout.useLocalAngles = true; // use local angles
86  sbaout.nFixed = 1;
87 
88  // set up projections
89  int ntot = 0;
90  cout << "Setting up projections..." << flush;
91  for (int i=0; i<npts; i++)
92  {
93  // track
94  vector<Vector4d, Eigen::aligned_allocator<Vector4d> > &ptt = ptts[i];
95  int nprjs = ptt.size();
96  for (int j=0; j<nprjs; j++)
97  {
98  // projection
99  Vector4d &prj = ptt[j];
100  int cami = (int)prj[0];
101  Vector2d pt = prj.segment<2>(2);
102  pt[1] = -pt[1]; // NOTE: Bundler image Y is reversed
103  if (cami >= ncams)
104  cout << "*** Cam index exceeds bounds: " << cami << endl;
105  sbaout.addMonoProj(cami,i,pt); // Monocular projections
106  ntot++;
107  }
108  }
109  cout << "done" << endl;
110 
111  return 0;
112 }
113 
114 int sba::writeBundlerFile(const char *filename, SysSBA& sbain)
115 {
116  ofstream outfile(filename, ios_base::trunc);
117  if (!outfile)
118  {
119  cout << "Can't open file " << filename << endl;
120  return -1;
121  }
122 
123  outfile.precision(10);
124  outfile.setf(ios_base::scientific);
125 
126  unsigned int i = 0;
127 
128  outfile << "# Bundle file v0.3" << endl;
129  // First line is number of cameras and number of points.
130  outfile << sbain.nodes.size() << ' ' << sbain.tracks.size() << endl;
131 
132  // Set up transform matrix for camera parameters
133  Matrix3d m180x; // rotate 180 deg around X axis, to convert Bundler frames to SBA frames
134  m180x << 1, 0, 0, 0, -1, 0, 0, 0, -1;
135 
136 
137  // Then goes information about each camera, in <f> <k1> <k2>\n<R>\n<t> format.
138  for (i = 0; i < sbain.nodes.size(); i++)
139  {
140  // Assuming fx = fy and using fx. Don't use k1 or k2.
141  outfile << sbain.nodes[i].Kcam(0, 0) << ' ' << 0.0 << ' ' << 0.0 << endl;
142 
143  Quaternion<double> quat(sbain.nodes[i].qrot);
144  /* cout << "\nQuat: [ " << sbain.nodes[i].qrot << " ]\n"; */
145  quat.normalize();
146  Matrix3d rotmat = m180x * quat.toRotationMatrix().transpose();
147 
148  outfile << rotmat(0, 0) << ' ' << rotmat(0, 1) << ' ' << rotmat(0, 2) << endl;
149  outfile << rotmat(1, 0) << ' ' << rotmat(1, 1) << ' ' << rotmat(1, 2) << endl;
150  outfile << rotmat(2, 0) << ' ' << rotmat(2, 1) << ' ' << rotmat(2, 2) << endl;
151 
152  Vector3d trans = sbain.nodes[i].trans.head<3>();
153  trans = -rotmat*trans;
154  outfile << trans(0) << ' ' << trans(1) << ' ' << trans(2) << endl;
155  }
156 
157  outfile.setf(ios_base::fixed);
158 
159  // Then goes information about each point. <pos>\n<color>\n<viewlist>
160  for (i = 0; i < sbain.tracks.size(); i++)
161  {
162  // World <x y z>
163  outfile << sbain.tracks[i].point(0) << ' ' << sbain.tracks[i].point(1)
164  << ' ' << sbain.tracks[i].point(2) << endl;
165  // Color <r g b> (Just say white instead)
166  outfile << "255 255 255" << endl;
167  // View list: <list_length><camera_index key u v>\n<camera_index key u v>
168  // Key is the keypoint # in SIFT, but we just use point number instead.
169  // We output these as monocular points because the file format does not
170  // support stereo points.
171 
172  ProjMap &prjs = sbain.tracks[i].projections;
173 
174  // List length
175  outfile << prjs.size() << ' ';
176 
177  // Output all the tracks as monocular tracks.
178  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
179  {
180  Proj &prj = itr->second;
181  // y is reversed (-y)
182  Node &node = sbain.nodes[prj.ndi];
183 
184  double cx = node.Kcam(0, 2);
185  double cy = node.Kcam(1, 2);
186 
187  outfile << prj.ndi << ' ' << i << ' ' << prj.kp(0)-cx << ' '
188  << -(prj.kp(1)-cy) << ' ';
189  }
190 
191  outfile << endl;
192  }
193 
194  return 0;
195 }
196 
197 int sba::ParseBundlerFile(const char *fin, // input file
198  vector< Vector3d, Eigen::aligned_allocator<Vector3d> > &camp, // cam params <f d1 d2>
199  vector< Matrix3d, Eigen::aligned_allocator<Matrix3d> > &camR, // cam rotation matrix
200  vector< Vector3d, Eigen::aligned_allocator<Vector3d> > &camt, // cam translation
201  vector< Vector3d, Eigen::aligned_allocator<Vector3d> > &ptp, // point position
202  vector< Vector3i, Eigen::aligned_allocator<Vector3i> > &ptc, // point color
203  vector< vector< Vector4d, Eigen::aligned_allocator<Vector4d> > > &ptt // point tracks - each vector is <camera_index u v>
204  )
205 {
206  ifstream ifs(fin);
207  if (!ifs)
208  {
209  cout << "Can't open file " << fin << endl;
210  return -1;
211  }
212  ifs.precision(10);
213 
214 
215  // read header
216  string line;
217  if (!getline(ifs,line) || line != "# Bundle file v0.3")
218  {
219  cout << "Bad header" << endl;
220  return -1;
221  }
222  cout << "Found Bundler 3.0 file" << endl;
223 
224  // read number of cameras and points
225  int ncams, npts;
226  if (!(ifs >> ncams >> npts))
227  {
228  cout << "Bad header" << endl;
229  return -1;
230  }
231  cout << "Number of cameras: " << ncams << " Number of points: " << npts << endl;
232 
233  cout << "Reading in camera data..." << flush;
234  for (int i=0; i<ncams; i++)
235  {
236  double v1,v2,v3,v4,v5,v6,v7,v8,v9;
237  if (!(ifs >> v1 >> v2 >> v3))
238  {
239  cout << "Bad camera params at number " << i << endl;
240  return -1;
241  }
242  camp.push_back(Vector3d(v1,v2,v3));
243 
244  if (!(ifs >> v1 >> v2 >> v3 >> v4 >> v5 >> v6 >> v7 >> v8 >> v9))
245  {
246  cout << "Bad camera rotation matrix at number " << i << endl;
247  return -1;
248  }
249  Matrix3d m;
250  m << v1,v2,v3,v4,v5,v6,v7,v8,v9;
251  camR.push_back(m);
252 
253  if (!(ifs >> v1 >> v2 >> v3))
254  {
255  cout << "Bad camera translation at number " << i << endl;
256  return -1;
257  }
258  camt.push_back(Vector3d(v1,v2,v3));
259  }
260  cout << "done" << endl;
261 
262  ptt.resize(npts);
263 
264  cout << "Reading in pts data..." << flush;
265  for (int i=0; i<npts; i++)
266  {
267  double v1,v2,v3;
268  int i1,i2,i3;
269 
270  if (!(ifs >> v1 >> v2 >> v3))
271  {
272  cout << "Bad point position at number " << i << endl;
273  return -1;
274  }
275  ptp.push_back(Vector3d(v1,v2,v3));
276 
277  if (!(ifs >> i1 >> i2 >> i3))
278  {
279  cout << "Bad point color at number " << i << endl;
280  return -1;
281  }
282  ptc.push_back(Vector3i(i1,i2,i3));
283 
284 
285  if (!(ifs >> i1))
286  {
287  cout << "Bad track count at number " << i << endl;
288  return -1;
289  }
290  int nprjs = i1;
291 
292  vector<Vector4d, Eigen::aligned_allocator<Vector4d> > &prjs = ptt[i];
293  for (int j=0; j<nprjs; j++)
294  {
295  if (!(ifs >> i1 >> i2 >> v1 >> v2))
296  {
297  cout << "Bad track parameter at number " << i << endl;
298  return -1;
299  }
300  prjs.push_back(Vector4d(i1,i2,v1,v2));
301  }
302 
303  } // end of pts loop
304  cout << "done" << endl;
305 
306  // print some stats
307  double nprjs = 0;
308  for (int i=0; i<npts; i++)
309  nprjs += ptt[i].size();
310  cout << "Number of projections: " << (int)nprjs << endl;
311  cout << "Average projections per camera: " << nprjs/(double)ncams << endl;
312  cout << "Average track length: " << nprjs/(double)npts << endl;
313  return 0;
314 }
315 
316 
317 // write out the system in an sba (Lourakis) format
318 // NOTE: Lourakis FAQ is wrong about coordinate systems
319 // Cameras are represented by the w2n transform, converted to
320 // a quaternion and translation vector
321 //
322 void sba::writeLourakisFile(const char *fname, SysSBA& sba)
323 {
324  char name[1024];
325  sprintf(name,"%s-cams.txt",fname);
326  FILE *fn = fopen(name,"w");
327  if (fn == nullptr)
328  {
329  cout << "[WriteFile] Can't open file " << name << endl;
330  return;
331  }
332 
333  // write out initial camera poses
334  int ncams = sba.nodes.size();
335  for (int i=0; i<ncams; i++)
336  {
337  Node &nd = sba.nodes[i];
338 
339  // Why not just use the Quaternion???
340  Quaternion<double> frq(nd.w2n.block<3,3>(0,0)); // rotation matrix of transform
341  fprintf(fn,"%f %f %f %f ", frq.w(), frq.x(), frq.y(), frq.z());
342  Vector3d tr = nd.w2n.col(3);
343  fprintf(fn,"%f %f %f\n", tr[0], tr[1], tr[2]);
344  }
345  fclose(fn);
346 
347  sprintf(name,"%s-pts.txt",fname);
348  fn = fopen(name,"w");
349  if (fn == nullptr)
350  {
351  cout << "[WriteFile] Can't open file " << name << endl;
352  return;
353  }
354 
355  fprintf(fn,"# X Y Z nframes frame0 x0 y0 frame1 x1 y1 ...\n");
356 
357  // write out points
358 
359  for(size_t i=0; i<sba.tracks.size(); i++)
360  {
361  ProjMap &prjs = sba.tracks[i].projections;
362  // Write out point
363  Point &pt = sba.tracks[i].point;
364 
365  fprintf(fn,"%f %f %f ", pt.x(), pt.y(), pt.z());
366  fprintf(fn,"%d ",(int)prjs.size());
367 
368  // Write out projections
369  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
370  {
371  Proj &prj = itr->second;
372  if (!prj.isValid) continue;
373  int cami = itr->first;//prj.ndi;
374  // NOTE: Lourakis projected y is reversed (???)
375  fprintf(fn," %d %f %f ",cami,prj.kp[0],prj.kp[1]);
376  }
377  fprintf(fn,"\n");
378  }
379 
380  fclose(fn);
381 
382  // write camera calibartion
383  sprintf(name,"%s-calib.txt",fname);
384  fn = fopen(name,"w");
385  if (fn == nullptr)
386  {
387  cout << "[WriteFile] Can't open file " << name << endl;
388  return;
389  }
390 
391  Matrix3d &K = sba.nodes[0].Kcam;
392  fprintf(fn,"%f %f %f\n", K(0,0), K(0,1), K(0,2));
393  fprintf(fn,"%f %f %f\n", K(1,0), K(1,1), K(1,2));
394  fprintf(fn,"%f %f %f\n", K(2,0), K(2,1), K(2,2));
395 
396  fclose(fn);
397 }
398 
399 
400 // write out the precision matrix
401 void sba::writeA(const char *fname, SysSBA& sba)
402 {
403  ofstream ofs(fname);
404  if (!ofs)
405  {
406  cout << "Can't open file " << fname << endl;
407  return;
408  }
409 
410  // cameras
411  Eigen::IOFormat pfmt(16);
412  ofs << sba.A.format(pfmt) << endl;
413  ofs.close();
414 }
415 
416 
417 // write out the precision matrix for CSparse
418 void sba::writeSparseA(const char *fname, SysSBA& sba)
419 {
420  char name[1024];
421  sprintf(name,"%s-A.tri",fname);
422 
423  {
424  ofstream ofs(name);
425  if (!ofs)
426  {
427  cout << "Can't open file " << fname << endl;
428  return;
429  }
430 
431  // cameras
432  Eigen::IOFormat pfmt(16);
433 
434  int nrows = sba.A.rows();
435  int ncols = sba.A.cols();
436 
437  cout << "[WriteSparseA] Size: " << nrows << "x" << ncols << endl;
438 
439  // find # nonzeros
440  int nnz = 0;
441  for (int i=0; i<nrows; i++)
442  for (int j=i; j<ncols; j++)
443  {
444  double a = sba.A(i,j);
445  if (a != 0.0) nnz++;
446  }
447 
448  ofs << nrows << " " << ncols << " " << nnz << " 1" << endl;
449 
450  for (int i=0; i<nrows; i++)
451  for (int j=i; j<ncols; j++)
452  {
453  double a = sba.A(i,j);
454  if (a != 0.0)
455  ofs << i << " " << j << " " << setprecision(16) << a << endl;
456  }
457 
458  ofs.close();
459  }
460 
461  sprintf(name,"%s-B.txt",fname);
462 
463  {
464  ofstream ofs(name);
465  if (!ofs)
466  {
467  cout << "Can't open file " << fname << endl;
468  return;
469  }
470 
471  // cameras
472  Eigen::IOFormat pfmt(16);
473 
474  int nrows = sba.B.rows();
475 
476  ofs << nrows << " " << 1 << endl;
477 
478  for (int i=0; i<nrows; i++)
479  {
480  double a = sba.B(i);
481  ofs << setprecision(16) << a << endl;
482  }
483  ofs.close();
484  }
485 }
486 
487 
488 int sba::readGraphFile(const char *filename, SysSBA& sbaout)
489 {
490  // Create vectors to hold the data from the graph file.
491  vector< Vector5d, Eigen::aligned_allocator<Vector5d> > camps; // cam params <f d1 d2>
492  vector< Vector4d, Eigen::aligned_allocator<Vector4d> > camqs; // cam rotation matrix
493  vector< Vector3d, Eigen::aligned_allocator<Vector3d> > camts; // cam translation
494  vector< Vector3d, Eigen::aligned_allocator<Vector3d> > ptps; // point position
495  vector< vector< Vector11d, Eigen::aligned_allocator<Vector11d> > > ptts; // point tracks - each vector is <camera_index kp_idex u v>
496 
497  int ret = ParseGraphFile(filename, camps, camqs, camts, ptps, ptts);
498  if (ret < 0)
499  return -1;
500 
501  int ncams = camps.size();
502  int npts = ptps.size();
503  int nprjs = 0;
504  for (int i=0; i<npts; i++)
505  nprjs += (int)ptts[i].size();
506  // cout << "Points: " << npts << " Tracks: " << ptts.size()
507  // << " Projections: " << nprjs << endl;
508 
509  // cout << "Setting up nodes..." << flush;
510  for (int i=0; i<ncams; i++)
511  {
512  // camera params
513  Vector5d &camp = camps[i];
514  CamParams cpars = {camp[0],camp[1],camp[2],camp[3],camp[4]}; // set focal length and offsets
515  // note fy is negative...
516  //
517  // NOTE: not sure how graph files parameterize rotations
518  //
519 
520  Quaternion<double> frq(camqs[i]); // quaternion coeffs
521  if (frq.w() < 0.0) // w negative, change to positive
522  {
523  frq.x() = -frq.x();
524  frq.y() = -frq.y();
525  frq.z() = -frq.z();
526  frq.w() = -frq.w();
527  }
528 
529  frq.normalize();
530 
531  // translation
532  Vector3d &camt = camts[i];
533  Vector4d frt;
534  frt.head<3>() = camt;
535  frt[3] = 1.0;
536 
537  Node nd;
538 
539  sbaout.addNode(frt, frq, cpars);
540  }
541  // cout << "done" << endl;
542 
543  // set up points
544  // cout << "Setting up points..." << flush;
545  for (int i=0; i<npts; i++)
546  {
547  // point
548  Vector3d &ptp = ptps[i];
549  Point pt;
550  pt.head<3>() = ptp;
551  pt[3] = 1.0;
552  sbaout.addPoint(pt);
553  }
554  // cout << "done" << endl;
555 
556 
557  sbaout.useLocalAngles = true; // use local angles
558  sbaout.nFixed = 1;
559 
560  // set up projections
561  int ntot = 0;
562  // cout << "Setting up projections..." << flush;
563  for (int i=0; i<npts; i++)
564  {
565  // track
566  vector<Vector11d, Eigen::aligned_allocator<Vector11d> > &ptt = ptts[i];
567  int nprjs = ptt.size();
568  for (int j=0; j<nprjs; j++)
569  {
570  // projection
571  Vector11d &prj = ptt[j];
572  int cami = (int)prj[0];
573  if (cami >= ncams)
574  cout << "*** Cam index exceeds bounds: " << cami << endl;
575  if (prj[4] > 0) // stereo
576  {
577  Vector3d pt = prj.segment<3>(2);
578  sbaout.addStereoProj(cami,i,pt); // Monocular projections
579  }
580  else // mono
581  {
582  Vector2d pt = prj.segment<2>(2);
583  sbaout.addMonoProj(cami,i,pt); // Monocular projections
584  }
585 
586  ntot++;
587  }
588  }
589  // cout << "done" << endl;
590 
591  return 0;
592 }
593 
594 
595 // makes a quaternion from fixed Euler RPY angles
596 // see the Wikipedia article on Euler anlges
597 static void make_qrot(double rr, double rp, double ry, Vector4d &v)
598 {
599  double sr = sin(rr/2.0);
600  double cr = cos(rr/2.0);
601  double sp = sin(rp/2.0);
602  double cp = cos(rp/2.0);
603  double sy = sin(ry/2.0);
604  double cy = cos(ry/2.0);
605  v[0] = sr*cp*cy - cr*sp*sy; // qx
606  v[1] = cr*sp*cy + sr*cp*sy; // qy
607  v[2] = cr*cp*sy - sr*sp*cy; // qz
608  v[3] = cr*cp*cy + sr*sp*sy; // qw
609 }
610 
611 int sba::ParseGraphFile(const char *fin, // input file
612  vector< Vector5d, Eigen::aligned_allocator<Vector5d> > &camp, // cam params <fx fy cx cy>
613  vector< Vector4d, Eigen::aligned_allocator<Vector4d> > &camq, // cam rotation quaternion
614  vector< Vector3d, Eigen::aligned_allocator<Vector3d> > &camt, // cam translation
615  vector< Vector3d, Eigen::aligned_allocator<Vector3d> > &ptp, // point position
616  // point tracks - each vector is <camera_index point_index u v d>;
617  // point index is redundant, d is 0 for mono, >0 for stereo
618  vector< vector< Vector11d, Eigen::aligned_allocator<Vector11d> > > &ptts
619  )
620 {
621  // input stream
622  ifstream ifs(fin);
623  if (!ifs)
624  {
625  cout << "Can't open file " << fin << endl;
626  return -1;
627  }
628  ifs.precision(10);
629 
630  // map of node and point indices
631  map<int,int> nodemap;
632  map<int,int> pointmap;
633 
634  // loop over lines
635  string line;
636  int nline = 0;
637  int nid = 0; // current node id
638  int pid = 0; // current point id
639  while (getline(ifs,line))
640  {
641  nline++;
642  stringstream ss(line); // make a string stream
643  string type;
644  ss >> type;
645  size_t pos = type.find("#");
646  if (pos != string::npos)
647  continue; // comment line
648 
649  if (type == "VERTEX_SE3" ||
650  type == "VERTEX_CAM") // have a camera node
651  {
652  int n;
653  double tx,ty,tz,qx,qy,qz,qw,fx,fy,cx,cy,bline;
654  if (!(ss >> n >> tx >> ty >> tz >> qx >> qy >> qz >> qw >> fx >> fy >>
655  cx >> cy >> bline))
656  {
657  cout << "[ReadSPA] Bad VERTEX_SE3 at line " << nline << endl;
658  return -1;
659  }
660  nodemap.insert(pair<int,int>(n,nid));
661  nid++;
662  camt.push_back(Vector3d(tx,ty,tz));
663  Vector4d v(qx,qy,qz,qw); // use quaternions
664  // make_qrot(rr,rp,ry,v);
665  camq.push_back(v);
666 
667  // fx,fy,cx,cy
668  Vector5d cp;
669  cp << fx,fy,cx,cy,bline;
670  camp.push_back(cp);
671  }
672 
673  else if (type == "VERTEX_XYZ") // have a point
674  {
675  int n;
676  double tx,ty,tz;
677  if (!(ss >> n >> tx >> ty >> tz))
678  {
679  cout << "[ReadSPA] Bad VERTEX_XYZ at line " << nline << endl;
680  return -1;
681  }
682  pointmap.insert(pair<int,int>(n,pid));
683  pid++;
684  ptp.push_back(Vector3d(tx,ty,tz));
685  }
686 
687  else if (type == "EDGE_PROJECT_XYZ" ||
688  type == "EDGE_PROJECT_P2MC" ||
689  type == "EDGE_PROJECT_P2SC") // have an edge
690  {
691  int n1,n2;
692  double u,v,d; // projection, including disparity
693  double cv0, cv1, cv2, cv3, cv4, cv5; // covars of point projection, not used
694  cv3 = cv4 =cv5 = 0.0;
695 
696  // indices and measurement
697  d = 0;
698  if (type == "EDGE_PROJECT_P2SC")
699  {
700  if (!(ss >> n1 >> n2 >> u >> v >> d >>
701  cv0 >> cv1 >> cv2 >> cv3 >> cv4 >> cv5))
702  {
703  cout << "[ReadSPA] Bad EDGE_PROJECT_XYZ at line " << nline << endl;
704  return -1;
705  }
706  }
707  else
708  {
709  if (!(ss >> n1 >> n2 >> u >> v >> cv0 >> cv1 >> cv2))
710  {
711  cout << "[ReadSPA] Bad EDGE_PROJECT_XYZ at line " << nline << endl;
712  return -1;
713  }
714  }
715 
716  // get true indices
717  map<int,int>::iterator it;
718  it = pointmap.find(n1);
719  if (it == pointmap.end())
720  {
721  cout << "[ReadSPA] Missing point index " << n1 << " at line " << nline << endl;
722  return -1;
723  }
724  int pi = it->second;
725  if (pi >= (int)ptp.size())
726  {
727  cout << "[ReadSPA] Point index " << pi << " too large at line " << nline << endl;
728  return -1;
729  }
730 
731 
732  it = nodemap.find(n2);
733  if (it == nodemap.end())
734  {
735  cout << "[ReadSPA] Missing camera index " << n2 << " at line " << nline << endl;
736  return -1;
737  }
738  int ci = it->second;
739  if (ci >= (int)camp.size())
740  {
741  cout << "[ReadSPA] Camera index " << ci << " too large at line " << nline << endl;
742  return -1;
743  }
744 
745 
746  // get point track
747  if (ptts.size() < (size_t)pi+1)
748  ptts.resize(pi+1);
749  vector< Vector11d, Eigen::aligned_allocator<Vector11d> > &trk = ptts[pi];
750  Vector11d tv;
751  tv << ci,pi,u,v,d,cv0,cv1,cv2,cv3,cv4,cv5;
752  trk.push_back(tv);
753  }
754 
755  else
756  {
757  cout << "[ReadSPA] Undefined type <" << type <<"> at line " << nline << endl;
758  return -1;
759  }
760  }
761 
762  // print some stats
763  double nprjs = 0;
764  int ncams = camp.size();
765  int npts = ptp.size();
766  for (int i=0; i<npts; i++)
767  nprjs += ptts[i].size();
768  cout << "Number of cameras: " << ncams << endl;
769  cout << "Number of points: " << npts << endl;
770  cout << "Number of projections: " << (int)nprjs << endl;
771  cout << "Average projections per camera: " << nprjs/(double)ncams << endl;
772  cout << "Average track length: " << nprjs/(double)npts << endl;
773  return 0;
774 }
775 
776 
783 int sba::writeGraphFile(const char *filename, SysSBA& sba, bool mono)
784 {
785  ofstream outfile(filename, ios_base::trunc);
786  if (!outfile)
787  {
788  cout << "Can't open file " << filename << endl;
789  return -1;
790  }
791 
792  outfile.precision(5);
793  // outfile.setf(ios_base::scientific);
794  outfile.setf(ios_base::fixed);
795 
796  unsigned int i = 0;
797 
798  // header, but we skip for now
799  // outfile << "# Bundle file v0.3" << endl;
800 
801  // Info about each camera
802  // VERTEX_CAM n x y z qx qy qz qw fx fy cx cy baseline
803  // <baseline> is 0 for monocular data
804  // <n> is the camera index, heading at 0
805  int ncams = sba.nodes.size();
806  for (i = 0; i < (unsigned int)ncams; i++)
807  {
808  outfile << "VERTEX_CAM" << " ";
809  outfile << i << " "; // node number
810  Vector3d trans = sba.nodes[i].trans.head<3>(); // position
811  outfile << trans(0) << ' ' << trans(1) << ' ' << trans(2) << ' ';
812  Vector4d rot = sba.nodes[i].qrot.coeffs(); // rotation
813  outfile << rot(0) << ' ' << rot(1) << ' ' << rot(2) << ' ' << rot(3) << ' ';
814  // cam params
815  outfile << sba.nodes[i].Kcam(0,0) << ' ' << sba.nodes[i].Kcam(1,1) << ' ' <<
816  sba.nodes[i].Kcam(0,2) << ' ' << sba.nodes[i].Kcam(1,2) << ' ' << sba.nodes[i].baseline << endl;
817  }
818 
819  // Info about each point
820  // point indices are sba indices plus ncams (so they're unique in the file)
821  // VERTEX_POINT n x y z
822  // after each point comes the projections
823  // EDGE_PROJECT_P2C pt_ind cam_ind u v
824  for (i = 0; i < sba.tracks.size(); i++)
825  {
826  outfile << "VERTEX_XYZ" << ' ' << ncams+i << ' '; // index
827  // World <x y z>
828  outfile << sba.tracks[i].point(0) << ' ' << sba.tracks[i].point(1)
829  << ' ' << sba.tracks[i].point(2) << endl;
830 
831  ProjMap &prjs = sba.tracks[i].projections;
832 
833  // Output all projections
834  // Mono projections have 0 for the disparity
835  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
836  {
837  // TODO: output real covariance, if available
838  Proj &prj = itr->second;
839  if (prj.stereo && !mono)
840  {
841  outfile << "EDGE_PROJECT_P2SC "; // stereo edge
842  outfile << ncams+i << ' ' << prj.ndi << ' ' << prj.kp(0) << ' '
843  << prj.kp(1) << ' ' << prj.kp(2) << ' ';
844  outfile << "1 0 0 0 1 1" << endl; // covariance
845  }
846  else
847  {
848  outfile << "EDGE_PROJECT_P2MC "; // mono edge
849  outfile << ncams+i << ' ' << prj.ndi << ' ' << prj.kp(0) << ' '
850  << prj.kp(1) << ' ';
851  outfile << "1 0 1" << endl; // covariance
852  }
853  }
854  }
855 
856  return 0;
857 }
858 
859 
860 //
861 // SPA (3D pose graphs)
862 //
863 
864 
865 //
866 // add a single node to the graph, in the position given by the VERTEX2 entry in the file
867 //
868 
869 void
870 addnode(SysSPA &spa, int n,
871  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ntrans, // node translation
872  std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > nqrot, // node rotation
873  std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > cind, // constraint indices
874  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ctrans, // constraint local translation
875  std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > cqrot, // constraint local rotation as quaternion
876  std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > prec) // constraint covariance
877 {
878  Node nd;
879 
880  // rotation
881  Quaternion<double> frq;
882  frq.coeffs() = nqrot[n];
883 #if 0
884  frq.normalize();
885  if (frq.w() <= 0.0) frq.coeffs() = -frq.coeffs();
886  nd.qrot = frq.coeffs();
887 #endif
888 
889  // translation
890  Vector4d v;
891  v.head(3) = ntrans[n];
892  v(3) = 1.0;
893 
894  spa.addNode(v,frq);
895 
896 #if 0
897  nd.trans = v;
898  nd.setTransform(); // set up world2node transform
899  nd.setDr(true);
900 
901  // add to system
902  spa.nodes.push_back(nd);
903 #endif
904 
905  // add in constraints
906  for (int i=0; i<(int)ctrans.size(); i++)
907  {
908  ConP2 con;
909  con.ndr = cind[i].x();
910  con.nd1 = cind[i].y();
911 
912  if ((con.ndr == n && con.nd1 <= n-1) ||
913  (con.nd1 == n && con.ndr <= n-1))
914  {
915  con.tmean = ctrans[i];
916  Quaternion<double> qr;
917  qr.coeffs() = cqrot[i];
918  qr.normalize();
919  con.qpmean = qr.inverse(); // inverse of the rotation measurement
920  con.prec = prec[i]; // ??? should this be inverted ???
921 
922  // need a boost for noise-offset system
923  //con.prec.block<3,3>(3,3) *= 10.0;
924  spa.p2cons.push_back(con);
925  }
926  }
927 }
928 
929 int sba::readSPAGraphFile(const char *filename, SysSPA& spaout)
930 {
931  // Create vectors to hold the data from the graph file.
932  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ntrans; // node translation
933  std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > nqrot; // node rotation
934  std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > cind; // constraint indices
935  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ctrans; // constraint local translation
936  std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > cqrot; // constraint local rotation as quaternion
937  std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > prec; // constraint covariance
938 
939  int ret = ParseSPAGraphFile(filename, ntrans, nqrot, cind, ctrans, cqrot, prec);
940  if (ret < 0)
941  return -1;
942 
943  cout << "# [ReadSPAFile] Found " << (int)ntrans.size() << " nodes and "
944  << (int)cind.size() << " constraints" << endl;
945 
946  int nnodes = ntrans.size();
947 
948  // add in nodes
949  for (int i=0; i<nnodes; i++)
950  addnode(spaout, i, ntrans, nqrot, cind, ctrans, cqrot, prec);
951 
952  return 0;
953 }
954 
955 
956 // cv is upper triangular
957 void make_covar(double *cv, Matrix<double,6,6> &m)
958 {
959  m.setZero();
960 
961  int i = 0;
962  m(0,0) = cv[i++];
963  m(0,1) = cv[i++];
964  m(0,2) = cv[i++];
965  m(0,3) = cv[i++];
966  m(0,4) = cv[i++];
967  m(0,5) = cv[i++];
968 
969  m(1,1) = cv[i++];
970  m(1,2) = cv[i++];
971  m(1,3) = cv[i++];
972  m(1,4) = cv[i++];
973  m(1,5) = cv[i++];
974 
975  m(2,2) = cv[i++];
976  m(2,3) = cv[i++];
977  m(2,4) = cv[i++];
978  m(2,5) = cv[i++];
979 
980  m(3,3) = cv[i++];
981  m(3,4) = cv[i++];
982  m(3,5) = cv[i++];
983 
984  m(4,4) = cv[i++];
985  m(4,5) = cv[i++];
986 
987  m(5,5) = cv[i++];
988 
989  // make symmetric
990  Matrix<double,6,6> mt = m.transpose();
991  mt.diagonal().setZero();
992  m = m+mt;
993 }
994 
995 int
996 sba::ParseSPAGraphFile(const char *fin, // input file
997  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &ntrans, // node translation
998  std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > &nqrot, // node rotation
999  std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > &cind, // constraint indices
1000  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &ctrans, // constraint local translation
1001  std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > &cqrot, // constraint local rotation as quaternion
1002  std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > &prec) // constraint covariance
1003 {
1004  ifstream ifs(fin);
1005  if (!ifs)
1006  {
1007  cout << "Can't open file " << fin << endl;
1008  return -1;
1009  }
1010  ifs.precision(10);
1011 
1012  // loop over lines
1013  string line;
1014  int nline = 0;
1015  bool first = true;
1016  while (getline(ifs,line))
1017  {
1018  nline++;
1019  stringstream ss(line); // make a string stream
1020  string type;
1021  ss >> type;
1022  size_t pos = type.find("#");
1023  if (pos != string::npos)
1024  continue; // comment line
1025 
1026  if (type == "VERTEX_SE3") // have a vertex
1027  {
1028  int n;
1029  double tx,ty,tz,rr,rp,ry;
1030  if (!(ss >> n >> tx >> ty >> tz >> rr >> rp >> ry))
1031  {
1032  cout << "[ReadSPA] Bad VERTEX_SE3 at line " << nline << endl;
1033  return -1;
1034  }
1035  ntrans.push_back(Vector3d(tx,ty,tz));
1036  Vector4d v;
1037  make_qrot(rr,rp,ry,v);
1038  nqrot.push_back(v);
1039  }
1040 
1041  if (type == "EDGE_SE3_SE3") // have an edge
1042  {
1043  int n1,n2;
1044  double tx,ty,tz,rr,rp,ry;
1045  double cv[21];
1046 
1047  // indices and measurement
1048  if (!(ss >> n1 >> n2 >> tx >> ty >> tz >> rr >> rp >> ry))
1049  {
1050  cout << "[ReadSPA] Bad EDGE_SE3_SE3 at line " << nline << endl;
1051  return -1;
1052  }
1053  cind.push_back(Vector2i(n1,n2));
1054  ctrans.push_back(Vector3d(tx,ty,tz));
1055  Vector4d v;
1056  make_qrot(rr,rp,ry,v);
1057  cqrot.push_back(v);
1058 
1059  // covar
1060  if (!(ss >> cv[0] >> cv[1] >> cv[2] >> cv[3] >> cv[4]
1061  >> cv[5] >> cv[6] >> cv[7] >> cv[8] >> cv[9]
1062  >> cv[10] >> cv[11] >> cv[12] >> cv[13] >> cv[14]
1063  >> cv[15] >> cv[16] >> cv[17] >> cv[18] >> cv[19] >> cv[20]))
1064  {
1065  cout << "[ReadSPA] Bad EDGE_SE3_SE3 at line " << nline << endl;
1066  return -1;
1067  }
1068  Matrix<double,6,6> m;
1069  make_covar(cv,m);
1070  if (first)
1071  {
1072  //cout << endl;
1073  //for (int j=0; j<21; j++);
1074  //cout << cv[j] << " ";
1075  //cout << endl << endl << << m << endl;
1076  first = false;
1077  }
1078  prec.push_back(m);
1079  }
1080 
1081  }
1082  return 0;
1083 }
int nFixed
Number of fixed nodes (nodes with known poses) from the first node.
Definition: sba.h:92
void setDr(bool local=false)
Set angle derivates.
Definition: node.cpp:56
int readGraphFile(const char *filename, sba::SysSBA &sbaout)
Reads bundle adjustment data from a graph-type file to an instance of SysSBA.
SysSPA holds a set of nodes and constraints for sparse pose adjustment.
Definition: sba.h:440
void addnode(SysSPA &spa, int n, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > ntrans, std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > nqrot, std::vector< Eigen::Vector2i, Eigen::aligned_allocator< Eigen::Vector2i > > cind, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > ctrans, std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > cqrot, std::vector< Eigen::Matrix< double, 6, 6 >, Eigen::aligned_allocator< Eigen::Matrix< double, 6, 6 > > > prec)
std::map< const int, Proj, std::less< int >, Eigen::aligned_allocator< Proj > > ProjMap
Definition: proj.h:48
Eigen::Matrix< double, 3, 3 > Kcam
Projection to frame image coordinates. pre-multiply by the frame camera matrix.
Definition: node.h:92
Eigen::MatrixXd A
linear system matrix and vector
Definition: sba.h:240
SysSBA holds a set of nodes and points for sparse bundle adjustment.
Definition: sba.h:75
Eigen::Matrix< double, 6, 6 > prec
Definition: sba.h:311
int nd1
Node index for the second node.
Definition: sba.h:306
Eigen::Vector3d tmean
Mean vector, quaternion (inverse) and precision matrix for this constraint.
Definition: sba.h:309
int ParseSPAGraphFile(const char *fin, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &ntrans, std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > &nqrot, std::vector< Eigen::Vector2i, Eigen::aligned_allocator< Eigen::Vector2i > > &cind, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &ctrans, std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > &cqrot, std::vector< Eigen::Matrix< double, 6, 6 >, Eigen::aligned_allocator< Eigen::Matrix< double, 6, 6 > > > &cvar)
A low-level parser for graph files.
Matrix< double, 11, 1 > Vector11d
Definition: sba_file_io.h:20
static void make_qrot(double rr, double rp, double ry, Vector4d &v)
int readBundlerFile(const char *filename, sba::SysSBA &sbaout)
Reads bundle adjustment data from a Bundler file to an instance of SysSBA.
Definition: sba_file_io.cpp:9
bool addMonoProj(int ci, int pi, Eigen::Vector2d &q)
Add a projection between point and camera, in setting up the system.
Definition: sba.cpp:145
NODE holds graph nodes corresponding to frames, for use in sparse bundle adjustment. Each node has a 6DOF pose, encoded as a translation vector and rotation unit quaternion (Eigen classes). These represent the pose of the node in the world frame.
Definition: node.h:63
int writeGraphFile(const char *filename, SysSBA &sba, bool mono=false)
Writes out the current SBA system as an ascii graph file suitable to be read in by the Freiburg HChol...
std::vector< Track, Eigen::aligned_allocator< Track > > tracks
Set of tracks for each point P (projections onto camera frames).
Definition: sba.h:95
void make_covar(double *cv, Matrix< double, 6, 6 > &m)
Matrix< double, 5, 1 > Vector5d
Definition: sba_file_io.h:21
std::vector< Node, Eigen::aligned_allocator< Node > > nodes
Set of nodes (camera frames) for SBA system, indexed by node number.
Definition: sba.h:89
bool stereo
Whether the projection is Stereo (True) or Monocular (False).
Definition: proj.h:87
Eigen::Matrix< double, 3, 4 > w2n
Resultant transform from world to node coordinates.
Definition: node.h:80
Definition: bpcg.h:60
int readSPAGraphFile(const char *filename, SysSPA &spaout)
Reads 3D pose graph data from a graph-type file to an instance of SysSPA.
bool useLocalAngles
local or global angle coordinates
Definition: sba.h:144
int addNode(Eigen::Matrix< double, 4, 1 > &trans, Eigen::Quaternion< double > &qrot, const fc::CamParams &cp, bool isFixed=false)
Adds a node to the system.
Definition: sba.cpp:83
Proj holds a projection measurement of a point onto a frame. They are a repository for the link betwe...
Definition: proj.h:57
void writeSparseA(const char *fname, SysSBA &sba)
Eigen::VectorXd B
Definition: sba.h:241
Eigen::Quaternion< double > qrot
Rotation of the node expressed as a Quaternion.
Definition: node.h:70
int ParseBundlerFile(const char *fin, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &camp, std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > &camR, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &camt, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &ptp, std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > &ptc, std::vector< std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > > &ptt)
A low-level parser for bundler files.
bool addStereoProj(int ci, int pi, Eigen::Vector3d &q)
Add a projection between point and camera, in setting up the system.
Definition: sba.cpp:160
int writeBundlerFile(const char *filename, sba::SysSBA &sbain)
Writes bundle adjustment data from an instance of SysSBA to a Bundler file.
int ndi
Node index, the camera node for this projection.
Definition: proj.h:78
int ParseGraphFile(const char *fin, std::vector< Eigen::Vector5d, Eigen::aligned_allocator< Eigen::Vector5d > > &camps, std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > &camqs, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &camts, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &ptps, std::vector< std::vector< Eigen::Vector11d, Eigen::aligned_allocator< Eigen::Vector11d > > > &ptts)
A low-level parser for graph files.
bool isValid
valid or not (could be out of bounds)
Definition: proj.h:127
void writeLourakisFile(const char *fname, SysSBA &sba)
write out system in SBA form
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 4, 1 > trans
Translation in homogeneous coordinates, last element is 1.0.
Definition: node.h:69
std::vector< Node, Eigen::aligned_allocator< Node > > nodes
set of nodes (camera frames) for SPA system, indexed by position;
Definition: sba.h:474
void setTransform()
Sets the transform matrices of the node.
Definition: node.cpp:8
void writeA(const char *fname, SysSBA &sba)
Eigen::Vector3d kp
Keypoint, u,v,u-d vector.
Definition: proj.h:81
int addPoint(Point p)
Adds a point to the system.
Definition: sba.cpp:104
Eigen::Vector4d Point
Point holds 3D points using in world coordinates. Currently we just represent these as 4-vectors...
Definition: node.h:43
int addNode(Eigen::Matrix< double, 4, 1 > &trans, Eigen::Quaternion< double > &qrot, bool isFixed=false)
Adds a node to the system.
Definition: spa.cpp:644
Eigen::Quaternion< double > qpmean
Definition: sba.h:310
std::vector< ConP2, Eigen::aligned_allocator< ConP2 > > p2cons
Set of P2 constraints.
Definition: sba.h:483
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int ndr
Reference pose index.
Definition: sba.h:303


sparse_bundle_adjustment
Author(s):
autogenerated on Fri Mar 15 2019 02:41:46