Loading...
Searching...
No Matches
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/base/serialization.h>
#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_ALLOW_DEPRECATED_SINCE_V42
void GTSAM_DEPRECATED add_measurement(size_t idx, const gtsam::Point2& m) {
measurements.emplace_back(idx, m);
}
size_t GTSAM_DEPRECATED number_measurements() const {
return measurements.size();
}
#endif
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);
}
};
template <typename T>
struct traits;
template <>
struct traits<SfmTrack> : public Testable<SfmTrack> {};
} // namespace gtsam
serialization.h
Convenience functions for serializing data structures via boost.serialization.
Point2.h
2D Point
Point3.h
3D Point
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::SiftIndex
std::pair< size_t, size_t > SiftIndex
Sift index for SfmTrack.
Definition SfmTrack.h:36
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
gtsam::SfmMeasurement
std::pair< size_t, Point2 > SfmMeasurement
A measurement with its camera index.
Definition SfmTrack.h:33
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::SfmTrack2d::addMeasurement
void addMeasurement(size_t idx, const gtsam::Point2 &m)
Add measurement (camera_idx, Point2) to track.
Definition SfmTrack.h:65
gtsam::SfmTrack2d::measurement
const SfmMeasurement & measurement(size_t idx) const
Get the measurement (camera index, Point2) at pose index idx.
Definition SfmTrack.h:73
gtsam::SfmTrack2d::siftIndices
std::vector< SiftIndex > siftIndices
The feature descriptors (optional).
Definition SfmTrack.h:48
gtsam::SfmTrack2d::siftIndex
const SiftIndex & siftIndex(size_t idx) const
Get the SIFT feature index corresponding to the measurement at idx.
Definition SfmTrack.h:78
gtsam::SfmTrack2d::hasUniqueCameras
bool hasUniqueCameras() const
Check that no two measurements are from the same camera.
Definition SfmTrack.h:84
gtsam::SfmTrack2d::measurementMatrix
Eigen::MatrixX2d measurementMatrix() const
Return the measurements as a 2D matrix.
Definition SfmTrack.h:100
gtsam::SfmTrack2d::numberMeasurements
size_t numberMeasurements() const
Total number of measurements in this track.
Definition SfmTrack.h:70
gtsam::SfmTrack2d::measurements
std::vector< SfmMeasurement > measurements
The 2D image projections (id,(u,v)).
Definition SfmTrack.h:45
gtsam::SfmTrack2d::indexVector
Eigen::VectorXi indexVector() const
Return the camera indices of the measurements.
Definition SfmTrack.h:109
gtsam::SfmTrack
Definition SfmTrack.h:126
gtsam::SfmTrack::rgb
Point3 rgb() const
Get RGB values describing 3d point.
Definition SfmTrack.h:148
gtsam::SfmTrack::b
float b
RGB color of the 3D point.
Definition SfmTrack.h:128
gtsam::SfmTrack::point3
const Point3 & point3() const
Get 3D point.
Definition SfmTrack.h:145
gtsam::SfmTrack::p
Point3 p
3D position of the point
Definition SfmTrack.h:127