1 from __future__
import print_function
5 from typing
import Tuple
9 from gtsam
import Cal3_S2, PinholeCameraCal3_S2, Point3, Pose3
14 Options to generate test scenario
17 def __init__(self, triangle: bool =
False, nrCameras: int = 3, K=
Cal3_S2()) ->
None:
19 Options to generate test scenario
20 @param triangle: generate a triangle scene with 3 points if True, otherwise
22 @param nrCameras: number of cameras to generate
23 @param K: camera calibration object
31 Object holding generated ground-truth data
34 def __init__(self, K=Cal3_S2(), nrCameras: int = 3, nrPoints: int = 4) ->
None:
39 def print(self, s: str =
"") ->
None:
53 Object holding generated measurement data
59 def __init__(self, K=Cal3_S2(), nrCameras: int = 3, nrPoints: int = 4) ->
None:
62 self.
J = [x[:]
for x
in [[0] * nrPoints] * nrCameras]
68 np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]))
72 np.array([0.05, 0.05, 0.05, 0.2, 0.2, 0.2]))
78 """ Generate ground-truth and measurement data. """
80 K =
Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
81 nrPoints = 3
if options.triangle
else 8
83 truth =
GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints)
84 data =
Data(K, nrCameras=options.nrCameras, nrPoints=nrPoints)
90 theta = j * 2 * pi / nrPoints
92 r * math.cos(theta), r * math.sin(theta), 0)
104 for i
in range(options.nrCameras):
105 theta = i * 2 * pi / options.nrCameras
106 t =
Point3(r * math.cos(theta), r * math.sin(theta), height)
107 truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
112 for j
in range(nrPoints):
114 data.Z[i][j] = truth.cameras[i].
project(truth.points[j])
118 for i
in range(1, options.nrCameras):
119 data.odometry[i] = truth.cameras[i - 1].
pose().
between(
120 truth.cameras[i].
pose())