SfmTrack.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/Point2.h>
#include <gtsam/geometry/Point3.h>

#include <Eigen/Core>
#include <string>
#include <utility>
#include <vector>

namespace gtsam {

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

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

struct GTSAM_EXPORT SfmTrack2d {
  std::vector<SfmMeasurement> measurements;

  std::vector<SiftIndex> siftIndices;


  // Default constructor.
  SfmTrack2d() = default;

  // Constructor from measurements.
  explicit SfmTrack2d(const std::vector<SfmMeasurement>& measurements)
      : measurements(measurements) {}


  void addMeasurement(size_t idx, const gtsam::Point2& m) {
    measurements.emplace_back(idx, m);
  }

  size_t numberMeasurements() const { return measurements.size(); }

  const SfmMeasurement& measurement(size_t idx) const {
    return measurements[idx];
  }

  const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; }

  bool hasUniqueCameras() const {
    std::vector<int> track_cam_indices;
    for (auto& measurement : measurements) {
      track_cam_indices.emplace_back(measurement.first);
    }
    auto i =
        std::adjacent_find(track_cam_indices.begin(), track_cam_indices.end());
    bool all_cameras_unique = (i == track_cam_indices.end());
    return all_cameras_unique;
  }


  Eigen::MatrixX2d measurementMatrix() const {
    Eigen::MatrixX2d m(numberMeasurements(), 2);
    for (size_t i = 0; i < numberMeasurements(); i++) {
      m.row(i) = measurement(i).second;
    }
    return m;
  }

  Eigen::VectorXi indexVector() const {
    Eigen::VectorXi v(numberMeasurements());
    for (size_t i = 0; i < numberMeasurements(); i++) {
      v(i) = measurement(i).first;
    }
    return v;
  }

};

using SfmTrack2dVector = std::vector<SfmTrack2d>;

struct GTSAM_EXPORT SfmTrack : SfmTrack2d {
  Point3 p;       
  float r, g, b;  


  explicit SfmTrack(float r = 0, float g = 0, float b = 0)
      : p(0, 0, 0), r(r), g(g), b(b) {}

  explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0,
                    float b = 0)
      : p(pt), r(r), g(g), b(b) {}


  const Point3& point3() const { return p; }

  Point3 rgb() const { return Point3(r, g, b); }


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

  bool equals(const SfmTrack& sfmTrack, 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(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);
  }
#endif
};

template <typename T>
struct traits;

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

}  // namespace gtsam