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
9 from typing
import List
13 from gtsam
import Point3, Pose3, Rot3
21 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),
36 init: Pose3 =
Pose3(Rot3.Ypr(_M_PI_2, 0, -_M_PI_2),
Point3(30, 0, 0)),
38 Rot3.Ypr(0, -_M_PI_4, 0),
39 Point3(np.sin(_M_PI_4) * 30, 0, 30 * (1 - np.sin(_M_PI_4))),
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
49 for _
in range(1, steps):
50 poses.append(poses[-1].
compose(delta))
55 """Create regularly spaced poses with specified radius and number of cameras."""
56 theta = 2 * np.pi / num_cameras
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)
64 delta_rotation = Rot3.Ypr(0, -theta, 0)
67 delta_translation_world = np.array([R * (np.cos(theta) - 1), R * np.sin(theta), 0])
70 delta_translation_local = init.rotation().
unrotate(delta_translation_world)
73 delta =
Pose3(delta_rotation, delta_translation_local)