SFMdata.py
Go to the documentation of this file.
1 """
2 A structure-from-motion example with landmarks
3  - The landmarks form a 10 meter cube
4  - The robot rotates around the landmarks, always facing towards the cube
5 """
6 
7 # pylint: disable=invalid-name, E1101
8 
9 from typing import List
10 
11 import numpy as np
12 
13 from gtsam import Point3, Pose3, Rot3
14 
15 
16 def createPoints() -> List[Point3]:
17  # Create the set of ground-truth landmarks
18  points = [
19  Point3(10.0, 10.0, 10.0),
20  Point3(-10.0, 10.0, 10.0),
21  Point3(-10.0, -10.0, 10.0),
22  Point3(10.0, -10.0, 10.0),
23  Point3(10.0, 10.0, -10.0),
24  Point3(-10.0, 10.0, -10.0),
25  Point3(-10.0, -10.0, -10.0),
26  Point3(10.0, -10.0, -10.0),
27  ]
28  return points
29 
30 
31 _M_PI_2 = np.pi / 2
32 _M_PI_4 = np.pi / 4
33 
34 
36  init: Pose3 = Pose3(Rot3.Ypr(_M_PI_2, 0, -_M_PI_2), Point3(30, 0, 0)),
37  delta: Pose3 = Pose3(
38  Rot3.Ypr(0, -_M_PI_4, 0),
39  Point3(np.sin(_M_PI_4) * 30, 0, 30 * (1 - np.sin(_M_PI_4))),
40  ),
41  steps: int = 8,
42 ) -> List[Pose3]:
43  """
44  Create a set of ground-truth poses
45  Default values give a circular trajectory, radius 30 at pi/4 intervals,
46  always facing the circle center
47  """
48  poses = [init]
49  for _ in range(1, steps):
50  poses.append(poses[-1].compose(delta))
51  return poses
52 
53 
54 def posesOnCircle(num_cameras=8, R=30):
55  """Create regularly spaced poses with specified radius and number of cameras."""
56  theta = 2 * np.pi / num_cameras
57 
58  # Initial pose at angle 0, position (R, 0, 0), facing the center with Y-axis pointing down
59  init_rotation = Rot3.Ypr(_M_PI_2, 0, -_M_PI_2)
60  init_position = np.array([R, 0, 0])
61  init = Pose3(init_rotation, init_position)
62 
63  # Delta rotation: rotate by -theta around Z-axis (counterclockwise movement)
64  delta_rotation = Rot3.Ypr(0, -theta, 0)
65 
66  # Delta translation in world frame
67  delta_translation_world = np.array([R * (np.cos(theta) - 1), R * np.sin(theta), 0])
68 
69  # Transform delta translation to local frame of the camera
70  delta_translation_local = init.rotation().unrotate(delta_translation_world)
71 
72  # Define delta pose
73  delta = Pose3(delta_rotation, delta_translation_local)
74 
75  # Generate poses
76  return createPoses(init, delta, num_cameras)
gtsam::compose
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:23
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
gtsam::unrotate
Point3_ unrotate(const Rot3_ &x, const Point3_ &p)
Definition: slam/expressions.h:109
gtsam::Pose3
Definition: Pose3.h:37
gtsam.examples.SFMdata.posesOnCircle
def posesOnCircle(num_cameras=8, R=30)
Definition: SFMdata.py:54
gtsam.examples.SFMdata.createPoints
List[Point3] createPoints()
Definition: SFMdata.py:16
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam.examples.SFMdata.createPoses
List[Pose3] createPoses(Pose3 init=Pose3(Rot3.Ypr(_M_PI_2, 0, -_M_PI_2), Point3(30, 0, 0)), Pose3 delta=Pose3(Rot3.Ypr(0, -_M_PI_4, 0), Point3(np.sin(_M_PI_4) *30, 0, 30 *(1 - np.sin(_M_PI_4))),), int steps=8)
Definition: SFMdata.py:35


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:15