def createPoses(K: Cal3_S2) -> List[Pose3]:
"""Generate a set of ground-truth camera poses arranged in a circle about the origin."""
radius = 4.0
height = -2.0
angles = np.linspace(0, 2 * np.pi, 8, endpoint=False)
up = Point3(0, 0, 1) # NED -> NORTH_EAST_DOWN ???
target = Point3(0, 0, 0)
poses = []
for theta in angles:
position = Point3(radius * np.cos(theta), radius * np.sin(theta), height)
camera = PinholeCameraCal3_S2.Lookat(position, target, up, K)
poses.append(camera.pose())
return poses
def generate_data(options) -> Tuple[Data, GroundTruth]:
""" Generate ground-truth and measurement data. """
K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
nrPoints = 3 if options.triangle else 8
truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints)
data = Data(K, nrCameras=options.nrCameras, nrPoints=nrPoints)
# Generate simulated data
if options.triangle: # Create a triangle target, just 3 points on a plane
r = 10
for j in range(len(truth.points)):
theta = j * 2 * pi / nrPoints
truth.points[j] = Point3(
r * math.cos(theta), r * math.sin(theta), 0)
else: # 3D landmarks as vertices of a cube
truth.points = [
Point3(10, 10, 10), Point3(-10, 10, 10),
Point3(-10, -10, 10), Point3(10, -10, 10),
Point3(10, 10, -10), Point3(-10, 10, -10),
Point3(-10, -10, -10), Point3(10, -10, -10)
]
# Create camera cameras on a circle around the triangle
height = 10
r = 40
for i in range(options.nrCameras):
theta = i * 2 * pi / options.nrCameras
t = Point3(r * math.cos(theta), r * math.sin(theta), height)
truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
Point3(0, 0, 0),
Point3(0, 0, 1),
truth.K)
# Create measurements
for j in range(nrPoints):
# All landmarks seen in every frame
data.Z[i][j] = truth.cameras[i].project(truth.points[j])
data.J[i][j] = j
# Calculate odometry between cameras
for i in range(1, options.nrCameras):
data.odometry[i] = truth.cameras[i - 1].pose().between(
truth.cameras[i].pose())
return data, truth
def get_camera(radius):
up = Point3(0, 0, 1)
target = Point3(0, 0, 0)
position = Point3(radius, 0, 0)
camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
return camera