From ed387e38173d837cf8c421eab649483b5e560231 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 18 Oct 2020 11:17:10 -0400 Subject: [PATCH] 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()