SfmTrack.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #pragma once
20 
21 #include <gtsam/geometry/Point2.h>
22 #include <gtsam/geometry/Point3.h>
23 
24 #include <Eigen/Core>
25 #include <string>
26 #include <utility>
27 #include <vector>
28 
29 namespace gtsam {
30 
32 typedef std::pair<size_t, Point2> SfmMeasurement;
33 
35 typedef std::pair<size_t, size_t> SiftIndex;
36 
42 struct GTSAM_EXPORT SfmTrack2d {
44  std::vector<SfmMeasurement> measurements;
45 
47  std::vector<SiftIndex> siftIndices;
48 
51 
52  // Default constructor.
53  SfmTrack2d() = default;
54 
55  // Constructor from measurements.
56  explicit SfmTrack2d(const std::vector<SfmMeasurement>& measurements)
57  : measurements(measurements) {}
58 
62 
64  void addMeasurement(size_t idx, const gtsam::Point2& m) {
65  measurements.emplace_back(idx, m);
66  }
67 
69  size_t numberMeasurements() const { return measurements.size(); }
70 
72  const SfmMeasurement& measurement(size_t idx) const {
73  return measurements[idx];
74  }
75 
77  const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; }
78 
83  bool hasUniqueCameras() const {
84  std::vector<int> track_cam_indices;
85  for (auto& measurement : measurements) {
86  track_cam_indices.emplace_back(measurement.first);
87  }
88  auto i =
89  std::adjacent_find(track_cam_indices.begin(), track_cam_indices.end());
90  bool all_cameras_unique = (i == track_cam_indices.end());
91  return all_cameras_unique;
92  }
93 
97 
99  Eigen::MatrixX2d measurementMatrix() const {
100  Eigen::MatrixX2d m(numberMeasurements(), 2);
101  for (size_t i = 0; i < numberMeasurements(); i++) {
102  m.row(i) = measurement(i).second;
103  }
104  return m;
105  }
106 
108  Eigen::VectorXi indexVector() const {
109  Eigen::VectorXi v(numberMeasurements());
110  for (size_t i = 0; i < numberMeasurements(); i++) {
111  v(i) = measurement(i).first;
112  }
113  return v;
114  }
115 
117 };
118 
119 using SfmTrack2dVector = std::vector<SfmTrack2d>;
120 
125 struct GTSAM_EXPORT SfmTrack : SfmTrack2d {
127  float r, g, b;
128 
131 
132  explicit SfmTrack(float r = 0, float g = 0, float b = 0)
133  : p(0, 0, 0), r(r), g(g), b(b) {}
134 
135  explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0,
136  float b = 0)
137  : p(pt), r(r), g(g), b(b) {}
138 
142 
144  const Point3& point3() const { return p; }
145 
147  Point3 rgb() const { return Point3(r, g, b); }
148 
152 
154  void print(const std::string& s = "") const;
155 
157  bool equals(const SfmTrack& sfmTrack, double tol = 1e-9) const;
158 
162 
163 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
164 
165  friend class boost::serialization::access;
166  template <class ARCHIVE>
167  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
168  ar& BOOST_SERIALIZATION_NVP(p);
169  ar& BOOST_SERIALIZATION_NVP(r);
170  ar& BOOST_SERIALIZATION_NVP(g);
171  ar& BOOST_SERIALIZATION_NVP(b);
172  ar& BOOST_SERIALIZATION_NVP(measurements);
173  ar& BOOST_SERIALIZATION_NVP(siftIndices);
174  }
175 #endif
176 };
178 
179 template <typename T>
180 struct traits;
181 
182 template <>
183 struct traits<SfmTrack> : public Testable<SfmTrack> {};
184 
185 } // namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Matrix3f m
Point3 rgb() const
Get RGB values describing 3d point.
Definition: SfmTrack.h:147
std::string serialize(const T &input)
serializes to a string
Vector2 Point2
Definition: Point2.h:32
std::vector< SiftIndex > siftIndices
The feature descriptors (optional)
Definition: SfmTrack.h:47
bool hasUniqueCameras() const
Check that no two measurements are from the same camera.
Definition: SfmTrack.h:83
static const Point3 pt(1.0, 2.0, 3.0)
const Point3 & point3() const
Get 3D point.
Definition: SfmTrack.h:144
void g(const string &key, int i)
Definition: testBTree.cpp:41
const SiftIndex & siftIndex(size_t idx) const
Get the SIFT feature index corresponding to the measurement at idx
Definition: SfmTrack.h:77
static Point2 measurement(323.0, 240.0)
std::pair< size_t, Point2 > SfmMeasurement
A measurement with its camera index.
Definition: SfmTrack.h:32
size_t numberMeasurements() const
Total number of measurements in this track.
Definition: SfmTrack.h:69
void addMeasurement(size_t idx, const gtsam::Point2 &m)
Add measurement (camera_idx, Point2) to track.
Definition: SfmTrack.h:64
const SfmMeasurement & measurement(size_t idx) const
Get the measurement (camera index, Point2) at pose index idx
Definition: SfmTrack.h:72
std::pair< size_t, size_t > SiftIndex
Sift index for SfmTrack.
Definition: SfmTrack.h:35
Track containing 2D measurements associated with a single 3D point. Note: Equivalent to gtsam...
Definition: SfmTrack.h:42
Array< int, Dynamic, 1 > v
Eigen::MatrixX2d measurementMatrix() const
Return the measurements as a 2D matrix.
Definition: SfmTrack.h:99
std::vector< SfmMeasurement > measurements
The 2D image projections (id,(u,v))
Definition: SfmTrack.h:44
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
SfmTrack2d(const std::vector< SfmMeasurement > &measurements)
Definition: SfmTrack.h:56
const G & b
Definition: Group.h:86
traits
Definition: chartTesting.h:28
std::vector< SfmTrack2d > SfmTrack2dVector
Definition: SfmTrack.h:119
Point3 p
3D position of the point
Definition: SfmTrack.h:126
3D Point
float * p
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
SfmTrack(float r=0, float g=0, float b=0)
Definition: SfmTrack.h:132
Eigen::VectorXi indexVector() const
Return the camera indices of the measurements.
Definition: SfmTrack.h:108
2D Point
SfmTrack(const gtsam::Point3 &pt, float r=0, float g=0, float b=0)
Definition: SfmTrack.h:135


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:42