diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 880b1d4c7..73ce4f203 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2759,21 +2759,34 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include +class SfmMeasurement{}; +class SiftIndex{ }; +class SfmMeasurements{}; + class SfmTrack { + SfmTrack(); Point3 point3() const; size_t number_measurements() const; - pair measurement(size_t idx) const; - pair siftIndex(size_t idx) const; + gtsam::SfmMeasurement measurement(size_t idx) const; + gtsam::SiftIndex siftIndex(size_t idx) const; + // gtsam::Measurements add_measurement(pair m); + void add_measurement(pair m); + SfmMeasurements& measurements(); }; class SfmData { + SfmData(); size_t number_cameras() const; size_t number_tracks() const; gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; + // std::vector add_track(gtsam::SfmTrack t); + void add_track(gtsam::SfmTrack t); + void delete_track(size_t idx); }; gtsam::SfmData readBal(string filename); +bool writeBAL(string filename, gtsam::SfmData& data); gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 270dbeb95..9b610b231 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1052,13 +1052,13 @@ bool readBundler(const string &filename, SfmData &data) { size_t nvisible = 0; is >> nvisible; - track.measurements.reserve(nvisible); + track.Measurements.reserve(nvisible); track.siftIndices.reserve(nvisible); for (size_t k = 0; k < nvisible; k++) { size_t cam_idx = 0, point_idx = 0; float u, v; is >> cam_idx >> point_idx >> u >> v; - track.measurements.emplace_back(cam_idx, Point2(u, -v)); + track.Measurements.emplace_back(cam_idx, Point2(u, -v)); track.siftIndices.emplace_back(cam_idx, point_idx); } @@ -1089,7 +1089,7 @@ bool readBAL(const string &filename, SfmData &data) { size_t i = 0, j = 0; float u, v; is >> i >> j >> u >> v; - data.tracks[j].measurements.emplace_back(i, Point2(u, -v)); + data.tracks[j].Measurements.emplace_back(i, Point2(u, -v)); } // Get the information for the camera poses @@ -1163,7 +1163,7 @@ bool writeBAL(const string &filename, SfmData &data) { for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j - size_t i = track.measurements[k].first; // camera id + size_t i = track.Measurements[k].first; // camera id double u0 = data.cameras[i].calibration().u0(); double v0 = data.cameras[i].calibration().v0(); @@ -1173,9 +1173,9 @@ bool writeBAL(const string &filename, SfmData &data) { << endl; } - double pixelBALx = track.measurements[k].second.x() - + double pixelBALx = track.Measurements[k].second.x() - u0; // center of image is the origin - double pixelBALy = -(track.measurements[k].second.y() - + double pixelBALy = -(track.Measurements[k].second.y() - v0); // center of image is the origin Point2 pixelMeasurement(pixelBALx, pixelBALy); os << i /*camera id*/ << " " << j /*point id*/ << " " diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index a8fddcfe4..5a206929c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -210,6 +210,7 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index typedef std::pair SfmMeasurement; +typedef std::vector SfmMeasurements; /// SfmTrack typedef std::pair SiftIndex; @@ -219,15 +220,16 @@ struct SfmTrack { SfmTrack(): p(0,0,0) {} 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 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(); + return Measurements.size(); } /// Get the measurement (camera index, Point2) at pose index `idx` SfmMeasurement measurement(size_t idx) const { - return measurements[idx]; + return Measurements[idx]; } /// Get the SIFT feature index corresponding to the measurement at `idx` SiftIndex siftIndex(size_t idx) const { @@ -236,13 +238,27 @@ struct SfmTrack { Point3 point3() const { return p; } + void add_measurement(pair m) { + Measurements.push_back(m); + } + + SfmMeasurements& measurements() { + return Measurements; + } + + void clear() { + Measurements.clear(); + } + }; + /// Define the structure for the camera poses typedef PinholeCamera SfmCamera; /// Define the structure for SfM data struct SfmData { + std::vector Measurements; std::vector cameras; ///< Set of cameras std::vector tracks; ///< Sparse set of points size_t number_cameras() const { @@ -260,6 +276,14 @@ struct SfmData { SfmTrack track(size_t idx) const { return tracks[idx]; } + + void add_track(SfmTrack t) { + tracks.push_back(t); + } + /// Delete track at `idx` + void delete_track(size_t idx){ + tracks[idx].clear(); + } }; /** diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 00b537340..322968694 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -36,6 +36,8 @@ set(ignore gtsam::BetweenFactorPose3s gtsam::Point2Vector gtsam::Pose3Vector + gtsam::SfmMeasurement + gtsam::SiftIndex gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 gtsam::KeyPairDoubleMap) diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 6166f615e..8525ed3a3 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -10,3 +10,5 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index cacad874c..03e2b606c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -13,3 +13,5 @@ py::bind_vector > >(m_, "Bina py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); +py::bind_vector >(m_, "Measurement"); +py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file