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