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
12/**
13 * @file SfmTrack.h
14 * @date January 2022
15 * @author Frank Dellaert
16 * @brief A simple data structure for a track in Structure from Motion
17 */
18
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
29namespace gtsam {
30
31/// A measurement with its camera index
32typedef std::pair<size_t, Point2> SfmMeasurement;
33
34/// Sift index for SfmTrack
35typedef std::pair<size_t, size_t> SiftIndex;
36
37/**
38 * @brief Track containing 2D measurements associated with a single 3D point.
39 * Note: Equivalent to gtsam.SfmTrack, but without the 3d measurement.
40 * This class holds data temporarily before 3D point is initialized.
41 */
42struct GTSAM_EXPORT SfmTrack2d {
43 /// The 2D image projections (id,(u,v))
44 std::vector<SfmMeasurement> measurements;
45
46 /// The feature descriptors (optional)
47 std::vector<SiftIndex> siftIndices;
48
49 /// @name Constructors
50 /// @{
51
52 // Default constructor.
53 SfmTrack2d() = default;
54
55 // Constructor from measurements.
56 explicit SfmTrack2d(const std::vector<SfmMeasurement>& measurements)
57 : measurements(measurements) {}
58
59 /// @}
60 /// @name Standard Interface
61 /// @{
62
63 /// Add measurement (camera_idx, Point2) to track
64 void addMeasurement(size_t idx, const gtsam::Point2& m) {
65 measurements.emplace_back(args&: idx, args: m);
66 }
67
68 /// Total number of measurements in this track
69 size_t numberMeasurements() const { return measurements.size(); }
70
71 /// Get the measurement (camera index, Point2) at pose index `idx`
72 const SfmMeasurement& measurement(size_t idx) const {
73 return measurements[idx];
74 }
75
76 /// Get the SIFT feature index corresponding to the measurement at `idx`
77 const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; }
78
79 /**
80 * @brief Check that no two measurements are from the same camera.
81 * @returns boolean result of the validation.
82 */
83 bool hasUniqueCameras() const {
84 std::vector<size_t> cameraIndices;
85 for (auto& measurement : measurements) {
86 cameraIndices.emplace_back(args: measurement.first);
87 }
88 auto i = std::adjacent_find(first: cameraIndices.begin(), last: cameraIndices.end());
89 bool all_cameras_unique = (i == cameraIndices.end());
90 return all_cameras_unique;
91 }
92
93 /// @}
94 /// @name Vectorized Interface
95 /// @{
96
97 /// @brief Return the measurements as a 2D matrix
98 Eigen::MatrixX2d measurementMatrix() const {
99 Eigen::MatrixX2d m(numberMeasurements(), 2);
100 for (size_t i = 0; i < numberMeasurements(); i++) {
101 m.row(i) = measurement(idx: i).second;
102 }
103 return m;
104 }
105
106 /// @brief Return the camera indices of the measurements
107 Eigen::VectorXi indexVector() const {
108 Eigen::VectorXi v(numberMeasurements());
109 for (size_t i = 0; i < numberMeasurements(); i++) {
110 v(i) = measurement(idx: i).first;
111 }
112 return v;
113 }
114
115 /// @}
116};
117
118using SfmTrack2dVector = std::vector<SfmTrack2d>;
119
120/**
121 * @brief An SfmTrack stores SfM measurements grouped in a track
122 * @addtogroup sfm
123 */
124struct GTSAM_EXPORT SfmTrack : SfmTrack2d {
125 Point3 p; ///< 3D position of the point
126 float r, g, b; ///< RGB color of the 3D point
127
128 /// @name Constructors
129 /// @{
130
131 explicit SfmTrack(float r = 0, float g = 0, float b = 0)
132 : p(0, 0, 0), r(r), g(g), b(b) {}
133
134 explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0,
135 float b = 0)
136 : p(pt), r(r), g(g), b(b) {}
137
138 /// @}
139 /// @name Standard Interface
140 /// @{
141
142 /// Get 3D point
143 const Point3& point3() const { return p; }
144
145 /// Get RGB values describing 3d point
146 Point3 rgb() const { return Point3(r, g, b); }
147
148 /// @}
149 /// @name Testable
150 /// @{
151
152 /// print
153 void print(const std::string& s = "") const;
154
155 /// assert equality up to a tolerance
156 bool equals(const SfmTrack& sfmTrack, double tol = 1e-9) const;
157
158 /// @}
159 /// @name Serialization
160 /// @{
161
162#if GTSAM_ENABLE_BOOST_SERIALIZATION
163 /** Serialization function */
164 friend class boost::serialization::access;
165 template <class ARCHIVE>
166 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
167 ar& BOOST_SERIALIZATION_NVP(p);
168 ar& BOOST_SERIALIZATION_NVP(r);
169 ar& BOOST_SERIALIZATION_NVP(g);
170 ar& BOOST_SERIALIZATION_NVP(b);
171 ar& BOOST_SERIALIZATION_NVP(measurements);
172 ar& BOOST_SERIALIZATION_NVP(siftIndices);
173 }
174#endif
175 /// @}
176};
177
178template <typename T>
179struct traits;
180
181template <>
182struct traits<SfmTrack> : public Testable<SfmTrack> {};
183
184} // namespace gtsam
185