From 5333396671a1499b7535b87c37c6d40a85cd1072 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 4 Jan 2021 22:59:48 +0530 Subject: [PATCH] Adding serialization support to be used for GT-SFM (#650) * adding serialization and other functions to enable testing * adding track serialization and testable trait * improving formatting * fixing variable names and comments * adding serialization functions to wrapper * fixing xml serialization issues * reverting SfmTrack to struct * printing out the 3d point * adding equals function to wrapper * adding inline comment for round trip --- gtsam/gtsam.i | 12 ++ gtsam/slam/dataset.h | 124 +++++++++++++++++- gtsam/slam/tests/testSerializationDataset.cpp | 58 ++++++++ 3 files changed, 191 insertions(+), 3 deletions(-) create mode 100644 gtsam/slam/tests/testSerializationDataset.cpp diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d3a4973ed..e1e11964f 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2768,6 +2768,12 @@ class SfmTrack { pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; void add_measurement(size_t idx, const gtsam::Point2& m); + + // enabling serialization functionality + void serialize() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmTrack& expected, double tol) const; }; class SfmData { @@ -2778,6 +2784,12 @@ class SfmData { gtsam::SfmTrack track(size_t idx) const; void add_track(const gtsam::SfmTrack& t) ; void add_camera(const gtsam::SfmCamera& cam); + + // enabling serialization functionality + void serialize() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmData& expected, double tol) const; }; gtsam::SfmData readBal(string filename); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 93bd2b2ee..d96c11167 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -29,6 +29,8 @@ #include #include #include +#include +#include #include #include @@ -222,7 +224,7 @@ struct SfmTrack { float r, g, b; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) std::vector siftIndices; - + /// Total number of measurements in this track size_t number_measurements() const { return measurements.size(); @@ -243,6 +245,73 @@ struct SfmTrack { 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 { + cout << "Track with " << measurements.size(); + cout << " measurements of point " << p << "\n"; + } +}; + +/* ************************************************************************* */ +/// traits +template<> +struct traits : public Testable { }; @@ -269,13 +338,62 @@ struct SfmData { return tracks[idx]; } /// Add a track to SfmData - void add_track(const SfmTrack& t) { + void add_track(const SfmTrack& t) { tracks.push_back(t); } /// Add a camera to SfmData - void add_camera(const SfmCamera& cam){ + 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 { + cout << "Number of cameras = " << number_cameras() << "\n"; + cout << "Number of tracks = " << number_tracks() << "\n"; + } +}; + +/* ************************************************************************* */ +/// traits +template<> +struct traits : public Testable { }; /** diff --git a/gtsam/slam/tests/testSerializationDataset.cpp b/gtsam/slam/tests/testSerializationDataset.cpp new file mode 100644 index 000000000..6ef82f07f --- /dev/null +++ b/gtsam/slam/tests/testSerializationDataset.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +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)); + + // round-trip equality check on serialization and subsequent deserialization + EXPECT(equalsObj(mydata)); + EXPECT(equalsXML(mydata)); + EXPECT(equalsBinary(mydata)); +} + +/* ************************************************************************* */ +TEST(dataSet, sfmTrackSerialization) { + // Test the serialization of SfmTrack + const string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfmData mydata; + CHECK(readBAL(filename, mydata)); + + SfmTrack track = mydata.track(0); + + // round-trip equality check on serialization and subsequent deserialization + EXPECT(equalsObj(track)); + EXPECT(equalsXML(track)); + EXPECT(equalsBinary(track)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */