12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added new constructor and changed to emplace

release/4.3a0
Sushmita 2020-10-27 21:52:31 -04:00
parent a7b71cf203
commit ee0eefbc86
3 changed files with 19 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/gtsam.i
View File

@ -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<size_t, gtsam::Point2> measurement(size_t idx) const;
pair<size_t, size_t> siftIndex(size_t idx) const;
void add_measurement(const pair<size_t, gtsam::Point2>& m);
void add_measurement(size_t idx, const gtsam::Point2& m);
};
class SfmData {

13
gtsam/slam/dataset.h
View File

@ -217,6 +217,7 @@ 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))
@ -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<size_t, gtsam::Point2>& 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);
}
};

24
python/gtsam/tests/test_SfmData.py
View File

@ -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()