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 numpy as np
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
@ -34,40 +33,39 @@ class TestSfmData(GtsamTestCase):
"""Test functions in SfmTrack"""
# measurement is of format (camera_idx, imgPoint)
# create arbitrary camera indices for two cameras
i1, i2 = 4,5
i1, i2 = 4, 5
# create arbitrary image measurements for cameras i1 and i2
uv_i1 = Point2(12.6, 82)
# translating point uv_i1 along X-axis
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(
Point3(0.,0.,0.),
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
i1, i2, i3 = 3, 5, 6
uv_i1 = Point2(21.23, 45.64)
# translating along X-axis
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 = Point3(1.0, 6.0, 2.0)
@ -80,14 +78,17 @@ class TestSfmData(GtsamTestCase):
# 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):
""" 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
filename = gtsam.findExampleDataFile("Balbianello")
filename = gtsam.findExampleDataFile("Balbianello.out")
sfm_data = SfmData.FromBundlerFile(filename)
# Check number of things
@ -104,7 +105,8 @@ class TestSfmData(GtsamTestCase):
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
model = gtsam.noiseModel.Isotropic.Sigma(
2, 1.0) # one pixel in u and v
# Convert to NonlinearFactorGraph
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 typing import Tuple
import math
import numpy as np
from math import pi
from typing import Tuple
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:
@ -36,7 +36,7 @@ class GroundTruth:
self.cameras = [Pose3()] * nrCameras
self.points = [Point3(0, 0, 0)] * nrPoints
def print(self, s="") -> None:
def print(self, s: string = "") -> None:
print(s)
print("K = ", self.K)
print("Cameras: ", len(self.cameras))
@ -88,7 +88,8 @@ def generate_data(options) -> Tuple[Data, GroundTruth]:
r = 10
for j in range(len(truth.points)):
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
truth.points = [
Point3(10, 10, 10), Point3(-10, 10, 10),