visual_data_generator.py
Go to the documentation of this file.
1 from __future__ import print_function
2 
3 import math
4 from math import pi
5 from typing import Tuple
6 
7 import gtsam
8 import numpy as np
9 from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point3, Pose3
10 
11 
12 class Options:
13  """
14  Options to generate test scenario
15  """
16 
17  def __init__(self, triangle: bool = False, nrCameras: int = 3, K=Cal3_S2()) -> None:
18  """
19  Options to generate test scenario
20  @param triangle: generate a triangle scene with 3 points if True, otherwise
21  a cube with 8 points
22  @param nrCameras: number of cameras to generate
23  @param K: camera calibration object
24  """
25  self.triangle = triangle
26  self.nrCameras = nrCameras
27 
28 
30  """
31  Object holding generated ground-truth data
32  """
33 
34  def __init__(self, K=Cal3_S2(), nrCameras: int = 3, nrPoints: int = 4) -> None:
35  self.K = K
36  self.cameras = [Pose3()] * nrCameras
37  self.points = [Point3(0, 0, 0)] * nrPoints
38 
39  def print(self, s: str = "") -> None:
40  print(s)
41  print("K = ", self.K)
42  print("Cameras: ", len(self.cameras))
43  for camera in self.cameras:
44  print("\t", camera)
45  print("Points: ", len(self.points))
46  for point in self.points:
47  print("\t", point)
48  pass
49 
50 
51 class Data:
52  """
53  Object holding generated measurement data
54  """
55 
56  class NoiseModels:
57  pass
58 
59  def __init__(self, K=Cal3_S2(), nrCameras: int = 3, nrPoints: int = 4) -> None:
60  self.K = K
61  self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras]
62  self.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
63  self.odometry = [Pose3()] * nrCameras
64 
65  # Set Noise parameters
68  np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]))
69  # noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas(
70  # 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]))
73  self.noiseModels.pointPrior = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
74  self.noiseModels.measurement = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)
75 
76 
77 def generate_data(options) -> Tuple[Data, GroundTruth]:
78  """ Generate ground-truth and measurement data. """
79 
80  K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
81  nrPoints = 3 if options.triangle else 8
82 
83  truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints)
84  data = Data(K, nrCameras=options.nrCameras, nrPoints=nrPoints)
85 
86  # Generate simulated data
87  if options.triangle: # Create a triangle target, just 3 points on a plane
88  r = 10
89  for j in range(len(truth.points)):
90  theta = j * 2 * pi / nrPoints
91  truth.points[j] = Point3(
92  r * math.cos(theta), r * math.sin(theta), 0)
93  else: # 3D landmarks as vertices of a cube
94  truth.points = [
95  Point3(10, 10, 10), Point3(-10, 10, 10),
96  Point3(-10, -10, 10), Point3(10, -10, 10),
97  Point3(10, 10, -10), Point3(-10, 10, -10),
98  Point3(-10, -10, -10), Point3(10, -10, -10)
99  ]
100 
101  # Create camera cameras on a circle around the triangle
102  height = 10
103  r = 40
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,
108  Point3(0, 0, 0),
109  Point3(0, 0, 1),
110  truth.K)
111  # Create measurements
112  for j in range(nrPoints):
113  # All landmarks seen in every frame
114  data.Z[i][j] = truth.cameras[i].project(truth.points[j])
115  data.J[i][j] = j
116 
117  # Calculate odometry between cameras
118  for i in range(1, options.nrCameras):
119  data.odometry[i] = truth.cameras[i - 1].pose().between(
120  truth.cameras[i].pose())
121 
122  return data, truth
gtsam::utils.visual_data_generator.Data.noiseModels
noiseModels
Definition: visual_data_generator.py:66
gtsam::utils.visual_data_generator.GroundTruth.__init__
None __init__(self, K=Cal3_S2(), int nrCameras=3, int nrPoints=4)
Definition: visual_data_generator.py:34
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::between
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:17
gtsam::utils.visual_data_generator.Data.NoiseModels
Definition: visual_data_generator.py:56
gtsam::utils.visual_data_generator.Data.odometry
odometry
Definition: visual_data_generator.py:63
gtsam::utils.visual_data_generator.GroundTruth.print
None print(self, str s="")
Definition: visual_data_generator.py:39
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270
gtsam::utils.visual_data_generator.GroundTruth
Definition: visual_data_generator.py:29
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
gtsam::utils.visual_data_generator.generate_data
Tuple[Data, GroundTruth] generate_data(options)
Definition: visual_data_generator.py:77
gtsam::utils.visual_data_generator.GroundTruth.points
points
Definition: visual_data_generator.py:37
gtsam::utils.visual_data_generator.GroundTruth.K
K
Definition: visual_data_generator.py:35
gtsam::Pose3
Definition: Pose3.h:37
gtsam::utils.visual_data_generator.Data.J
J
Definition: visual_data_generator.py:62
gtsam::utils.visual_data_generator.Options.triangle
triangle
Definition: visual_data_generator.py:25
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::utils.visual_data_generator.GroundTruth.cameras
cameras
Definition: visual_data_generator.py:36
gtsam::utils.visual_data_generator.Options.__init__
None __init__(self, bool triangle=False, int nrCameras=3, K=Cal3_S2())
Definition: visual_data_generator.py:17
gtsam::utils.visual_data_generator.Options
Definition: visual_data_generator.py:12
gtsam::utils.visual_data_generator.Data.__init__
None __init__(self, K=Cal3_S2(), int nrCameras=3, int nrPoints=4)
Definition: visual_data_generator.py:59
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:594
len
size_t len(handle h)
Get the length of a Python object.
Definition: pytypes.h:2399
gtsam::utils.visual_data_generator.Data
Definition: visual_data_generator.py:51
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition: slam/expressions.h:131
gtsam::utils.visual_data_generator.Data.K
K
Definition: visual_data_generator.py:60
gtsam::utils.visual_data_generator.Options.nrCameras
nrCameras
Definition: visual_data_generator.py:26
gtsam::utils.visual_data_generator.Data.Z
Z
Definition: visual_data_generator.py:61


gtsam
Author(s):
autogenerated on Tue Jun 25 2024 03:07:58