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)); } /* ************************************************************************* */