sba_file_io.h
Go to the documentation of this file.
1 #ifndef SBA_BUNDLER_FILE_H
2 #define SBA_BUNDLER_FILE_H
3 
4 #ifndef EIGEN_USE_NEW_STDVECTOR
5 #define EIGEN_USE_NEW_STDVECTOR
6 #endif // EIGEN_USE_NEW_STDVECTOR
7 
8 //#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(10)
9 
10 #include <Eigen/Eigen>
11 #include <vector>
12 #include <iostream>
13 #include <fstream>
14 #include <iomanip>
15 
17 
18 namespace Eigen
19 {
20  typedef Matrix<double,11,1> Vector11d;
21  typedef Matrix<double,5,1> Vector5d;
22 }
23 
24 namespace sba
25 {
26 
36  int readBundlerFile(const char *filename, sba::SysSBA& sbaout);
37 
49  int writeBundlerFile(const char *filename, sba::SysSBA& sbain);
50 
52  int
53  ParseBundlerFile(const char *fin, // input file
54  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &camp, // cam params <f d1 d2>
55  std::vector< Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> > &camR, // cam rotation matrix
56  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &camt, // cam translation
57  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &ptp, // point position
58  std::vector< Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > &ptc, // point color
59  std::vector< std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > > &ptt // point tracks - each vector is <camera_index u v>
60  );
61 
68  int readGraphFile(const char *filename, sba::SysSBA& sbaout);
69 
71  int
72  ParseGraphFile(const char *fin, // input file
73  std::vector< Eigen::Vector5d, Eigen::aligned_allocator<Eigen::Vector5d> > &camps, // cam params <fx fy cx cy>
74  std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > &camqs, // cam rotation matrix
75  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &camts, // cam translation
76  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &ptps, // point position
77  std::vector< std::vector< Eigen::Vector11d, Eigen::aligned_allocator<Eigen::Vector11d> > > &ptts // point tracks - each vector is <camera_index u v>
78  );
79 
80 
82  void writeLourakisFile(const char *fname, SysSBA& sba);
83  void writeA(const char *fname, SysSBA& sba); // save precision matrix
84  void writeSparseA(const char *fname, SysSBA& sba); // save precision matrix in CSPARSE format
85 
91  int writeGraphFile(const char *filename, SysSBA& sba, bool mono=false);
92 
93 
101  int readSPAGraphFile(const char *filename, SysSPA& spaout);
102 
104  int
105  ParseSPAGraphFile(const char *fin, // input file
106  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &ntrans, // node translation
107  std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > &nqrot, // node rotation
108  std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > &cind, // constraint indices
109  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &ctrans, // constraint local translation
110  std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > &cqrot, // constraint local rotation as quaternion
111  std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > &cvar); // constraint covariance
112 
113 }; // namespace SBA
114 
115 #endif
int readGraphFile(const char *filename, sba::SysSBA &sbaout)
Reads bundle adjustment data from a graph-type file to an instance of SysSBA.
SysSBA holds a set of nodes and points for sparse bundle adjustment.
Definition: sba.h:75
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
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
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...
Matrix< double, 5, 1 > Vector5d
Definition: sba_file_io.h:21
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.
void writeSparseA(const char *fname, SysSBA &sba)
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.
int writeBundlerFile(const char *filename, sba::SysSBA &sbain)
Writes bundle adjustment data from an instance of SysSBA to a Bundler file.
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.
void writeLourakisFile(const char *fname, SysSBA &sba)
write out system in SBA form
void writeA(const char *fname, SysSBA &sba)


sparse_bundle_adjustment
Author(s):
autogenerated on Fri Apr 3 2020 03:30:53