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

Add to wrapper

release/4.3a0
Frank Dellaert 2022-01-31 13:44:28 -05:00
parent 9dfe52d0b6
commit 5d6b8f445e
3 changed files with 67 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/sfm/sfm.i
View File

@ -31,15 +31,23 @@ class SfmTrack {
#include <gtsam/sfm/SfmData.h>
class SfmData {
SfmData();
gtsam::SfmData FromBundlerFile(string filename);
gtsam::SfmData FromBalFile(string filename);
static gtsam::SfmData FromBundlerFile(string filename);
static gtsam::SfmData FromBalFile(string filename);
size_t numberCameras() const;
size_t numberTracks() const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::SfmTrack track(size_t idx) const;
void addTrack(const gtsam::SfmTrack& t);
void addCamera(const gtsam::SfmCamera& cam);
size_t numberTracks() const;
size_t numberCameras() const;
gtsam::SfmTrack track(size_t idx) const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::NonlinearFactorGraph generalSfmFactors(
const gtsam::SharedNoiseModel& model =
gtsam::noiseModel::Isotropic::Sigma(2, 1.0)) const;
gtsam::NonlinearFactorGraph sfmFactorGraph(
const gtsam::SharedNoiseModel& model =
gtsam::noiseModel::Isotropic::Sigma(2, 1.0),
size_t fixedCamera = 0, size_t fixedPoint = 0) const;
// enabling serialization functionality
void serialize() const;

6
python/gtsam/examples/SFMExample_bal.py
View File

@ -15,7 +15,7 @@ import logging
import sys
import gtsam
from gtsam import (GeneralSFMFactorCal3Bundler,
from gtsam import (GeneralSFMFactorCal3Bundler, SfmData,
PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3)
from gtsam.symbol_shorthand import C, P # type: ignore
from gtsam.utils import plot # type: ignore
@ -26,7 +26,7 @@ logging.basicConfig(stream=sys.stdout, level=logging.INFO)
DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre"
def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None:
def plot_scene(scene_data: SfmData, result: gtsam.Values) -> None:
"""Plot the SFM results."""
plot_vals = gtsam.Values()
for cam_idx in range(scene_data.numberCameras()):
@ -46,7 +46,7 @@ def run(args: argparse.Namespace) -> None:
input_file = args.input_file
# Load the SfM data from file
scene_data = gtsam.readBal(input_file)
scene_data = SfmData.FromBalFile(input_file)
logging.info("read %d tracks on %d cameras\n", scene_data.numberTracks(),
scene_data.numberCameras())

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

@ -17,6 +17,7 @@ import unittest
import numpy as np
import gtsam
from gtsam import SfmData, SfmTrack, Point2, Point3
from gtsam.utils.test_case import GtsamTestCase
@ -25,29 +26,34 @@ class TestSfmData(GtsamTestCase):
def setUp(self):
"""Initialize SfmData and SfmTrack"""
self.data = gtsam.SfmData()
self.data = SfmData()
# initialize SfmTrack with 3D point
self.tracks = gtsam.SfmTrack()
self.tracks = 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)
uv_i1 = Point2(12.6, 82)
# translating point uv_i1 along X-axis
uv_i2 = gtsam.Point2(24.88, 82)
uv_i2 = Point2(24.88, 82)
# add measurements to the track
self.tracks.addMeasurement(i1, uv_i1)
self.tracks.addMeasurement(i2, uv_i2)
# Number of measurements in the track is 2
self.assertEqual(self.tracks.numberMeasurements(), 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.),
Point3(0.,0.,0.),
self.tracks.point3()
)
@ -56,24 +62,58 @@ class TestSfmData(GtsamTestCase):
"""Test functions in SfmData"""
# Create new track with 3 measurements
i1, i2, i3 = 3,5,6
uv_i1 = gtsam.Point2(21.23, 45.64)
uv_i1 = 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)
uv_i2 = Point2(45.7, 45.64)
uv_i3 = 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)
pt = Point3(1.0, 6.0, 2.0)
track2 = SfmTrack(pt)
track2.addMeasurement(i1, uv_i1)
track2.addMeasurement(i2, uv_i2)
track2.addMeasurement(i3, uv_i3)
self.data.addTrack(self.tracks)
self.data.addTrack(track2)
# Number of tracks in SfmData is 2
self.assertEqual(self.data.numberTracks(), 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)
def test_Balbianello(self):
# The structure where we will save the SfM data
filename = gtsam.findExampleDataFile("Balbianello")
sfm_data = SfmData.FromBundlerFile(filename)
# Check number of things
self.assertEqual(5, sfm_data.numberCameras())
self.assertEqual(544, sfm_data.numberTracks())
track0 = sfm_data.track(0)
self.assertEqual(3, track0.numberMeasurements())
# Check projection of a given point
self.assertEqual(0, track0.measurement(0)[0])
camera0 = sfm_data.camera(0)
expected = camera0.project(track0.point3())
actual = track0.measurement(0)[1]
self.gtsamAssertEquals(expected, actual, 1)
# We share *one* noiseModel between all projection factors
model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
# Convert to NonlinearFactorGraph
graph = sfm_data.sfmFactorGraph(model)
self.assertEqual(1419, graph.size()) # regression
# Get initial estimate
values = gtsam.initialCamerasAndPointsEstimate(sfm_data)
self.assertEqual(549, values.size()) # regression
if __name__ == '__main__':
unittest.main()