From bda6222da40e537772657958172cb01da11704b6 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 11 Oct 2020 16:46:10 -0400 Subject: [PATCH 01/11] python wrapper for sfmdata --- gtsam/gtsam.i | 17 +++++++++++++++-- gtsam/slam/dataset.cpp | 12 ++++++------ gtsam/slam/dataset.h | 30 +++++++++++++++++++++++++++--- python/CMakeLists.txt | 2 ++ python/gtsam/preamble.h | 2 ++ python/gtsam/specializations.h | 2 ++ 6 files changed, 54 insertions(+), 11 deletions(-) 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 From ed387e38173d837cf8c421eab649483b5e560231 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 18 Oct 2020 11:17:10 -0400 Subject: [PATCH 02/11] unittested features in SfmData --- gtsam/gtsam.i | 7 +-- gtsam/slam/dataset.h | 15 +++--- python/CMakeLists.txt | 1 + python/gtsam/preamble.h | 3 +- python/gtsam/specializations.h | 3 +- python/gtsam/tests/test_SfmData.py | 79 ++++++++++++++++++++++++++++++ 6 files changed, 96 insertions(+), 12 deletions(-) create mode 100644 python/gtsam/tests/test_SfmData.py diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 73ce4f203..b3792fbec 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2759,17 +2759,19 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include +// Dummy classes, for MATLAB wrappers class SfmMeasurement{}; class SiftIndex{ }; class SfmMeasurements{}; +class SfmCamera{}; class SfmTrack { SfmTrack(); Point3 point3() const; size_t number_measurements() const; + void setP(gtsam::Point3& p_); 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(); }; @@ -2780,9 +2782,8 @@ class SfmData { 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); + void add_camera(gtsam::SfmCamera cam); }; gtsam::SfmData readBal(string filename); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 5a206929c..2e4c715be 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -227,6 +227,11 @@ struct SfmTrack { size_t number_measurements() const { return Measurements.size(); } + /// Set 3D point + void setP(Point3& p_){ + p = p_; + } + /// Get the measurement (camera index, Point2) at pose index `idx` SfmMeasurement measurement(size_t idx) const { return Measurements[idx]; @@ -246,10 +251,6 @@ struct SfmTrack { return Measurements; } - void clear() { - Measurements.clear(); - } - }; @@ -280,9 +281,9 @@ struct SfmData { void add_track(SfmTrack t) { tracks.push_back(t); } - /// Delete track at `idx` - void delete_track(size_t idx){ - tracks[idx].clear(); + + void add_camera(SfmCamera cam){ + cameras.push_back(cam); } }; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 322968694..7d12b9800 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -37,6 +37,7 @@ set(ignore gtsam::Point2Vector gtsam::Pose3Vector gtsam::SfmMeasurement + gtsam::SfmCamera gtsam::SiftIndex gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 8525ed3a3..e6ed64689 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -11,4 +11,5 @@ 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 +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 03e2b606c..dcaaa4c76 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -14,4 +14,5 @@ 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 +py::bind_vector >(m_, "SiftIndexVector"); +py::bind_vector >(m_, "cameras"); \ No newline at end of file diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py new file mode 100644 index 000000000..4664d746e --- /dev/null +++ b/python/gtsam/tests/test_SfmData.py @@ -0,0 +1,79 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for testing dataset access. +Author: Frank Dellaert (Python: Sushmita Warrier) +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import numpy as np + +import gtsam +#from gtsam import SfmCamera +from gtsam.utils.test_case import GtsamTestCase + + +class TestSfmData(GtsamTestCase): + """Tests for SfmData and SfmTrack modules.""" + + def setUp(self): + """Initialize SfmData and SfmTrack""" + self.data = gtsam.SfmData() + self.tracks = gtsam.SfmTrack() + + def test_tracks(self): + """Test functions in SfmTrack""" + # measurement is of format (camera_idx, imgPoint) + # create camera indices for two cameras + i1, i2 = np.random.randint(5), np.random.randint(5) + # create imgPoint for cameras i1 and i2 + uv_i1 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + uv_i2 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + m_i1 = (i1, uv_i1) + m_i2 = (i2, uv_i2) + # add measurements to the track + self.tracks.add_measurement(m_i1) + self.tracks.add_measurement(m_i2) + # Number of measurements in the track is 2 + self.assertEqual(self.tracks.number_measurements(), 2) + # camera_idx in the first measurement of the track corresponds to i1 + self.assertEqual(self.tracks.measurement(0)[0], i1) + # Set arbitrary 3D point corresponding to the track + self.tracks.setP(gtsam.Point3(2.5, 3.3, 1.2)) + np.testing.assert_array_almost_equal( + gtsam.Point3(2.5,3.3,1.2), + self.tracks.point3() + ) + + + def test_data(self): + """Test functions in SfmData""" + #cam1 = gtsam.SfmCamera(1500, 1200, 0, 640, 480) + # Create new track with 3 measurements + track2 = gtsam.SfmTrack() + i1, i2, i3 = np.random.randint(5), np.random.randint(5), np.random.randint(5) + uv_i1 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + uv_i2 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + uv_i3 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + m_i1, m_i2, m_i3 = (i1, uv_i1), (i2, uv_i2), (i3, uv_i3) + # add measurements to the track + track2.add_measurement(m_i1) + track2.add_measurement(m_i2) + track2.add_measurement(m_i3) + self.data.add_track(self.tracks) + self.data.add_track(track2) + # Number of tracks in SfmData is 2 + self.assertEqual(self.data.number_tracks(), 2) + # camera idx of first measurement of second track corresponds to i1 + self.assertEqual(self.data.track(1).measurement(0)[0], i1) + +if __name__ == '__main__': + unittest.main() From 1f09f4aab680f49da367d2ceb3639356bd20746f Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 18 Oct 2020 19:05:09 -0400 Subject: [PATCH 03/11] python wrapped SfmData and SfmTrack --- gtsam/slam/SmartFactorBase.h | 4 ++-- gtsam/slam/tests/testDataset.cpp | 16 ++++++++-------- gtsam/slam/tests/testEssentialMatrixFactor.cpp | 8 ++++---- gtsam/slam/tests/testSmartProjectionFactor.cpp | 4 ++-- tests/testGeneralSFMFactorB.cpp | 2 +- 5 files changed, 17 insertions(+), 17 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index d9f2b9c3d..5afc159bd 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -144,8 +144,8 @@ protected: template void add(const SFM_TRACK& trackToAdd) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { - this->measured_.push_back(trackToAdd.measurements[k].second); - this->keys_.push_back(trackToAdd.measurements[k].first); + this->measured_.push_back(trackToAdd.Measurements[k].second); + this->keys_.push_back(trackToAdd.Measurements[k].first); } } diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index aad9124c5..ff5cf8b46 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -166,9 +166,9 @@ TEST( dataSet, Balbianello) EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; EXPECT(assert_equal(expected,actual,1)); } @@ -476,9 +476,9 @@ TEST( dataSet, readBAL_Dubrovnik) EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; EXPECT(assert_equal(expected,actual,12)); } @@ -557,8 +557,8 @@ TEST( dataSet, writeBAL_Dubrovnik) // check measurements for (size_t k = 0; k < actualTrack.number_measurements(); k++){ - EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first); - EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second)); + EXPECT_LONGS_EQUAL(expectedTrack.Measurements[k].first,actualTrack.Measurements[k].first); + EXPECT(assert_equal(expectedTrack.Measurements[k].second,actualTrack.Measurements[k].second)); } } } @@ -600,9 +600,9 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); const SfmCamera& camera0 = writtenData.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; EXPECT(assert_equal(expected,actual,12)); Pose3 expectedPose = camera0.pose(); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 95db611d7..6bece036f 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -46,10 +46,10 @@ EssentialMatrix trueE(trueRotation, trueDirection); double baseline = 0.1; // actual baseline of the camera Point2 pA(size_t i) { - return data.tracks[i].measurements[0].second; + return data.tracks[i].Measurements[0].second; } Point2 pB(size_t i) { - return data.tracks[i].measurements[1].second; + return data.tracks[i].Measurements[1].second; } Vector vA(size_t i) { return EssentialMatrix::Homogeneous(pA(i)); @@ -367,10 +367,10 @@ EssentialMatrix trueE(aRb, Unit3(aTb)); double baseline = 10; // actual baseline of the camera Point2 pA(size_t i) { - return data.tracks[i].measurements[0].second; + return data.tracks[i].Measurements[0].second; } Point2 pB(size_t i) { - return data.tracks[i].measurements[1].second; + return data.tracks[i].Measurements[1].second; } boost::shared_ptr // diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 1fd06cc9f..1f3a5d1cb 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -283,14 +283,14 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) { SfmTrack track1; for (size_t i = 0; i < 3; ++i) { - track1.measurements.emplace_back(i + 1, measurements_cam1.at(i)); + track1.Measurements.emplace_back(i + 1, measurements_cam1.at(i)); } SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); smartFactor1->add(track1); SfmTrack track2; for (size_t i = 0; i < 3; ++i) { - track2.measurements.emplace_back(i + 1, measurements_cam2.at(i)); + track2.Measurements.emplace_back(i + 1, measurements_cam2.at(i)); } SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); smartFactor2->add(track2); diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index 05b4c7f66..fafe87d13 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -50,7 +50,7 @@ TEST(PinholeCamera, BAL) { NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { - for (const SfmMeasurement& m: db.tracks[j].measurements) + for (const SfmMeasurement& m: db.tracks[j].Measurements) graph.emplace_shared(m.second, unit2, m.first, P(j)); } From b0cca7eeddbc280aa1c4eaa4c8588e44905159f1 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 18 Oct 2020 19:05:42 -0400 Subject: [PATCH 04/11] python wrapper sfmtrack reflected in other files --- examples/CreateSFMExampleData.cpp | 2 +- examples/SFMExampleExpressions_bal.cpp | 2 +- examples/SFMExample_bal.cpp | 2 +- examples/SFMExample_bal_COLAMD_METIS.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index aa0bde8f6..602d396d9 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -51,7 +51,7 @@ void createExampleBALFile(const string& filename, const vector& P, // Project points in both cameras for (size_t i = 0; i < 2; i++) - track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); + track.Measurements.push_back(make_pair(i, data.cameras[i].project(p))); // Add track to data data.tracks.push_back(track); diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index 3768ee2a3..fd9ef5648 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -77,7 +77,7 @@ int main(int argc, char* argv[]) { for (const SfmTrack& track : mydata.tracks) { // Leaf expression for j^th point Point3_ point_('p', j); - for (const SfmMeasurement& m : track.measurements) { + for (const SfmMeasurement& m : track.Measurements) { size_t i = m.first; Point2 uv = m.second; // Leaf expression for i^th camera diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index ffb5b195b..4572d8424 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -55,7 +55,7 @@ int main (int argc, char* argv[]) { // Add measurements to the factor graph size_t j = 0; for(const SfmTrack& track: mydata.tracks) { - for(const SfmMeasurement& m: track.measurements) { + for(const SfmMeasurement& m: track.Measurements) { size_t i = m.first; Point2 uv = m.second; graph.emplace_shared(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index b79a9fa28..d378d89c3 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -59,7 +59,7 @@ int main(int argc, char* argv[]) { // Add measurements to the factor graph size_t j = 0; for (const SfmTrack& track : mydata.tracks) { - for (const SfmMeasurement& m : track.measurements) { + for (const SfmMeasurement& m : track.Measurements) { size_t i = m.first; Point2 uv = m.second; graph.emplace_shared( From 045780a151513a1723efce05fc95cdec28e14673 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Wed, 21 Oct 2020 23:43:17 -0400 Subject: [PATCH 05/11] changed Measurements to measurements --- examples/CreateSFMExampleData.cpp | 2 +- examples/SFMExampleExpressions_bal.cpp | 2 +- examples/SFMExample_bal.cpp | 2 +- examples/SFMExample_bal_COLAMD_METIS.cpp | 2 +- gtsam/gtsam.i | 25 +++++++++++++------ gtsam/slam/SmartFactorBase.h | 4 +-- gtsam/slam/dataset.cpp | 12 ++++----- gtsam/slam/dataset.h | 18 ++++++------- gtsam/slam/tests/testDataset.cpp | 16 ++++++------ .../slam/tests/testEssentialMatrixFactor.cpp | 8 +++--- .../slam/tests/testSmartProjectionFactor.cpp | 4 +-- python/gtsam/preamble.h | 3 +-- python/gtsam/specializations.h | 3 +-- tests/testGeneralSFMFactorB.cpp | 2 +- 14 files changed, 55 insertions(+), 48 deletions(-) diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index 602d396d9..aa0bde8f6 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -51,7 +51,7 @@ void createExampleBALFile(const string& filename, const vector& P, // Project points in both cameras for (size_t i = 0; i < 2; i++) - track.Measurements.push_back(make_pair(i, data.cameras[i].project(p))); + track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); // Add track to data data.tracks.push_back(track); diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index fd9ef5648..3768ee2a3 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -77,7 +77,7 @@ int main(int argc, char* argv[]) { for (const SfmTrack& track : mydata.tracks) { // Leaf expression for j^th point Point3_ point_('p', j); - for (const SfmMeasurement& m : track.Measurements) { + for (const SfmMeasurement& m : track.measurements) { size_t i = m.first; Point2 uv = m.second; // Leaf expression for i^th camera diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 4572d8424..ffb5b195b 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -55,7 +55,7 @@ int main (int argc, char* argv[]) { // Add measurements to the factor graph size_t j = 0; for(const SfmTrack& track: mydata.tracks) { - for(const SfmMeasurement& m: track.Measurements) { + for(const SfmMeasurement& m: track.measurements) { size_t i = m.first; Point2 uv = m.second; graph.emplace_shared(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index d378d89c3..b79a9fa28 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -59,7 +59,7 @@ int main(int argc, char* argv[]) { // Add measurements to the factor graph size_t j = 0; for (const SfmTrack& track : mydata.tracks) { - for (const SfmMeasurement& m : track.Measurements) { + for (const SfmMeasurement& m : track.measurements) { size_t i = m.first; Point2 uv = m.second; graph.emplace_shared( diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index b3792fbec..5334a233d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2759,11 +2759,20 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include -// Dummy classes, for MATLAB wrappers -class SfmMeasurement{}; -class SiftIndex{ }; -class SfmMeasurements{}; -class SfmCamera{}; +class SfmMeasurement{ + SfmMeasurement(); + size_t i() const; + Point2 j() const; +}; +class SiftIndex{ + SiftIndex(); + size_t i() const; + size_t j() const; + }; +class SfmMeasurements{ + SfmMeasurements(); + +}; class SfmTrack { SfmTrack(); @@ -2772,7 +2781,7 @@ class SfmTrack { void setP(gtsam::Point3& p_); gtsam::SfmMeasurement measurement(size_t idx) const; gtsam::SiftIndex siftIndex(size_t idx) const; - void add_measurement(pair m); + void add_measurement(const pair& m); SfmMeasurements& measurements(); }; @@ -2782,8 +2791,8 @@ class SfmData { size_t number_tracks() const; gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; - void add_track(gtsam::SfmTrack t); - void add_camera(gtsam::SfmCamera cam); + void add_track(const gtsam::SfmTrack& t) ; + void add_camera(const gtsam::SfmCamer& cam); }; gtsam::SfmData readBal(string filename); diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 5afc159bd..d9f2b9c3d 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -144,8 +144,8 @@ protected: template void add(const SFM_TRACK& trackToAdd) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { - this->measured_.push_back(trackToAdd.Measurements[k].second); - this->keys_.push_back(trackToAdd.Measurements[k].first); + this->measured_.push_back(trackToAdd.measurements[k].second); + this->keys_.push_back(trackToAdd.measurements[k].first); } } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 9b610b231..270dbeb95 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 2e4c715be..062d8728c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -220,12 +220,12 @@ 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(); } /// Set 3D point void setP(Point3& p_){ @@ -234,7 +234,7 @@ struct SfmTrack { /// 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 { @@ -243,12 +243,12 @@ struct SfmTrack { Point3 point3() const { return p; } - void add_measurement(pair m) { - Measurements.push_back(m); + void add_measurement(pair& m) const{ + measurements.push_back(m); } SfmMeasurements& measurements() { - return Measurements; + return measurements; } }; @@ -259,7 +259,7 @@ typedef PinholeCamera SfmCamera; /// Define the structure for SfM data struct SfmData { - std::vector Measurements; + std::vector measurements; /// cameras; ///< Set of cameras std::vector tracks; ///< Sparse set of points size_t number_cameras() const { @@ -278,11 +278,11 @@ struct SfmData { return tracks[idx]; } - void add_track(SfmTrack t) { + void add_track(SfmTrack& t) const { tracks.push_back(t); } - void add_camera(SfmCamera cam){ + void add_camera(SfmCamera& cam) const{ cameras.push_back(cam); } }; diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index ff5cf8b46..aad9124c5 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -166,9 +166,9 @@ TEST( dataSet, Balbianello) EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected,actual,1)); } @@ -476,9 +476,9 @@ TEST( dataSet, readBAL_Dubrovnik) EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected,actual,12)); } @@ -557,8 +557,8 @@ TEST( dataSet, writeBAL_Dubrovnik) // check measurements for (size_t k = 0; k < actualTrack.number_measurements(); k++){ - EXPECT_LONGS_EQUAL(expectedTrack.Measurements[k].first,actualTrack.Measurements[k].first); - EXPECT(assert_equal(expectedTrack.Measurements[k].second,actualTrack.Measurements[k].second)); + EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first); + EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second)); } } } @@ -600,9 +600,9 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); const SfmCamera& camera0 = writtenData.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected,actual,12)); Pose3 expectedPose = camera0.pose(); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 6bece036f..95db611d7 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -46,10 +46,10 @@ EssentialMatrix trueE(trueRotation, trueDirection); double baseline = 0.1; // actual baseline of the camera Point2 pA(size_t i) { - return data.tracks[i].Measurements[0].second; + return data.tracks[i].measurements[0].second; } Point2 pB(size_t i) { - return data.tracks[i].Measurements[1].second; + return data.tracks[i].measurements[1].second; } Vector vA(size_t i) { return EssentialMatrix::Homogeneous(pA(i)); @@ -367,10 +367,10 @@ EssentialMatrix trueE(aRb, Unit3(aTb)); double baseline = 10; // actual baseline of the camera Point2 pA(size_t i) { - return data.tracks[i].Measurements[0].second; + return data.tracks[i].measurements[0].second; } Point2 pB(size_t i) { - return data.tracks[i].Measurements[1].second; + return data.tracks[i].measurements[1].second; } boost::shared_ptr // diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 1f3a5d1cb..1fd06cc9f 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -283,14 +283,14 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) { SfmTrack track1; for (size_t i = 0; i < 3; ++i) { - track1.Measurements.emplace_back(i + 1, measurements_cam1.at(i)); + track1.measurements.emplace_back(i + 1, measurements_cam1.at(i)); } SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); smartFactor1->add(track1); SfmTrack track2; for (size_t i = 0; i < 3; ++i) { - track2.Measurements.emplace_back(i + 1, measurements_cam2.at(i)); + track2.measurements.emplace_back(i + 1, measurements_cam2.at(i)); } SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); smartFactor2->add(track2); diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index e6ed64689..8525ed3a3 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -11,5 +11,4 @@ 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 +PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index dcaaa4c76..03e2b606c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -14,5 +14,4 @@ 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"); -py::bind_vector >(m_, "cameras"); \ No newline at end of file +py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index fafe87d13..05b4c7f66 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -50,7 +50,7 @@ TEST(PinholeCamera, BAL) { NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { - for (const SfmMeasurement& m: db.tracks[j].Measurements) + for (const SfmMeasurement& m: db.tracks[j].measurements) graph.emplace_shared(m.second, unit2, m.first, P(j)); } From a68b0798f9f63b49fd57243804fd49f7d8272a31 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Wed, 21 Oct 2020 23:44:02 -0400 Subject: [PATCH 06/11] wrapped sfmtrack --- gtsam/gtsam.i | 21 +++------------------ gtsam/slam/dataset.h | 11 ++++------- python/gtsam/preamble.h | 4 ++-- python/gtsam/specializations.h | 4 ++-- 4 files changed, 11 insertions(+), 29 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 5334a233d..5307ef45d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2759,30 +2759,15 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include -class SfmMeasurement{ - SfmMeasurement(); - size_t i() const; - Point2 j() const; -}; -class SiftIndex{ - SiftIndex(); - size_t i() const; - size_t j() const; - }; -class SfmMeasurements{ - SfmMeasurements(); - -}; class SfmTrack { SfmTrack(); Point3 point3() const; size_t number_measurements() const; void setP(gtsam::Point3& p_); - gtsam::SfmMeasurement measurement(size_t idx) const; - gtsam::SiftIndex siftIndex(size_t idx) const; + pair measurement(size_t idx) const; + pair siftIndex(size_t idx) const; void add_measurement(const pair& m); - SfmMeasurements& measurements(); }; class SfmData { @@ -2792,7 +2777,7 @@ class SfmData { gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; void add_track(const gtsam::SfmTrack& t) ; - void add_camera(const gtsam::SfmCamer& cam); + void add_camera(const gtsam::SfmCamera& cam); }; gtsam::SfmData readBal(string filename); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 062d8728c..1a01d8a38 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -228,7 +228,7 @@ struct SfmTrack { return measurements.size(); } /// Set 3D point - void setP(Point3& p_){ + void setP(const Point3& p_){ p = p_; } @@ -243,13 +243,10 @@ struct SfmTrack { Point3 point3() const { return p; } - void add_measurement(pair& m) const{ + void add_measurement(const pair& m) { measurements.push_back(m); } - SfmMeasurements& measurements() { - return measurements; - } }; @@ -278,11 +275,11 @@ struct SfmData { return tracks[idx]; } - void add_track(SfmTrack& t) const { + void add_track(const SfmTrack& t) { tracks.push_back(t); } - void add_camera(SfmCamera& cam) const{ + void add_camera(const SfmCamera& cam){ cameras.push_back(cam); } }; diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 8525ed3a3..a8dfc5787 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -10,5 +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 +//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 03e2b606c..2fd353b5f 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -13,5 +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 +//py::bind_vector >(m_, "Measurement"); +//py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file From 6362b5648adebb3a480aaed4d317cf66835bce3f Mon Sep 17 00:00:00 2001 From: Sushmita Date: Thu, 22 Oct 2020 08:41:50 -0400 Subject: [PATCH 07/11] removed measurements from sfmdata --- gtsam/slam/dataset.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 1a01d8a38..c9c451660 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -256,7 +256,6 @@ typedef PinholeCamera SfmCamera; /// Define the structure for SfM data struct SfmData { - std::vector measurements; /// cameras; ///< Set of cameras std::vector tracks; ///< Sparse set of points size_t number_cameras() const { From 38010860e449f91634deab3bf8613498b9a3aca8 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sat, 24 Oct 2020 15:46:47 -0400 Subject: [PATCH 08/11] changed setP method name removed commented code --- gtsam/gtsam.i | 2 +- gtsam/slam/dataset.h | 14 ++++++-------- python/CMakeLists.txt | 3 --- python/gtsam/preamble.h | 4 +--- python/gtsam/specializations.h | 2 -- python/gtsam/tests/test_SfmData.py | 22 ++++++++++++---------- 6 files changed, 20 insertions(+), 27 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 5307ef45d..d6ae409f8 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2764,7 +2764,7 @@ class SfmTrack { SfmTrack(); Point3 point3() const; size_t number_measurements() const; - void setP(gtsam::Point3& p_); + void set_point3(gtsam::Point3& p_); pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; void add_measurement(const pair& m); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index c9c451660..85b32ff5a 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -210,9 +210,8 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index typedef std::pair SfmMeasurement; -typedef std::vector SfmMeasurements; -/// SfmTrack +/// Sift index for SfmTrack typedef std::pair SiftIndex; /// Define the structure for the 3D points @@ -228,10 +227,9 @@ struct SfmTrack { return measurements.size(); } /// Set 3D point - void setP(const Point3& p_){ + void set_point3(const Point3& p_){ p = p_; } - /// Get the measurement (camera index, Point2) at pose index `idx` SfmMeasurement measurement(size_t idx) const { return measurements[idx]; @@ -240,14 +238,14 @@ struct SfmTrack { SiftIndex siftIndex(size_t idx) const { return siftIndices[idx]; } + /// Get 3D point Point3 point3() const { return p; } + /// Add measurement to track void add_measurement(const pair& m) { measurements.push_back(m); } - - }; @@ -273,11 +271,11 @@ struct SfmData { 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); } diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 7d12b9800..00b537340 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -36,9 +36,6 @@ set(ignore gtsam::BetweenFactorPose3s gtsam::Point2Vector gtsam::Pose3Vector - gtsam::SfmMeasurement - gtsam::SfmCamera - gtsam::SiftIndex gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 gtsam::KeyPairDoubleMap) diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index a8dfc5787..e31b5e6d8 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -9,6 +9,4 @@ 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 +PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 2fd353b5f..cacad874c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -13,5 +13,3 @@ 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 diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index 4664d746e..6263f19a0 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -32,11 +32,12 @@ class TestSfmData(GtsamTestCase): def test_tracks(self): """Test functions in SfmTrack""" # measurement is of format (camera_idx, imgPoint) - # create camera indices for two cameras - i1, i2 = np.random.randint(5), np.random.randint(5) - # create imgPoint for cameras i1 and i2 - uv_i1 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) - uv_i2 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + # create arbitrary camera indices for two cameras + i1, i2 = 4,5 + # create arbitrary image measurements for cameras i1 and i2 + uv_i1 = gtsam.Point2(12.6, 82) + # translating point uv_i1 along X-axis + uv_i2 = gtsam.Point2(24.88, 82) m_i1 = (i1, uv_i1) m_i2 = (i2, uv_i2) # add measurements to the track @@ -47,7 +48,7 @@ class TestSfmData(GtsamTestCase): # camera_idx in the first measurement of the track corresponds to i1 self.assertEqual(self.tracks.measurement(0)[0], i1) # Set arbitrary 3D point corresponding to the track - self.tracks.setP(gtsam.Point3(2.5, 3.3, 1.2)) + self.tracks.set_point3(gtsam.Point3(2.5, 3.3, 1.2)) np.testing.assert_array_almost_equal( gtsam.Point3(2.5,3.3,1.2), self.tracks.point3() @@ -59,10 +60,11 @@ class TestSfmData(GtsamTestCase): #cam1 = gtsam.SfmCamera(1500, 1200, 0, 640, 480) # Create new track with 3 measurements track2 = gtsam.SfmTrack() - i1, i2, i3 = np.random.randint(5), np.random.randint(5), np.random.randint(5) - uv_i1 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) - uv_i2 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) - uv_i3 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + i1, i2, i3 = 3,5,6 + uv_i1 = gtsam.Point2(21.23, 45.64) + # translating along X-axis + uv_i2 = gtsam.Point2(45.7, 45.64) + uv_i3 = gtsam.Point2(68.35, 45.64) m_i1, m_i2, m_i3 = (i1, uv_i1), (i2, uv_i2), (i3, uv_i3) # add measurements to the track track2.add_measurement(m_i1) From a7b71cf203b88fdae9244c6faab3e1b12a96f6d2 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sat, 24 Oct 2020 19:06:35 -0400 Subject: [PATCH 09/11] remved commented code --- python/gtsam/tests/test_SfmData.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index 6263f19a0..aa93a69bb 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -17,7 +17,6 @@ import unittest import numpy as np import gtsam -#from gtsam import SfmCamera from gtsam.utils.test_case import GtsamTestCase @@ -57,7 +56,6 @@ class TestSfmData(GtsamTestCase): def test_data(self): """Test functions in SfmData""" - #cam1 = gtsam.SfmCamera(1500, 1200, 0, 640, 480) # Create new track with 3 measurements track2 = gtsam.SfmTrack() i1, i2, i3 = 3,5,6 From ee0eefbc86a20725a95ac4c8f97b3ae060a8da5e Mon Sep 17 00:00:00 2001 From: Sushmita Date: Tue, 27 Oct 2020 21:52:31 -0400 Subject: [PATCH 10/11] added new constructor and changed to emplace --- gtsam/gtsam.i | 6 +++--- gtsam/slam/dataset.h | 13 +++++-------- python/gtsam/tests/test_SfmData.py | 24 +++++++++++------------- 3 files changed, 19 insertions(+), 24 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d6ae409f8..dcacf14d9 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2762,12 +2762,12 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { class SfmTrack { SfmTrack(); - Point3 point3() const; + SfmTrack(const gtsam::Point3& pt); + const Point3& point3() const; size_t number_measurements() const; - void set_point3(gtsam::Point3& p_); pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; - void add_measurement(const pair& m); + void add_measurement(size_t idx, const gtsam::Point2& m); }; class SfmData { diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 85b32ff5a..93bd2b2ee 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -217,6 +217,7 @@ 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)) @@ -226,10 +227,6 @@ struct SfmTrack { size_t number_measurements() const { return measurements.size(); } - /// Set 3D point - void set_point3(const Point3& p_){ - p = p_; - } /// Get the measurement (camera index, Point2) at pose index `idx` SfmMeasurement measurement(size_t idx) const { return measurements[idx]; @@ -239,12 +236,12 @@ struct SfmTrack { return siftIndices[idx]; } /// Get 3D point - Point3 point3() const { + const Point3& point3() const { return p; } - /// Add measurement to track - void add_measurement(const pair& m) { - measurements.push_back(m); + /// Add measurement (camera_idx, Point2) to track + void add_measurement(size_t idx, const gtsam::Point2& m) { + measurements.emplace_back(idx, m); } }; diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index aa93a69bb..51564fb6f 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -26,7 +26,8 @@ class TestSfmData(GtsamTestCase): def setUp(self): """Initialize SfmData and SfmTrack""" self.data = gtsam.SfmData() - self.tracks = gtsam.SfmTrack() + # initialize SfmTrack with 3D point + self.tracks = gtsam.SfmTrack(gtsam.Point3(2.5, 3.3, 1.2)) def test_tracks(self): """Test functions in SfmTrack""" @@ -37,17 +38,14 @@ class TestSfmData(GtsamTestCase): uv_i1 = gtsam.Point2(12.6, 82) # translating point uv_i1 along X-axis uv_i2 = gtsam.Point2(24.88, 82) - m_i1 = (i1, uv_i1) - m_i2 = (i2, uv_i2) # add measurements to the track - self.tracks.add_measurement(m_i1) - self.tracks.add_measurement(m_i2) + self.tracks.add_measurement(i1, uv_i1) + self.tracks.add_measurement(i2, uv_i2) # Number of measurements in the track is 2 self.assertEqual(self.tracks.number_measurements(), 2) # camera_idx in the first measurement of the track corresponds to i1 - self.assertEqual(self.tracks.measurement(0)[0], i1) - # Set arbitrary 3D point corresponding to the track - self.tracks.set_point3(gtsam.Point3(2.5, 3.3, 1.2)) + cam_idx, img_measurement = self.tracks.measurement(0) + self.assertEqual(cam_idx, i1) np.testing.assert_array_almost_equal( gtsam.Point3(2.5,3.3,1.2), self.tracks.point3() @@ -63,17 +61,17 @@ class TestSfmData(GtsamTestCase): # translating along X-axis uv_i2 = gtsam.Point2(45.7, 45.64) uv_i3 = gtsam.Point2(68.35, 45.64) - m_i1, m_i2, m_i3 = (i1, uv_i1), (i2, uv_i2), (i3, uv_i3) # add measurements to the track - track2.add_measurement(m_i1) - track2.add_measurement(m_i2) - track2.add_measurement(m_i3) + track2.add_measurement(i1, uv_i1) + track2.add_measurement(i2, uv_i2) + track2.add_measurement(i3, uv_i3) self.data.add_track(self.tracks) self.data.add_track(track2) # Number of tracks in SfmData is 2 self.assertEqual(self.data.number_tracks(), 2) # camera idx of first measurement of second track corresponds to i1 - self.assertEqual(self.data.track(1).measurement(0)[0], i1) + cam_idx, img_measurement = self.data.track(1).measurement(0) + self.assertEqual(cam_idx, i1) if __name__ == '__main__': unittest.main() From 65a6d06bf170206779ab82b301a0b95354ee07f0 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 1 Nov 2020 21:29:38 -0500 Subject: [PATCH 11/11] sfmtrack constructor changed to accept point --- python/gtsam/preamble.h | 2 +- python/gtsam/tests/test_SfmData.py | 10 ++++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index e31b5e6d8..6166f615e 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -9,4 +9,4 @@ 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 +PYBIND11_MAKE_OPAQUE(std::vector); diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index 51564fb6f..9c965ddc5 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -27,7 +27,7 @@ class TestSfmData(GtsamTestCase): """Initialize SfmData and SfmTrack""" self.data = gtsam.SfmData() # initialize SfmTrack with 3D point - self.tracks = gtsam.SfmTrack(gtsam.Point3(2.5, 3.3, 1.2)) + self.tracks = gtsam.SfmTrack() def test_tracks(self): """Test functions in SfmTrack""" @@ -47,7 +47,7 @@ class TestSfmData(GtsamTestCase): cam_idx, img_measurement = self.tracks.measurement(0) self.assertEqual(cam_idx, i1) np.testing.assert_array_almost_equal( - gtsam.Point3(2.5,3.3,1.2), + gtsam.Point3(0.,0.,0.), self.tracks.point3() ) @@ -55,13 +55,15 @@ class TestSfmData(GtsamTestCase): def test_data(self): """Test functions in SfmData""" # Create new track with 3 measurements - track2 = gtsam.SfmTrack() i1, i2, i3 = 3,5,6 uv_i1 = gtsam.Point2(21.23, 45.64) # translating along X-axis uv_i2 = gtsam.Point2(45.7, 45.64) uv_i3 = gtsam.Point2(68.35, 45.64) - # add measurements to the track + # add measurements and arbitrary point to the track + measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)] + pt = gtsam.Point3(1.0, 6.0, 2.0) + track2 = gtsam.SfmTrack(pt) track2.add_measurement(i1, uv_i1) track2.add_measurement(i2, uv_i2) track2.add_measurement(i3, uv_i3)