Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def createPoints():
# 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)]
return points
def createPoints():
# 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)]
return points
def createPoints():
# 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)]
return points
z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer)
Vehicle at p0 is looking towards y axis (X-axis points towards world y)
"""
# Force symbolChar to be a single character
if type(symbolChar) is str:
symbolChar = ord(symbolChar[0])
values = gtsam.Values()
theta = 0.0
dtheta = 2 * pi / numPoses
gRo = gtsam.Rot3(
np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F'))
for i in range(numPoses):
key = gtsam.symbol(symbolChar, i)
gti = gtsam.Point3(radius * cos(theta), radius * sin(theta), 0)
oRi = gtsam.Rot3.Yaw(
-theta) # negative yaw goes counterclockwise, with Z down !
gTi = gtsam.Pose3(gRo.compose(oRi), gti)
values.insert(key, gTi)
theta = theta + dtheta
return values
def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
self.K = K
self.cameras = [gtsam.Pose3()] * nrCameras
self.points = [gtsam.Point3()] * nrPoints
K = gtsam.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] = gtsam.Point3(r * cos(theta), r * sin(theta), 0)
else: # 3D landmarks as vertices of a cube
truth.points = [
gtsam.Point3(10, 10, 10), gtsam.Point3(-10, 10, 10),
gtsam.Point3(-10, -10, 10), gtsam.Point3(10, -10, 10),
gtsam.Point3(10, 10, -10), gtsam.Point3(-10, 10, -10),
gtsam.Point3(-10, -10, -10), gtsam.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 = gtsam.Point3(r * cos(theta), r * sin(theta), height)
truth.cameras[i] = gtsam.SimpleCamera.Lookat(t,
gtsam.Point3(),
gtsam.Point3(0, 0, 1),
truth.K)
# Create measurements
def createPoints():
# 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)]
return points
def createPoses(K):
# Create the set of ground-truth poses
radius = 30.0
angles = np.linspace(0, 2*np.pi, 8, endpoint=False)
up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0)
poses = []
for theta in angles:
position = gtsam.Point3(radius*np.cos(theta),
radius*np.sin(theta), 0.0)
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
poses.append(camera.pose())
return poses
def createPoints():
# 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)]
return points
def createPoses():
# Create the set of ground-truth poses
radius = 30.0
angles = np.linspace(0,2*np.pi,8,endpoint=False)
up = gtsam.Point3(0,0,1)
target = gtsam.Point3(0,0,0)
poses = []
for theta in angles:
position = gtsam.Point3(radius*np.cos(theta), radius*np.sin(theta), 0.0)
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up)
poses.append(camera.pose())
return poses