From 0fd49efabae89457500948bcae54560d834e44c9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 08:32:13 -0500 Subject: [PATCH] Moved Sfm classes to their own files --- gtsam/sfm/SfmData.cpp | 51 +++++++++++ gtsam/sfm/SfmData.h | 103 ++++++++++++++++++++++ gtsam/sfm/SfmTrack.cpp | 71 +++++++++++++++ gtsam/sfm/SfmTrack.h | 133 ++++++++++++++++++++++++++++ gtsam/slam/dataset.cpp | 24 +++--- gtsam/slam/dataset.h | 191 +---------------------------------------- 6 files changed, 371 insertions(+), 202 deletions(-) create mode 100644 gtsam/sfm/SfmData.cpp create mode 100644 gtsam/sfm/SfmData.h create mode 100644 gtsam/sfm/SfmTrack.cpp create mode 100644 gtsam/sfm/SfmTrack.h diff --git a/gtsam/sfm/SfmData.cpp b/gtsam/sfm/SfmData.cpp new file mode 100644 index 000000000..715dda2ed --- /dev/null +++ b/gtsam/sfm/SfmData.cpp @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file SfmData.cpp + * @date January 2022 + * @author Frank dellaert + * @brief Data structure for dealing with Structure from Motion data + */ + +#include + +namespace gtsam { + +void SfmData::print(const std::string& s) const { + std::cout << "Number of cameras = " << nrCameras() << std::endl; + std::cout << "Number of tracks = " << nrTracks() << std::endl; +} + +bool SfmData::equals(const SfmData& sfmData, double tol) const { + // check number of cameras and tracks + if (nrCameras() != sfmData.nrCameras() || nrTracks() != sfmData.nrTracks()) { + return false; + } + + // check each camera + for (size_t i = 0; i < nrCameras(); ++i) { + if (!camera(i).equals(sfmData.camera(i), tol)) { + return false; + } + } + + // check each track + for (size_t j = 0; j < nrTracks(); ++j) { + if (!track(j).equals(sfmData.track(j), tol)) { + return false; + } + } + + return true; +} + +} // namespace gtsam diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h new file mode 100644 index 000000000..91b7a5d1e --- /dev/null +++ b/gtsam/sfm/SfmData.h @@ -0,0 +1,103 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file SfmData.h + * @date January 2022 + * @author Frank dellaert + * @brief Data structure for dealing with Structure from Motion data + */ + +#pragma once + +#include +#include +#include + +#include +#include + +namespace gtsam { + +/// Define the structure for the camera poses +typedef PinholeCamera SfmCamera; + +/** + * @brief SfmData stores a bunch of SfmTracks + * @addtogroup sfm + */ +struct SfmData { + std::vector cameras; ///< Set of cameras + + std::vector tracks; ///< Sparse set of points + + /// @name Standard Interface + /// @{ + + /// Add a track to SfmData + void addTrack(const SfmTrack& t) { tracks.push_back(t); } + + /// Add a camera to SfmData + void addCamera(const SfmCamera& cam) { cameras.push_back(cam); } + + /// The number of reconstructed 3D points + size_t nrTracks() const { return tracks.size(); } + + /// The number of cameras + size_t nrCameras() const { return cameras.size(); } + + /// The track formed by series of landmark measurements + SfmTrack track(size_t idx) const { return tracks[idx]; } + + /// The camera pose at frame index `idx` + SfmCamera camera(size_t idx) const { return cameras[idx]; } + + /// @} + /// @name Testable + /// @{ + + /// print + void print(const std::string& s = "") const; + + /// assert equality up to a tolerance + bool equals(const SfmData& sfmData, double tol = 1e-9) const; + + /// @} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /// @name Deprecated + /// @{ + void GTSAM_DEPRECATED add_track(const SfmTrack& t) { tracks.push_back(t); } + void GTSAM_DEPRECATED add_camera(const SfmCamera& cam) { + cameras.push_back(cam); + } + size_t GTSAM_DEPRECATED number_tracks() const { return tracks.size(); } + size_t GTSAM_DEPRECATED number_cameras() const { return cameras.size(); } + /// @} +#endif + /// @name Serialization + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(cameras); + ar& BOOST_SERIALIZATION_NVP(tracks); + } + + /// @} +}; + +/// traits +template <> +struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/sfm/SfmTrack.cpp b/gtsam/sfm/SfmTrack.cpp new file mode 100644 index 000000000..11f104c09 --- /dev/null +++ b/gtsam/sfm/SfmTrack.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file SfmTrack.cpp + * @date January 2022 + * @author Frank Dellaert + * @brief A simple data structure for a track in Structure from Motion + */ + +#include + +#include + +namespace gtsam { + +void SfmTrack::print(const std::string& s) const { + std::cout << "Track with " << measurements.size(); + std::cout << " measurements of point " << p << std::endl; +} + +bool SfmTrack::equals(const SfmTrack& sfmTrack, double tolÏ) 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 (nrMeasurements() != sfmTrack.nrMeasurements() || + siftIndices.size() != sfmTrack.siftIndices.size()) { + return false; + } + + // compare measurements (order sensitive) + for (size_t idx = 0; idx < nrMeasurements(); ++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; +} + +} // namespace gtsam diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h new file mode 100644 index 000000000..35603a6f3 --- /dev/null +++ b/gtsam/sfm/SfmTrack.h @@ -0,0 +1,133 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file SfmTrack.h + * @date January 2022 + * @author Frank Dellaert + * @brief A simple data structure for a track in Structure from Motion + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace gtsam { + +/// A measurement with its camera index +typedef std::pair SfmMeasurement; + +/// Sift index for SfmTrack +typedef std::pair SiftIndex; + +/** + * @brief An SfmTrack stores SfM measurements grouped in a track + * @addtogroup sfm + */ +struct SfmTrack { + Point3 p; ///< 3D position of the point + float r, g, b; ///< RGB color of the 3D point + + /// The 2D image projections (id,(u,v)) + std::vector measurements; + + /// The feature descriptors + std::vector siftIndices; + + /// @name Constructors + /// @{ + + 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) {} + + /// @} + /// @name Standard Interface + /// @{ + + /// Add measurement (camera_idx, Point2) to track + void addMeasurement(size_t idx, const gtsam::Point2& m) { + measurements.emplace_back(idx, m); + } + + /// Total number of measurements in this track + size_t nrMeasurements() const { return measurements.size(); } + + /// Get the measurement (camera index, Point2) at pose index `idx` + const SfmMeasurement& measurement(size_t idx) const { + return measurements[idx]; + } + + /// Get the SIFT feature index corresponding to the measurement at `idx` + const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; } + + /// Get 3D point + const Point3& point3() const { return p; } + + /// Get RGB values describing 3d point + Point3 rgb() const { return Point3(r, g, b); } + + /// @} + /// @name Testable + /// @{ + + /// print + void print(const std::string& s = "") const; + + /// assert equality up to a tolerance + bool equals(const SfmTrack& sfmTrack, double tol = 1e-9) const; + + /// @} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /// @name Deprecated + /// @{ + 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 + /// @name Serialization + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + 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 +struct traits; + +template <> +struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index fc020b05b..8acdbfc14 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1149,19 +1149,19 @@ bool writeBAL(const string &filename, SfmData &data) { // Write the number of camera poses and 3D points size_t nrObservations = 0; - for (size_t j = 0; j < data.number_tracks(); j++) { - nrObservations += data.tracks[j].number_measurements(); + for (size_t j = 0; j < data.nrTracks(); j++) { + nrObservations += data.tracks[j].nrMeasurements(); } // Write observations - os << data.number_cameras() << " " << data.number_tracks() << " " + os << data.nrCameras() << " " << data.nrTracks() << " " << nrObservations << endl; os << endl; - for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j + for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j const SfmTrack &track = data.tracks[j]; - for (size_t k = 0; k < track.number_measurements(); + for (size_t k = 0; k < track.nrMeasurements(); k++) { // for each observation of the 3D point j size_t i = track.measurements[k].first; // camera id double u0 = data.cameras[i].calibration().px(); @@ -1186,7 +1186,7 @@ bool writeBAL(const string &filename, SfmData &data) { os << endl; // Write cameras - for (size_t i = 0; i < data.number_cameras(); i++) { // for each camera + for (size_t i = 0; i < data.nrCameras(); i++) { // for each camera Pose3 poseGTSAM = data.cameras[i].pose(); Cal3Bundler cameraCalibration = data.cameras[i].calibration(); Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); @@ -1201,7 +1201,7 @@ bool writeBAL(const string &filename, SfmData &data) { } // Write the points - for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j + for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j Point3 point = data.tracks[j].p; os << point.x() << endl; os << point.y() << endl; @@ -1221,8 +1221,8 @@ bool writeBALfromValues(const string &filename, const SfmData &data, // Store poses or cameras in SfmData size_t nrPoses = values.count(); if (nrPoses == - dataValues.number_cameras()) { // we only estimated camera poses - for (size_t i = 0; i < dataValues.number_cameras(); + dataValues.nrCameras()) { // we only estimated camera poses + for (size_t i = 0; i < dataValues.nrCameras(); i++) { // for each camera Pose3 pose = values.at(X(i)); Cal3Bundler K = dataValues.cameras[i].calibration(); @@ -1231,7 +1231,7 @@ bool writeBALfromValues(const string &filename, const SfmData &data, } } else { size_t nrCameras = values.count(); - if (nrCameras == dataValues.number_cameras()) { // we only estimated camera + if (nrCameras == dataValues.nrCameras()) { // we only estimated camera // poses and calibration for (size_t i = 0; i < nrCameras; i++) { // for each camera Key cameraKey = i; // symbol('c',i); @@ -1241,7 +1241,7 @@ bool writeBALfromValues(const string &filename, const SfmData &data, } else { cout << "writeBALfromValues: different number of cameras in " "SfM_dataValues (#cameras " - << dataValues.number_cameras() << ") and values (#cameras " + << dataValues.nrCameras() << ") and values (#cameras " << nrPoses << ", #poses " << nrCameras << ")!!" << endl; return false; } @@ -1249,7 +1249,7 @@ bool writeBALfromValues(const string &filename, const SfmData &data, // Store 3D points in SfmData size_t nrPoints = values.count(), - nrTracks = dataValues.number_tracks(); + nrTracks = dataValues.nrTracks(); if (nrPoints != nrTracks) { cout << "writeBALfromValues: different number of points in " "SfM_dataValues (#points= " diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 1611d78f6..d5c93c7f8 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -208,196 +209,6 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, /// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); -/// A measurement with its camera index -typedef std::pair SfmMeasurement; - -/// Sift index for SfmTrack -typedef std::pair SiftIndex; - -/// Define the structure for the 3D points -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; ///< 3D position of the point - float r, g, b; ///< RGB color of the 3D point - std::vector measurements; ///< The 2D image projections (id,(u,v)) - std::vector siftIndices; - - /// Get RGB values describing 3d point - const Point3 rgb() const { return Point3(r, g, b); } - - /// Total number of measurements in this track - size_t number_measurements() const { - return measurements.size(); - } - /// Get the measurement (camera index, Point2) at pose index `idx` - SfmMeasurement measurement(size_t idx) const { - return measurements[idx]; - } - /// Get the SIFT feature index corresponding to the measurement at `idx` - SiftIndex siftIndex(size_t idx) const { - return siftIndices[idx]; - } - /// Get 3D point - const Point3& point3() const { - return p; - } - /// Add measurement (camera_idx, Point2) to track - void add_measurement(size_t idx, const gtsam::Point2& m) { - measurements.emplace_back(idx, m); - } - - /** Serialization function */ - friend class boost::serialization::access; - template - 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); - } - - /// assert equality up to a tolerance - 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; - } - - /// print - void print(const std::string& s = "") const { - std::cout << "Track with " << measurements.size(); - std::cout << " measurements of point " << p << std::endl; - } -}; - -/* ************************************************************************* */ -/// traits -template<> -struct traits : public Testable { -}; - - -/// Define the structure for the camera poses -typedef PinholeCamera SfmCamera; - -/// Define the structure for SfM data -struct SfmData { - std::vector cameras; ///< Set of cameras - std::vector tracks; ///< Sparse set of points - size_t number_cameras() const { - return cameras.size(); - } - /// The number of reconstructed 3D points - size_t number_tracks() const { - return tracks.size(); - } - /// The camera pose at frame index `idx` - SfmCamera camera(size_t idx) const { - return cameras[idx]; - } - /// The track formed by series of landmark measurements - SfmTrack track(size_t idx) const { - return tracks[idx]; - } - /// Add a track to SfmData - void add_track(const SfmTrack& t) { - tracks.push_back(t); - } - /// Add a camera to SfmData - void add_camera(const SfmCamera& cam) { - cameras.push_back(cam); - } - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(cameras); - ar & BOOST_SERIALIZATION_NVP(tracks); - } - - /// @} - /// @name Testable - /// @{ - - /// assert equality up to a tolerance - 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; - } - - /// print - 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; - } -}; - -/* ************************************************************************* */ -/// traits -template<> -struct traits : public Testable { -}; - /** * @brief This function parses a bundler output file and stores the data into a * SfmData structure