77 lines
2.2 KiB
Python
77 lines
2.2 KiB
Python
"""
|
|
A structure-from-motion example with landmarks
|
|
- The landmarks form a 10 meter cube
|
|
- The robot rotates around the landmarks, always facing towards the cube
|
|
"""
|
|
|
|
# pylint: disable=invalid-name, E1101
|
|
|
|
from typing import List
|
|
|
|
import numpy as np
|
|
|
|
from gtsam import Point3, Pose3, Rot3
|
|
|
|
|
|
def createPoints() -> List[Point3]:
|
|
# Create the set of ground-truth landmarks
|
|
points = [
|
|
Point3(10.0, 10.0, 10.0),
|
|
Point3(-10.0, 10.0, 10.0),
|
|
Point3(-10.0, -10.0, 10.0),
|
|
Point3(10.0, -10.0, 10.0),
|
|
Point3(10.0, 10.0, -10.0),
|
|
Point3(-10.0, 10.0, -10.0),
|
|
Point3(-10.0, -10.0, -10.0),
|
|
Point3(10.0, -10.0, -10.0),
|
|
]
|
|
return points
|
|
|
|
|
|
_M_PI_2 = np.pi / 2
|
|
_M_PI_4 = np.pi / 4
|
|
|
|
|
|
def createPoses(
|
|
init: Pose3 = Pose3(Rot3.Ypr(_M_PI_2, 0, -_M_PI_2), Point3(30, 0, 0)),
|
|
delta: Pose3 = Pose3(
|
|
Rot3.Ypr(0, -_M_PI_4, 0),
|
|
Point3(np.sin(_M_PI_4) * 30, 0, 30 * (1 - np.sin(_M_PI_4))),
|
|
),
|
|
steps: int = 8,
|
|
) -> List[Pose3]:
|
|
"""
|
|
Create a set of ground-truth poses
|
|
Default values give a circular trajectory, radius 30 at pi/4 intervals,
|
|
always facing the circle center
|
|
"""
|
|
poses = [init]
|
|
for _ in range(1, steps):
|
|
poses.append(poses[-1].compose(delta))
|
|
return poses
|
|
|
|
|
|
def posesOnCircle(num_cameras=8, R=30):
|
|
"""Create regularly spaced poses with specified radius and number of cameras."""
|
|
theta = 2 * np.pi / num_cameras
|
|
|
|
# Initial pose at angle 0, position (R, 0, 0), facing the center with Y-axis pointing down
|
|
init_rotation = Rot3.Ypr(_M_PI_2, 0, -_M_PI_2)
|
|
init_position = np.array([R, 0, 0])
|
|
init = Pose3(init_rotation, init_position)
|
|
|
|
# Delta rotation: rotate by -theta around Z-axis (counterclockwise movement)
|
|
delta_rotation = Rot3.Ypr(0, -theta, 0)
|
|
|
|
# Delta translation in world frame
|
|
delta_translation_world = np.array([R * (np.cos(theta) - 1), R * np.sin(theta), 0])
|
|
|
|
# Transform delta translation to local frame of the camera
|
|
delta_translation_local = init.rotation().unrotate(delta_translation_world)
|
|
|
|
# Define delta pose
|
|
delta = Pose3(delta_rotation, delta_translation_local)
|
|
|
|
# Generate poses
|
|
return createPoses(init, delta, num_cameras)
|