diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 93bd2b2ee..a6b81eab1 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -30,7 +30,9 @@ #include #include #include +#include +#include #include #include #include // for pair @@ -243,6 +245,25 @@ struct SfmTrack { void add_measurement(size_t idx, const gtsam::Point2& m) { measurements.emplace_back(idx, m); } + + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & p; + ar & r; + ar & g; + ar & b; + ar & measurements; + ar & siftIndices; + } + + /// assert equality up to a tolerance + bool equals(const SfmTrack &sfmTrack, double tol = 1e-9) const { + return true; + } + + // inline bool SfmTrack::operator == (const SfmTrack& rhs) const{ + // return p==rhs.p; + // } }; @@ -250,32 +271,70 @@ struct SfmTrack { 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); - } +class GTSAM_EXPORT SfmData { + public: + 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); + } + + /// @} + /// @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 cam_idx = 0; cam_idx < number_cameras(); cam_idx++){ + if(!camera(cam_idx).equals(sfmData.camera(cam_idx), tol)){ + return false; + } + } + + return true; + } + + /// print + void print(const std::string& s = "") const; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + // ar & cameras; + // ar & tracks; + } + + // inline bool SfmData::operator == (const SfmData& rhs) const{ + // return cameras==rhs.cameras && tracks==rhs.tracks; + // } }; /** diff --git a/gtsam/slam/tests/testSerializationDataset.cpp b/gtsam/slam/tests/testSerializationDataset.cpp new file mode 100644 index 000000000..38749cb48 --- /dev/null +++ b/gtsam/slam/tests/testSerializationDataset.cpp @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSerializationDataset.cpp + * @brief serialization tests for dataset.cpp + * @author Ayush Baid + * @date Jan 1, 2021 + */ + +#include + +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +TEST(dataSet, sfmDataSerialization){ + // Test the serialization of SfmData + const string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfmData mydata; + CHECK(readBAL(filename, mydata)); + + EXPECT(equalsObj(mydata)); + // EXPECT(equalsXML(mydata)); + // EXPECT(equalsBinary(mydata)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ \ No newline at end of file