Address comments
parent
bc721e7736
commit
c7fb4a1619
|
@ -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,7 +33,7 @@ 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)
|
||||
|
@ -53,15 +52,14 @@ class TestSfmData(GtsamTestCase):
|
|||
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
|
||||
|
@ -86,8 +84,11 @@ class TestSfmData(GtsamTestCase):
|
|||
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)
|
||||
|
|
|
@ -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),
|
||||
|
|
Loading…
Reference in New Issue