12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

clean up SFMdata

release/4.3a0
John Lambert 2021-08-21 21:08:37 -06:00 committed by GitHub
parent 52161785cf
commit 67403b0e96
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 18 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
python/gtsam/examples/SFMdata.py
View File

@ -5,26 +5,31 @@ A structure-from-motion example with landmarks
"""
# pylint: disable=invalid-name, E1101
from typing import List
import numpy as np
import gtsam
from gtsam import Cal3_S2, Point3, Pose3
def createPoints():
def createPoints() -> List[Point3]:
# Create the set of ground-truth landmarks
points = [gtsam.Point3(10.0, 10.0, 10.0),
gtsam.Point3(-10.0, 10.0, 10.0),
gtsam.Point3(-10.0, -10.0, 10.0),
gtsam.Point3(10.0, -10.0, 10.0),
gtsam.Point3(10.0, 10.0, -10.0),
gtsam.Point3(-10.0, 10.0, -10.0),
gtsam.Point3(-10.0, -10.0, -10.0),
gtsam.Point3(10.0, -10.0, -10.0)]
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
def createPoses(K):
# Create the set of ground-truth poses
def createPoses(K: Cal3_S2) -> List[Pose3]:
"""Generate a set of ground-truth camera poses arranged in a circle about the origin."""
radius = 40.0
height = 10.0
angles = np.linspace(0, 2 * np.pi, 8, endpoint=False)
@ -32,9 +37,7 @@ def createPoses(K):
target = gtsam.Point3(0, 0, 0)
poses = []
for theta in angles:
position = gtsam.Point3(radius*np.cos(theta),
radius*np.sin(theta),
height)
position = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta), height)
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
poses.append(camera.pose())
return poses