import python classes
Varun Agrawal
parent
8fdbf2fa6e
commit
87c338a18b
|
|
@ -14,6 +14,11 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
|
|||
|
||||
import gtsam
|
||||
import gtsam.utils.plot as gtsam_plot
|
||||
from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
|
||||
ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
|
||||
PinholeCameraCal3_S2, Point3, Pose3,
|
||||
PriorFactorConstantBias, PriorFactorPose3,
|
||||
PriorFactorVector, Rot3, Values)
|
||||
|
||||
|
||||
def X(key):
|
||||
|
|
@ -69,8 +74,8 @@ PARAMS.setUse2ndOrderCoriolis(False)
|
|||
PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
|
||||
|
||||
BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
|
||||
DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0),
|
||||
gtsam.Point3(0.05, -0.10, 0.20))
|
||||
DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
|
||||
Point3(0.05, -0.10, 0.20))
|
||||
|
||||
|
||||
def IMU_example():
|
||||
|
|
@ -78,10 +83,10 @@ def IMU_example():
|
|||
|
||||
# Start with a camera on x-axis looking at origin
|
||||
radius = 30
|
||||
up = gtsam.Point3(0, 0, 1)
|
||||
target = gtsam.Point3(0, 0, 0)
|
||||
position = gtsam.Point3(radius, 0, 0)
|
||||
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, gtsam.Cal3_S2())
|
||||
up = Point3(0, 0, 1)
|
||||
target = Point3(0, 0, 0)
|
||||
position = Point3(radius, 0, 0)
|
||||
camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
|
||||
pose_0 = camera.pose()
|
||||
|
||||
# Create the set of ground-truth landmarks and poses
|
||||
|
|
@ -90,29 +95,29 @@ def IMU_example():
|
|||
|
||||
angular_velocity_vector = vector3(0, -angular_velocity, 0)
|
||||
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
|
||||
scenario = gtsam.ConstantTwistScenario(
|
||||
scenario = ConstantTwistScenario(
|
||||
angular_velocity_vector, linear_velocity_vector, pose_0)
|
||||
|
||||
# Create a factor graph
|
||||
newgraph = gtsam.NonlinearFactorGraph()
|
||||
newgraph = NonlinearFactorGraph()
|
||||
|
||||
# Create (incremental) ISAM2 solver
|
||||
isam = gtsam.ISAM2()
|
||||
isam = ISAM2()
|
||||
|
||||
# Create the initial estimate to the solution
|
||||
# Intentionally initialize the variables off from the ground truth
|
||||
initialEstimate = gtsam.Values()
|
||||
initialEstimate = Values()
|
||||
|
||||
# Add a prior on pose x0. This indirectly specifies where the origin is.
|
||||
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
noise = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
|
||||
newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
|
||||
newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise))
|
||||
|
||||
# Add imu priors
|
||||
biasKey = gtsam.symbol(ord('b'), 0)
|
||||
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
|
||||
biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
|
||||
biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
|
||||
biasnoise)
|
||||
newgraph.push_back(biasprior)
|
||||
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
|
||||
|
|
@ -120,7 +125,7 @@ def IMU_example():
|
|||
|
||||
# Calculate with correct initial velocity
|
||||
n_velocity = vector3(0, angular_velocity * radius, 0)
|
||||
velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise)
|
||||
velprior = PriorFactorVector(V(0), n_velocity, velnoise)
|
||||
newgraph.push_back(velprior)
|
||||
initialEstimate.insert(V(0), n_velocity)
|
||||
|
||||
|
|
@ -141,7 +146,7 @@ def IMU_example():
|
|||
# Add Bias variables periodically
|
||||
if i % 5 == 0:
|
||||
biasKey += 1
|
||||
factor = gtsam.BetweenFactorConstantBias(
|
||||
factor = BetweenFactorConstantBias(
|
||||
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
|
||||
newgraph.add(factor)
|
||||
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
|
||||
|
|
@ -154,8 +159,7 @@ def IMU_example():
|
|||
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
|
||||
|
||||
# Add Imu Factor
|
||||
imufac = gtsam.ImuFactor(
|
||||
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
|
||||
imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
|
||||
newgraph.add(imufac)
|
||||
|
||||
# insert new velocity, which is wrong
|
||||
|
|
@ -168,7 +172,7 @@ def IMU_example():
|
|||
ISAM2_plot(result)
|
||||
|
||||
# reset
|
||||
newgraph = gtsam.NonlinearFactorGraph()
|
||||
newgraph = NonlinearFactorGraph()
|
||||
initialEstimate.clear()
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,8 +1,9 @@
|
|||
from __future__ import print_function
|
||||
|
||||
import numpy as np
|
||||
from math import pi, cos, sin
|
||||
|
||||
import gtsam
|
||||
from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3
|
||||
|
||||
|
||||
class Options:
|
||||
|
|
@ -10,7 +11,7 @@ class Options:
|
|||
Options to generate test scenario
|
||||
"""
|
||||
|
||||
def __init__(self, triangle=False, nrCameras=3, K=gtsam.Cal3_S2()):
|
||||
def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()):
|
||||
"""
|
||||
Options to generate test scenario
|
||||
@param triangle: generate a triangle scene with 3 points if True, otherwise
|
||||
|
|
@ -27,10 +28,10 @@ class GroundTruth:
|
|||
Object holding generated ground-truth data
|
||||
"""
|
||||
|
||||
def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
|
||||
def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
|
||||
self.K = K
|
||||
self.cameras = [gtsam.Pose3()] * nrCameras
|
||||
self.points = [gtsam.Point3()] * nrPoints
|
||||
self.cameras = [Pose3()] * nrCameras
|
||||
self.points = [Point3()] * nrPoints
|
||||
|
||||
def print_(self, s=""):
|
||||
print(s)
|
||||
|
|
@ -52,11 +53,11 @@ class Data:
|
|||
class NoiseModels:
|
||||
pass
|
||||
|
||||
def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
|
||||
def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
|
||||
self.K = K
|
||||
self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras]
|
||||
self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras]
|
||||
self.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
|
||||
self.odometry = [gtsam.Pose3()] * nrCameras
|
||||
self.odometry = [Pose3()] * nrCameras
|
||||
|
||||
# Set Noise parameters
|
||||
self.noiseModels = Data.NoiseModels()
|
||||
|
|
@ -73,7 +74,7 @@ class Data:
|
|||
def generate_data(options):
|
||||
""" Generate ground-truth and measurement data. """
|
||||
|
||||
K = gtsam.Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
|
||||
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)
|
||||
|
|
@ -83,25 +84,25 @@ def generate_data(options):
|
|||
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)
|
||||
theta = j * 2 * np.pi / nrPoints
|
||||
truth.points[j] = Point3(r * np.cos(theta), r * np.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)
|
||||
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 = gtsam.Point3(r * cos(theta), r * sin(theta), height)
|
||||
truth.cameras[i] = gtsam.PinholeCameraCal3_S2.Lookat(t,
|
||||
gtsam.Point3(),
|
||||
gtsam.Point3(0, 0, 1),
|
||||
theta = i * 2 * np.pi / options.nrCameras
|
||||
t = Point3(r * np.cos(theta), r * np.sin(theta), height)
|
||||
truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
|
||||
Point3(),
|
||||
Point3(0, 0, 1),
|
||||
truth.K)
|
||||
# Create measurements
|
||||
for j in range(nrPoints):
|
||||
|
|
|
|||
Loading…
Reference in New Issue