SfmData.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/SfmTrack.h>

#include <string>
#include <vector>

namespace gtsam {

typedef PinholeCamera<Cal3Bundler> SfmCamera;

struct GTSAM_EXPORT SfmData {
  std::vector<SfmCamera> cameras;  

  std::vector<SfmTrack> tracks;  


  static SfmData FromBundlerFile(const std::string& filename);

  static SfmData FromBalFile(const std::string& filename);


  void addTrack(const SfmTrack& t) { tracks.push_back(t); }

  void addCamera(const SfmCamera& cam) { cameras.push_back(cam); }

  size_t numberTracks() const { return tracks.size(); }

  size_t numberCameras() const { return cameras.size(); }

  const SfmTrack& track(size_t idx) const { return tracks[idx]; }

  const SfmCamera& camera(size_t idx) const { return cameras[idx]; }

  const std::vector<SfmCamera>& cameraList() const { return cameras; }
  const std::vector<SfmTrack>& trackList() const { return tracks; }

  NonlinearFactorGraph generalSfmFactors(
      const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2,
                                                                   1.0)) const;

  NonlinearFactorGraph sfmFactorGraph(
      const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, 1.0),
      std::optional<size_t> fixedCamera = 0,
      std::optional<size_t> fixedPoint = 0) const;


  void print(const std::string& s = "") const;

  bool equals(const SfmData& sfmData, double tol = 1e-9) const;


#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class Archive>
  void serialize(Archive& ar, const unsigned int /*version*/) {
    ar& BOOST_SERIALIZATION_NVP(cameras);
    ar& BOOST_SERIALIZATION_NVP(tracks);
  }
#endif


};

template <>
struct traits<SfmData> : public Testable<SfmData> {};

GTSAM_EXPORT SfmData readBal(const std::string& filename);

GTSAM_EXPORT bool writeBAL(const std::string& filename, const SfmData& data);

GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
                                     const SfmData& data, const Values& values);

GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz);

GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);

GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);

GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);

GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);

}  // namespace gtsam