Merge pull request #569 from borglab/feature/wrap_SfmData
Adding utility functions to classes SfmTrack and SfmDatarelease/4.3a0
						commit
						ed971210c7
					
				|  | @ -2767,6 +2767,8 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { | |||
| 
 | ||||
| class SfmTrack { | ||||
|   SfmTrack(); | ||||
|   SfmTrack(const gtsam::Point3& pt); | ||||
|   const Point3& point3() const; | ||||
| 
 | ||||
|   double r; | ||||
|   double g; | ||||
|  | @ -2774,20 +2776,24 @@ class SfmTrack { | |||
|   // TODO Need to close wrap#10 to allow this to work. | ||||
|   // std::vector<pair<size_t, gtsam::Point2>> measurements; | ||||
| 
 | ||||
|   Point3 point3() const; | ||||
|   size_t number_measurements() const; | ||||
|   pair<size_t, gtsam::Point2> measurement(size_t idx) const; | ||||
|   pair<size_t, size_t> siftIndex(size_t idx) const; | ||||
|   void add_measurement(size_t idx, const gtsam::Point2& m); | ||||
| }; | ||||
| 
 | ||||
| class SfmData { | ||||
|   SfmData(); | ||||
|   size_t number_cameras() const; | ||||
|   size_t number_tracks() const; | ||||
|   gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const; | ||||
|   gtsam::SfmTrack track(size_t idx) const; | ||||
|   void add_track(const gtsam::SfmTrack& t) ; | ||||
|   void add_camera(const gtsam::SfmCamera& cam); | ||||
| }; | ||||
| 
 | ||||
| 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); | ||||
| 
 | ||||
|  |  | |||
|  | @ -211,16 +211,18 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); | |||
| /// A measurement with its camera index
 | ||||
| typedef std::pair<size_t, Point2> SfmMeasurement; | ||||
| 
 | ||||
| /// SfmTrack
 | ||||
| /// Sift index for SfmTrack
 | ||||
| typedef std::pair<size_t, size_t> 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<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
 | ||||
|   std::vector<SiftIndex> siftIndices; | ||||
|    | ||||
|   /// Total number of measurements in this track
 | ||||
|   size_t number_measurements() const { | ||||
|     return measurements.size(); | ||||
|  | @ -233,11 +235,17 @@ struct SfmTrack { | |||
|   SiftIndex siftIndex(size_t idx) const { | ||||
|     return siftIndices[idx]; | ||||
|   } | ||||
|   Point3 point3() const { | ||||
|   /// 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); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
| /// Define the structure for the camera poses
 | ||||
| typedef PinholeCamera<Cal3Bundler> SfmCamera; | ||||
| 
 | ||||
|  | @ -260,6 +268,14 @@ 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); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  |  | |||
|  | @ -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.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() | ||||
|         # initialize SfmTrack with 3D point | ||||
|         self.tracks = gtsam.SfmTrack() | ||||
| 
 | ||||
|     def test_tracks(self): | ||||
|         """Test functions in SfmTrack""" | ||||
|         # measurement is of format (camera_idx, imgPoint) | ||||
|         # 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) | ||||
|         # add measurements to the track | ||||
|         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 | ||||
|         cam_idx, img_measurement = self.tracks.measurement(0) | ||||
|         self.assertEqual(cam_idx, i1) | ||||
|         np.testing.assert_array_almost_equal( | ||||
|             gtsam.Point3(0.,0.,0.),  | ||||
|             self.tracks.point3() | ||||
|         ) | ||||
| 
 | ||||
| 
 | ||||
|     def test_data(self): | ||||
|         """Test functions in SfmData""" | ||||
|         # Create new track with 3 measurements | ||||
|         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 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) | ||||
|         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 | ||||
|         cam_idx, img_measurement = self.data.track(1).measurement(0) | ||||
|         self.assertEqual(cam_idx, i1) | ||||
| 
 | ||||
| if __name__ == '__main__': | ||||
|     unittest.main() | ||||
		Loading…
	
		Reference in New Issue