From b5db391e776f1112cc404f4d86ce2be11e3fe76c Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Fri, 1 Jan 2021 21:09:42 +0530 Subject: [PATCH 01/14] adding serialization and other functions to enable testing --- gtsam/slam/dataset.h | 111 ++++++++++++++---- gtsam/slam/tests/testSerializationDataset.cpp | 44 +++++++ 2 files changed, 129 insertions(+), 26 deletions(-) create mode 100644 gtsam/slam/tests/testSerializationDataset.cpp 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 From df4419b6093bfcc9beb8c9073dc7d8fbef691ce1 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Sat, 2 Jan 2021 17:28:22 +0530 Subject: [PATCH 02/14] adding track serialization and testable trait --- gtsam/slam/dataset.h | 55 +++++++++++++++++-- gtsam/slam/tests/testSerializationDataset.cpp | 18 +++++- 2 files changed, 64 insertions(+), 9 deletions(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index a6b81eab1..89cd1f71b 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -29,8 +29,9 @@ #include #include #include -#include #include +#include + #include #include @@ -224,7 +225,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(); @@ -245,7 +246,7 @@ 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; @@ -258,14 +259,47 @@ struct SfmTrack { /// assert equality up to a tolerance bool equals(const SfmTrack &sfmTrack, double tol = 1e-9) const { + if(!p.isApprox(sfmTrack.p)){ + return false; + } + + // TODO: compare RGB values + + // compare size of vectors + if(number_measurements() != sfmTrack.number_measurements() || + siftIndices.size() != sfmTrack.siftIndices.size()){ + return false; + } + + // compare measurements (order sensitive) + for(size_t idx=0; idx +struct traits : public Testable { +}; + /// Define the structure for the camera poses typedef PinholeCamera SfmCamera; @@ -321,15 +355,18 @@ class GTSAM_EXPORT SfmData { } /// print - void print(const std::string& s = "") const; + void print(const std::string& s = "") const { + cout << "Number of cameras = " << number_cameras() << "\n"; + cout << "Number of tracks = " << number_tracks() << "\n"; + } private: /** Serialization function */ friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int /*version*/) { - // ar & cameras; - // ar & tracks; + ar & cameras; + ar & tracks; } // inline bool SfmData::operator == (const SfmData& rhs) const{ @@ -337,6 +374,12 @@ class GTSAM_EXPORT SfmData { // } }; +/* ************************************************************************* */ +/// traits +template<> +struct traits : public Testable { +}; + /** * @brief This function parses a bundler output file and stores the data into a * SfmData structure diff --git a/gtsam/slam/tests/testSerializationDataset.cpp b/gtsam/slam/tests/testSerializationDataset.cpp index 38749cb48..c6fed7b4f 100644 --- a/gtsam/slam/tests/testSerializationDataset.cpp +++ b/gtsam/slam/tests/testSerializationDataset.cpp @@ -18,8 +18,6 @@ #include -#include - #include #include @@ -39,6 +37,20 @@ TEST(dataSet, sfmDataSerialization){ // EXPECT(equalsBinary(mydata)); } +/* ************************************************************************* */ +TEST(dataSet, sfmTrackSerialization){ + // Test the serialization of SfmData + const string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfmData mydata; + CHECK(readBAL(filename, mydata)); + + SfmTrack track = mydata.track(0); + + EXPECT(equalsObj(track)); + // EXPECT(equalsXML(mydata)); + // EXPECT(equalsBinary(mydata)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ \ No newline at end of file +/* ************************************************************************* */ From 9d61e5bc4a6ed82cefb07b9791072b70d9596f52 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Sat, 2 Jan 2021 20:40:29 +0530 Subject: [PATCH 03/14] improving formatting --- gtsam/slam/dataset.h | 154 ++++++++++++++++++++++++------------------- 1 file changed, 85 insertions(+), 69 deletions(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 89cd1f71b..8e275e766 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -247,6 +247,8 @@ struct SfmTrack { measurements.emplace_back(idx, m); } + /** Serialization function */ + friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & p; @@ -259,24 +261,40 @@ struct SfmTrack { /// assert equality up to a tolerance bool equals(const SfmTrack &sfmTrack, double tol = 1e-9) const { - if(!p.isApprox(sfmTrack.p)){ + // check the 3D point + if (!p.isApprox(sfmTrack.p)) { return false; } - // TODO: compare RGB values + // check the RGB values + if (r!=sfmTrack.r || g!=sfmTrack.g || b!=sfmTrack.b) { + return false; + } - // compare size of vectors - if(number_measurements() != sfmTrack.number_measurements() || - siftIndices.size() != sfmTrack.siftIndices.size()){ + // 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 : public Testable { typedef PinholeCamera SfmCamera; /// Define the structure for SfM data -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); +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 & cameras; + ar & 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; } - /// @} - /// @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()){ + // check each camera + for (size_t i = 0; i < number_cameras(); ++i) { + if (!camera(i).equals(sfmData.camera(i), tol)) { 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; - } + // 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"; - } + return true; + } - 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; - // } + /// print + void print(const std::string& s = "") const { + cout << "Number of cameras = " << number_cameras() << "\n"; + cout << "Number of tracks = " << number_tracks() << "\n"; + } }; /* ************************************************************************* */ From f88b4565d727470af708e0a16262a05818cca142 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Sat, 2 Jan 2021 21:21:43 +0530 Subject: [PATCH 04/14] fixing variable names and comments --- gtsam/slam/tests/testSerializationDataset.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/tests/testSerializationDataset.cpp b/gtsam/slam/tests/testSerializationDataset.cpp index c6fed7b4f..a9fd9a259 100644 --- a/gtsam/slam/tests/testSerializationDataset.cpp +++ b/gtsam/slam/tests/testSerializationDataset.cpp @@ -39,7 +39,7 @@ TEST(dataSet, sfmDataSerialization){ /* ************************************************************************* */ TEST(dataSet, sfmTrackSerialization){ - // Test the serialization of SfmData + // Test the serialization of SfmTrack const string filename = findExampleDataFile("dubrovnik-3-7-pre"); SfmData mydata; CHECK(readBAL(filename, mydata)); @@ -47,8 +47,8 @@ TEST(dataSet, sfmTrackSerialization){ SfmTrack track = mydata.track(0); EXPECT(equalsObj(track)); - // EXPECT(equalsXML(mydata)); - // EXPECT(equalsBinary(mydata)); + // EXPECT(equalsXML(track)); + // EXPECT(equalsBinary(track)); } /* ************************************************************************* */ From c46e3dbee61faa3c192812b4dac1a3f0b291a173 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Sat, 2 Jan 2021 21:25:41 +0530 Subject: [PATCH 05/14] adding serialization functions to wrapper --- gtsam/gtsam.i | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d3a4973ed..efa740e02 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2768,6 +2768,9 @@ 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; }; class SfmData { @@ -2778,6 +2781,9 @@ 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; }; gtsam::SfmData readBal(string filename); From e0cda60b9b2303f6bbb0789b6fdae57b8cb0d99b Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 4 Jan 2021 00:02:21 +0530 Subject: [PATCH 06/14] fixing xml serialization issues --- gtsam/slam/dataset.h | 160 +++++++++--------- gtsam/slam/tests/testSerializationDataset.cpp | 12 +- 2 files changed, 86 insertions(+), 86 deletions(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 8e275e766..0156b3008 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -29,11 +29,10 @@ #include #include #include +#include #include #include - -#include #include #include #include // for pair @@ -218,94 +217,95 @@ typedef std::pair SfmMeasurement; typedef std::pair SiftIndex; /// Define the structure for the 3D points -struct SfmTrack { - SfmTrack(): p(0,0,0) {} - SfmTrack(const gtsam::Point3& pt) : p(pt) {} - 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; +class GTSAM_EXPORT SfmTrack { + public: + SfmTrack(): p(0,0,0) {} + SfmTrack(const gtsam::Point3& pt) : p(pt) {} + 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; - /// 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 & 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 { - // check the 3D point - if (!p.isApprox(sfmTrack.p)) { - return false; + /// 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); } - // check the RGB values - if (r!=sfmTrack.r || g!=sfmTrack.g || b!=sfmTrack.b) { - return false; + /** 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); } - // 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)) { + /// 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; } - } - // 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) { + // 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; } - return true; - } - - /// print - void print(const std::string& s = "") const { - cout << "Track with " << measurements.size() << "measurements\n"; - } + /// print + void print(const std::string& s = "") const { + cout << "Track with " << measurements.size() << "measurements\n"; + } }; /* ************************************************************************* */ @@ -350,8 +350,8 @@ struct SfmData { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int /*version*/) { - ar & cameras; - ar & tracks; + ar & BOOST_SERIALIZATION_NVP(cameras); + ar & BOOST_SERIALIZATION_NVP(tracks); } /// @} diff --git a/gtsam/slam/tests/testSerializationDataset.cpp b/gtsam/slam/tests/testSerializationDataset.cpp index a9fd9a259..db23ce64d 100644 --- a/gtsam/slam/tests/testSerializationDataset.cpp +++ b/gtsam/slam/tests/testSerializationDataset.cpp @@ -26,19 +26,19 @@ using namespace gtsam; using namespace gtsam::serializationTestHelpers; /* ************************************************************************* */ -TEST(dataSet, sfmDataSerialization){ +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)); + EXPECT(equalsXML(mydata)); + EXPECT(equalsBinary(mydata)); } /* ************************************************************************* */ -TEST(dataSet, sfmTrackSerialization){ +TEST(dataSet, sfmTrackSerialization) { // Test the serialization of SfmTrack const string filename = findExampleDataFile("dubrovnik-3-7-pre"); SfmData mydata; @@ -47,8 +47,8 @@ TEST(dataSet, sfmTrackSerialization){ SfmTrack track = mydata.track(0); EXPECT(equalsObj(track)); - // EXPECT(equalsXML(track)); - // EXPECT(equalsBinary(track)); + EXPECT(equalsXML(track)); + EXPECT(equalsBinary(track)); } /* ************************************************************************* */ From 2e8692105a6bf7aad74509ba613a6d4f33c47537 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 4 Jan 2021 00:07:20 +0530 Subject: [PATCH 07/14] reverting SfmTrack to struct --- gtsam/slam/dataset.h | 157 +++++++++++++++++++++---------------------- 1 file changed, 78 insertions(+), 79 deletions(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 0156b3008..a0f54b6d3 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -217,95 +217,94 @@ typedef std::pair SfmMeasurement; typedef std::pair SiftIndex; /// Define the structure for the 3D points -class GTSAM_EXPORT SfmTrack { - public: - SfmTrack(): p(0,0,0) {} - SfmTrack(const gtsam::Point3& pt) : p(pt) {} - 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; +struct SfmTrack { + SfmTrack(): p(0,0,0) {} + SfmTrack(const gtsam::Point3& pt) : p(pt) {} + 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; - /// 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); + /// 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; } - /** 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); + // check the RGB values + if (r!=sfmTrack.r || g!=sfmTrack.g || b!=sfmTrack.b) { + return false; } - /// 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)) { + // 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; } - - // 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() << "measurements\n"; + // 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() << "measurements\n"; + } }; /* ************************************************************************* */ From 0ce1db2d89781efb633903ec25568aacef4f5c13 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 4 Jan 2021 00:18:04 +0530 Subject: [PATCH 08/14] printing out the 3d point --- gtsam/slam/dataset.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index a0f54b6d3..d96c11167 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -303,7 +303,8 @@ struct SfmTrack { /// print void print(const std::string& s = "") const { - cout << "Track with " << measurements.size() << "measurements\n"; + cout << "Track with " << measurements.size(); + cout << " measurements of point " << p << "\n"; } }; From 915b4398beaf328f90bb1967879781e723c5eafe Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 4 Jan 2021 22:03:41 +0530 Subject: [PATCH 09/14] adding equals function to wrapper --- gtsam/gtsam.i | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index efa740e02..e1e11964f 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2771,6 +2771,9 @@ class SfmTrack { // enabling serialization functionality void serialize() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmTrack& expected, double tol) const; }; class SfmData { @@ -2784,6 +2787,9 @@ class SfmData { // 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); From 1d417546eca4c5103938b8a919350b56d6e9e819 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 4 Jan 2021 22:09:55 +0530 Subject: [PATCH 10/14] adding inline comment for round trip --- gtsam/slam/tests/testSerializationDataset.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/slam/tests/testSerializationDataset.cpp b/gtsam/slam/tests/testSerializationDataset.cpp index db23ce64d..6ef82f07f 100644 --- a/gtsam/slam/tests/testSerializationDataset.cpp +++ b/gtsam/slam/tests/testSerializationDataset.cpp @@ -32,6 +32,7 @@ TEST(dataSet, sfmDataSerialization) { SfmData mydata; CHECK(readBAL(filename, mydata)); + // round-trip equality check on serialization and subsequent deserialization EXPECT(equalsObj(mydata)); EXPECT(equalsXML(mydata)); EXPECT(equalsBinary(mydata)); @@ -46,6 +47,7 @@ TEST(dataSet, sfmTrackSerialization) { SfmTrack track = mydata.track(0); + // round-trip equality check on serialization and subsequent deserialization EXPECT(equalsObj(track)); EXPECT(equalsXML(track)); EXPECT(equalsBinary(track)); From 7ae050cc87a77e062920a51e401478bb5653a1c0 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Wed, 6 Jan 2021 12:08:22 +0530 Subject: [PATCH 11/14] adding default color values to fix equality check --- gtsam/slam/dataset.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index d96c11167..8ceacf31c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -221,7 +221,7 @@ struct SfmTrack { SfmTrack(): p(0,0,0) {} SfmTrack(const gtsam::Point3& pt) : p(pt) {} Point3 p; ///< 3D position of the point - float r, g, b; ///< RGB color of the 3D point + float r = 0, g = 0, b = 0; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) std::vector siftIndices; From ecb22263458d67de763d9362ffa8265525715b7b Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 6 Jan 2021 12:55:20 -0500 Subject: [PATCH 12/14] make r,g,b part of constructor w/ default values --- gtsam/slam/dataset.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index e4162fa75..00e3516d0 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -218,10 +218,11 @@ typedef std::pair SiftIndex; /// Define the structure for the 3D points struct SfmTrack { - SfmTrack(): p(0,0,0) {} - SfmTrack(const gtsam::Point3& pt) : p(pt) {} + 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 = 0, g = 0, b = 0; ///< RGB color of the 3D point + float r, g, b; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) std::vector siftIndices; From 758ece7238db0e29c7b249c23a3dd4dcff7140eb Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 6 Jan 2021 15:48:49 -0500 Subject: [PATCH 13/14] add getter for rgb --- gtsam/slam/dataset.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 00e3516d0..5730487ce 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -225,6 +225,9 @@ 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; + + /// Get RGB values describing 3d point + Point3 rgb() const { return Point3(r, g, b); } /// Total number of measurements in this track size_t number_measurements() const { From 7daf01be3db9ff57afc69c2e49ec4b5d15cc5da3 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 6 Jan 2021 15:50:08 -0500 Subject: [PATCH 14/14] make getter const --- gtsam/slam/dataset.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 5730487ce..ec5d6dce9 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -227,7 +227,7 @@ struct SfmTrack { std::vector siftIndices; /// Get RGB values describing 3d point - Point3 rgb() const { return Point3(r, g, b); } + const Point3 rgb() const { return Point3(r, g, b); } /// Total number of measurements in this track size_t number_measurements() const {