sba_file_io.h

Go to the documentation of this file.

#ifndef SBA_BUNDLER_FILE_H
#define SBA_BUNDLER_FILE_H

#ifndef EIGEN_USE_NEW_STDVECTOR
#define EIGEN_USE_NEW_STDVECTOR
#endif // EIGEN_USE_NEW_STDVECTOR

//#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(10)

#include <Eigen/Eigen>
#include <vector>
#include <iostream>
#include <fstream>
#include <iomanip>

#include "sba/sba.h"

namespace Eigen
{
  typedef Matrix<double,11,1> Vector11d;
  typedef Matrix<double,5,1>  Vector5d;
}

namespace sba
{

  int readBundlerFile(const char *filename, sba::SysSBA& sbaout);

  int writeBundlerFile(const char *filename, sba::SysSBA& sbain);

  int 
  ParseBundlerFile(const char *fin,     // input file
                  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &camp, // cam params <f d1 d2>
                  std::vector< Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> > &camR, // cam rotation matrix
                  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &camt, // cam translation
                  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &ptp, // point position
                  std::vector< Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > &ptc, // point color
                  std::vector< std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > > &ptt // point tracks - each vector is <camera_index u v>
    );

  int readGraphFile(const char *filename, sba::SysSBA& sbaout);

  int 
  ParseGraphFile(const char *fin,       // input file
                  std::vector< Eigen::Vector5d, Eigen::aligned_allocator<Eigen::Vector5d> > &camps, // cam params <fx fy cx cy>
                  std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > &camqs, // cam rotation matrix
                  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &camts, // cam translation
                  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &ptps, // point position
                  std::vector< std::vector< Eigen::Vector11d, Eigen::aligned_allocator<Eigen::Vector11d> > > &ptts // point tracks - each vector is <camera_index u v>
    );


  void writeLourakisFile(const char *fname, SysSBA& sba);
  void writeA(const char *fname, SysSBA& sba); // save precision matrix
  void writeSparseA(const char *fname, SysSBA& sba); // save precision matrix in CSPARSE format

  int writeGraphFile(const char *filename, SysSBA& sba, bool mono=false);


  int readSPAGraphFile(const char *filename, SysSPA& spaout);

  int
  ParseSPAGraphFile(const char *fin, // input file
   std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &ntrans, // node translation
   std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > &nqrot,  // node rotation
   std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > &cind,   // constraint indices
   std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &ctrans, // constraint local translation 
   std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > &cqrot,  // constraint local rotation as quaternion
   std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > &cvar); // constraint covariance

}; // namespace SBA

#endif