Moved Sfm classes to their own files
							parent
							
								
									cd94e7dda2
								
							
						
					
					
						commit
						0fd49efaba
					
				|  | @ -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 <gtsam/sfm/SfmData.h> | ||||
| 
 | ||||
| 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
 | ||||
|  | @ -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 <gtsam/geometry/Cal3Bundler.h> | ||||
| #include <gtsam/geometry/PinholeCamera.h> | ||||
| #include <gtsam/sfm/SfmTrack.h> | ||||
| 
 | ||||
| #include <string> | ||||
| #include <vector> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /// Define the structure for the camera poses
 | ||||
| typedef PinholeCamera<Cal3Bundler> SfmCamera; | ||||
| 
 | ||||
| /**
 | ||||
|  * @brief SfmData stores a bunch of SfmTracks | ||||
|  * @addtogroup sfm | ||||
|  */ | ||||
| struct SfmData { | ||||
|   std::vector<SfmCamera> cameras;  ///< Set of cameras
 | ||||
| 
 | ||||
|   std::vector<SfmTrack> 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 <class Archive> | ||||
|   void serialize(Archive& ar, const unsigned int /*version*/) { | ||||
|     ar& BOOST_SERIALIZATION_NVP(cameras); | ||||
|     ar& BOOST_SERIALIZATION_NVP(tracks); | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
| }; | ||||
| 
 | ||||
| /// traits
 | ||||
| template <> | ||||
| struct traits<SfmData> : public Testable<SfmData> {}; | ||||
| 
 | ||||
| }  // namespace gtsam
 | ||||
|  | @ -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 <gtsam/sfm/SfmTrack.h> | ||||
| 
 | ||||
| #include <iostream> | ||||
| 
 | ||||
| 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
 | ||||
|  | @ -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 <gtsam/base/serialization.h> | ||||
| #include <gtsam/geometry/Point2.h> | ||||
| #include <gtsam/geometry/Point3.h> | ||||
| 
 | ||||
| #include <string> | ||||
| #include <utility> | ||||
| #include <vector> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /// A measurement with its camera index
 | ||||
| typedef std::pair<size_t, Point2> SfmMeasurement; | ||||
| 
 | ||||
| /// Sift index for SfmTrack
 | ||||
| typedef std::pair<size_t, size_t> 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<SfmMeasurement> measurements; | ||||
| 
 | ||||
|   /// The feature descriptors
 | ||||
|   std::vector<SiftIndex> 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 <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
 | ||||
|  | @ -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<Pose3>(); | ||||
|   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<Pose3>(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<Camera>(); | ||||
|     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<Point3>(), | ||||
|          nrTracks = dataValues.number_tracks(); | ||||
|          nrTracks = dataValues.nrTracks(); | ||||
|   if (nrPoints != nrTracks) { | ||||
|     cout << "writeBALfromValues: different number of points in " | ||||
|             "SfM_dataValues (#points= " | ||||
|  |  | |||
|  | @ -22,6 +22,7 @@ | |||
| 
 | ||||
| #include <gtsam/sfm/BinaryMeasurement.h> | ||||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| #include <gtsam/sfm/SfmData.h> | ||||
| #include <gtsam/geometry/Cal3Bundler.h> | ||||
| #include <gtsam/geometry/PinholeCamera.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
|  | @ -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<size_t, Point2> SfmMeasurement; | ||||
| 
 | ||||
| /// Sift index for SfmTrack
 | ||||
| typedef std::pair<size_t, size_t> 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<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
 | ||||
|   std::vector<SiftIndex> 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<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); | ||||
|   } | ||||
| 
 | ||||
|   /// 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<SfmTrack> : public Testable<SfmTrack> { | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
| /// Define the structure for the camera poses
 | ||||
| typedef PinholeCamera<Cal3Bundler> SfmCamera; | ||||
| 
 | ||||
| /// Define the structure for SfM data
 | ||||
| struct SfmData { | ||||
|   std::vector<SfmCamera> cameras; ///< Set of cameras
 | ||||
|   std::vector<SfmTrack> 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<class Archive> | ||||
|   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<SfmData> : public Testable<SfmData> { | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * @brief This function parses a bundler output file and stores the data into a | ||||
|  * SfmData structure | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue