dataset.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/sfm/BinaryMeasurement.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/serialization.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/types.h>

#include <boost/smart_ptr/shared_ptr.hpp>
#include <string>
#include <utility> // for pair
#include <vector>
#include <iosfwd>
#include <map>

namespace gtsam {

GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);

GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);

enum NoiseFormat {
  NoiseFormatG2O, 
  NoiseFormatTORO, 
  NoiseFormatGRAPH, 
  NoiseFormatCOV, 
  NoiseFormatAUTO  
};

enum KernelFunctionType {
  KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
};

template <typename T>
GTSAM_EXPORT std::map<size_t, T> parseVariables(const std::string &filename,
                                                size_t maxIndex = 0);

template <typename T>
GTSAM_EXPORT std::vector<BinaryMeasurement<T>>
parseMeasurements(const std::string &filename,
                  const noiseModel::Diagonal::shared_ptr &model = nullptr,
                  size_t maxIndex = 0);

template <typename T>
GTSAM_EXPORT std::vector<typename BetweenFactor<T>::shared_ptr>
parseFactors(const std::string &filename,
             const noiseModel::Diagonal::shared_ptr &model = nullptr,
             size_t maxIndex = 0);

typedef std::pair<size_t, Pose2> IndexedPose;
typedef std::pair<size_t, Point2> IndexedLandmark;
typedef std::pair<std::pair<size_t, size_t>, Pose2> IndexedEdge;

GTSAM_EXPORT boost::optional<IndexedPose> parseVertexPose(std::istream& is,
    const std::string& tag);

GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream& is,
    const std::string& tag);

GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is,
    const std::string& tag);

using GraphAndValues =
    std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr>;

GTSAM_EXPORT GraphAndValues load2D(
    std::pair<std::string, SharedNoiseModel> dataset, size_t maxIndex = 0,
    bool addNoise = false,
    bool smart = true, //
    NoiseFormat noiseFormat = NoiseFormatAUTO,
    KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);

GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
    SharedNoiseModel model = SharedNoiseModel(), size_t maxIndex = 0, bool addNoise =
        false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, //
    KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);

GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
    const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);

GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
    const Values& config, const noiseModel::Diagonal::shared_ptr model,
    const std::string& filename);

GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D = false,
    KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);

GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
    const Values& estimate, const std::string& filename);

GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);

typedef std::pair<size_t, Point2> SfmMeasurement;

typedef std::pair<size_t, size_t> SiftIndex;

struct SfmTrack {
  SfmTrack(float r = 0, float g = 0, float b = 0): p(0,0,0), r(r), g(g), b(b) {}
  SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0, float b = 0) : p(pt), r(r), g(g), b(b) {}
 
  Point3 p; 
  float r, g, b; 
  std::vector<SfmMeasurement> measurements; 
  std::vector<SiftIndex> siftIndices;
  
  const Point3 rgb() const { return Point3(r, g, b); }

  size_t number_measurements() const {
    return measurements.size();
  }
  SfmMeasurement measurement(size_t idx) const {
    return measurements[idx];
  }
  SiftIndex siftIndex(size_t idx) const {
    return siftIndices[idx];
  }
  const Point3& point3() const {
    return p;
  }
  void add_measurement(size_t idx, const gtsam::Point2& m) {
    measurements.emplace_back(idx, m);
  }

  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(p);
    ar & BOOST_SERIALIZATION_NVP(r);
    ar & BOOST_SERIALIZATION_NVP(g);
    ar & BOOST_SERIALIZATION_NVP(b);
    ar & BOOST_SERIALIZATION_NVP(measurements);
    ar & BOOST_SERIALIZATION_NVP(siftIndices);
  }

  bool equals(const SfmTrack &sfmTrack, double tol = 1e-9) const {
    // check the 3D point
    if (!p.isApprox(sfmTrack.p)) {
      return false;
    }

    // check the RGB values
    if (r!=sfmTrack.r || g!=sfmTrack.g || b!=sfmTrack.b) {
      return false;
    }

    // compare size of vectors for measurements and siftIndices
    if (number_measurements() != sfmTrack.number_measurements() ||
        siftIndices.size() != sfmTrack.siftIndices.size()) {
      return false;
    }

    // compare measurements (order sensitive)
    for (size_t idx = 0; idx < number_measurements(); ++idx) {
      SfmMeasurement measurement = measurements[idx];
      SfmMeasurement otherMeasurement = sfmTrack.measurements[idx];

      if (measurement.first != otherMeasurement.first ||
          !measurement.second.isApprox(otherMeasurement.second)) {
        return false;
      }
    }

    // compare sift indices (order sensitive)
    for (size_t idx = 0; idx < siftIndices.size(); ++idx) {
      SiftIndex index = siftIndices[idx];
      SiftIndex otherIndex = sfmTrack.siftIndices[idx];

      if (index.first != otherIndex.first ||
          index.second != otherIndex.second) {
        return false;
      }
    }

    return true;
  }

  void print(const std::string& s = "") const {
    std::cout << "Track with " << measurements.size();
    std::cout << " measurements of point " << p << std::endl;
  }
};

/* ************************************************************************* */
template<>
struct traits<SfmTrack> : public Testable<SfmTrack> {
};


typedef PinholeCamera<Cal3Bundler> SfmCamera;

struct SfmData {
  std::vector<SfmCamera> cameras; 
  std::vector<SfmTrack> tracks; 
  size_t number_cameras() const {
    return cameras.size();
  }
  size_t number_tracks() const {
    return tracks.size();
  }
  SfmCamera camera(size_t idx) const {
    return cameras[idx];
  }
  SfmTrack track(size_t idx) const {
    return tracks[idx];
  }
  void add_track(const SfmTrack& t) {
    tracks.push_back(t);
  }
  void add_camera(const SfmCamera& cam) {
    cameras.push_back(cam);
  }

  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);
  }


  bool equals(const SfmData &sfmData, double tol = 1e-9) const {
    // check number of cameras and tracks
    if (number_cameras() != sfmData.number_cameras() ||
        number_tracks() != sfmData.number_tracks()) {
      return false;
    }

    // check each camera
    for (size_t i = 0; i < number_cameras(); ++i) {
      if (!camera(i).equals(sfmData.camera(i), tol)) {
        return false;
      }
    }

    // check each track
    for (size_t j = 0; j < number_tracks(); ++j) {
      if (!track(j).equals(sfmData.track(j), tol)) {
        return false;
      }
    }

    return true;
  }

  void print(const std::string& s = "") const {
    std::cout << "Number of cameras = " << number_cameras() << std::endl;
    std::cout << "Number of tracks = " << number_tracks() << std::endl;
  }
};

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

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

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

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

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

GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
    const SfmData &data, 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);

// Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose2s
parse2DFactors(const std::string &filename,
               const noiseModel::Diagonal::shared_ptr &model = nullptr,
               size_t maxIndex = 0);

using BetweenFactorPose3s = std::vector<BetweenFactor<Pose3>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose3s
parse3DFactors(const std::string &filename,
               const noiseModel::Diagonal::shared_ptr &model = nullptr,
               size_t maxIndex = 0);

using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
inline boost::optional<IndexedPose> parseVertex(std::istream &is,
                                                const std::string &tag) {
  return parseVertexPose(is, tag);
}

GTSAM_EXPORT std::map<size_t, Pose3> parse3DPoses(const std::string &filename,
                                                  size_t maxIndex = 0);

GTSAM_EXPORT std::map<size_t, Point3>
parse3DLandmarks(const std::string &filename, size_t maxIndex = 0);

#endif
}  // namespace gtsam