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

Address comments

release/4.3a0
Frank Dellaert 2022-02-01 15:20:10 -05:00
parent bc721e7736
commit c7fb4a1619
2 changed files with 24 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -14,10 +14,9 @@ from __future__ import print_function
import unittest import unittest
import numpy as np
import gtsam import gtsam
from gtsam import SfmData, SfmTrack, Point2, Point3 import numpy as np
from gtsam import Point2, Point3, SfmData, SfmTrack
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
@ -34,40 +33,39 @@ class TestSfmData(GtsamTestCase):
"""Test functions in SfmTrack""" """Test functions in SfmTrack"""
# measurement is of format (camera_idx, imgPoint) # measurement is of format (camera_idx, imgPoint)
# create arbitrary camera indices for two cameras # create arbitrary camera indices for two cameras
i1, i2 = 4,5 i1, i2 = 4, 5
# create arbitrary image measurements for cameras i1 and i2 # create arbitrary image measurements for cameras i1 and i2
uv_i1 = Point2(12.6, 82) uv_i1 = Point2(12.6, 82)
# translating point uv_i1 along X-axis # translating point uv_i1 along X-axis
uv_i2 = Point2(24.88, 82) uv_i2 = Point2(24.88, 82)
# add measurements to the track # add measurements to the track
self.tracks.addMeasurement(i1, uv_i1) self.tracks.addMeasurement(i1, uv_i1)
self.tracks.addMeasurement(i2, uv_i2) self.tracks.addMeasurement(i2, uv_i2)
# Number of measurements in the track is 2 # Number of measurements in the track is 2
self.assertEqual(self.tracks.numberMeasurements(), 2) self.assertEqual(self.tracks.numberMeasurements(), 2)
# camera_idx in the first measurement of the track corresponds to i1 # camera_idx in the first measurement of the track corresponds to i1
cam_idx, img_measurement = self.tracks.measurement(0) cam_idx, img_measurement = self.tracks.measurement(0)
self.assertEqual(cam_idx, i1) self.assertEqual(cam_idx, i1)
np.testing.assert_array_almost_equal( np.testing.assert_array_almost_equal(
Point3(0.,0.,0.), Point3(0., 0., 0.),
self.tracks.point3() self.tracks.point3()
) )
def test_data(self): def test_data(self):
"""Test functions in SfmData""" """Test functions in SfmData"""
# Create new track with 3 measurements # Create new track with 3 measurements
i1, i2, i3 = 3,5,6 i1, i2, i3 = 3, 5, 6
uv_i1 = Point2(21.23, 45.64) uv_i1 = Point2(21.23, 45.64)
# translating along X-axis # translating along X-axis
uv_i2 = Point2(45.7, 45.64) uv_i2 = Point2(45.7, 45.64)
uv_i3 = Point2(68.35, 45.64) uv_i3 = Point2(68.35, 45.64)
# add measurements and arbitrary point to the track # add measurements and arbitrary point to the track
measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)] measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
pt = Point3(1.0, 6.0, 2.0) pt = Point3(1.0, 6.0, 2.0)
@ -80,14 +78,17 @@ class TestSfmData(GtsamTestCase):
# Number of tracks in SfmData is 2 # Number of tracks in SfmData is 2
self.assertEqual(self.data.numberTracks(), 2) self.assertEqual(self.data.numberTracks(), 2)
# camera idx of first measurement of second track corresponds to i1 # camera idx of first measurement of second track corresponds to i1
cam_idx, img_measurement = self.data.track(1).measurement(0) cam_idx, img_measurement = self.data.track(1).measurement(0)
self.assertEqual(cam_idx, i1) self.assertEqual(cam_idx, i1)
def test_Balbianello(self): def test_Balbianello(self):
""" Check that we can successfully read a bundler file and create a
factor graph from it
"""
# The structure where we will save the SfM data # The structure where we will save the SfM data
filename = gtsam.findExampleDataFile("Balbianello") filename = gtsam.findExampleDataFile("Balbianello.out")
sfm_data = SfmData.FromBundlerFile(filename) sfm_data = SfmData.FromBundlerFile(filename)
# Check number of things # Check number of things
@ -104,7 +105,8 @@ class TestSfmData(GtsamTestCase):
self.gtsamAssertEquals(expected, actual, 1) self.gtsamAssertEquals(expected, actual, 1)
# We share *one* noiseModel between all projection factors # We share *one* noiseModel between all projection factors
model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v model = gtsam.noiseModel.Isotropic.Sigma(
2, 1.0) # one pixel in u and v
# Convert to NonlinearFactorGraph # Convert to NonlinearFactorGraph
graph = sfm_data.sfmFactorGraph(model) graph = sfm_data.sfmFactorGraph(model)

11
python/gtsam/utils/visual_data_generator.py
View File

@ -1,12 +1,12 @@
from __future__ import print_function from __future__ import print_function
from typing import Tuple
import math import math
import numpy as np
from math import pi from math import pi
from typing import Tuple
import gtsam import gtsam
from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2 import numpy as np
from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point3, Pose3
class Options: class Options:
@ -36,7 +36,7 @@ class GroundTruth:
self.cameras = [Pose3()] * nrCameras self.cameras = [Pose3()] * nrCameras
self.points = [Point3(0, 0, 0)] * nrPoints self.points = [Point3(0, 0, 0)] * nrPoints
def print(self, s="") -> None: def print(self, s: string = "") -> None:
print(s) print(s)
print("K = ", self.K) print("K = ", self.K)
print("Cameras: ", len(self.cameras)) print("Cameras: ", len(self.cameras))
@ -88,7 +88,8 @@ def generate_data(options) -> Tuple[Data, GroundTruth]:
r = 10 r = 10
for j in range(len(truth.points)): for j in range(len(truth.points)):
theta = j * 2 * pi / nrPoints theta = j * 2 * pi / nrPoints
truth.points[j] = Point3(r * math.cos(theta), r * math.sin(theta), 0) truth.points[j] = Point3(
r * math.cos(theta), r * math.sin(theta), 0)
else: # 3D landmarks as vertices of a cube else: # 3D landmarks as vertices of a cube
truth.points = [ truth.points = [
Point3(10, 10, 10), Point3(-10, 10, 10), Point3(10, 10, 10), Point3(-10, 10, 10),