Merge pull request #569 from borglab/feature/wrap_SfmData

Adding utility functions to classes SfmTrack and SfmData
release/4.3a0
Sushmita Warrier 2020-11-09 10:29:45 -05:00 committed by GitHub
commit ed971210c7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 104 additions and 3 deletions

View File

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

View File

@ -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);
}
};
/**

View File

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