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

import python classes

release/4.3a0
Varun Agrawal 2020-03-22 12:32:16 -04:00
parent 8fdbf2fa6e
commit 87c338a18b
2 changed files with 44 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
cython/gtsam/examples/ImuFactorExample2.py
View File

@ -14,6 +14,11 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import gtsam import gtsam
import gtsam.utils.plot as gtsam_plot 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): def X(key):
@ -69,8 +74,8 @@ PARAMS.setUse2ndOrderCoriolis(False)
PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1) BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
gtsam.Point3(0.05, -0.10, 0.20)) Point3(0.05, -0.10, 0.20))
def IMU_example(): def IMU_example():
@ -78,10 +83,10 @@ def IMU_example():
# Start with a camera on x-axis looking at origin # Start with a camera on x-axis looking at origin
radius = 30 radius = 30
up = gtsam.Point3(0, 0, 1) up = Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0) target = Point3(0, 0, 0)
position = gtsam.Point3(radius, 0, 0) position = Point3(radius, 0, 0)
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, gtsam.Cal3_S2()) camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
pose_0 = camera.pose() pose_0 = camera.pose()
# Create the set of ground-truth landmarks and poses # Create the set of ground-truth landmarks and poses
@ -90,37 +95,37 @@ def IMU_example():
angular_velocity_vector = vector3(0, -angular_velocity, 0) angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = gtsam.ConstantTwistScenario( scenario = ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0) angular_velocity_vector, linear_velocity_vector, pose_0)
# Create a factor graph # Create a factor graph
newgraph = gtsam.NonlinearFactorGraph() newgraph = NonlinearFactorGraph()
# Create (incremental) ISAM2 solver # Create (incremental) ISAM2 solver
isam = gtsam.ISAM2() isam = ISAM2()
# Create the initial estimate to the solution # Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth # 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. # 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 # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noise = gtsam.noiseModel_Diagonal.Sigmas( noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) 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 # Add imu priors
biasKey = gtsam.symbol(ord('b'), 0) biasKey = gtsam.symbol(ord('b'), 0)
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
biasnoise) biasnoise)
newgraph.push_back(biasprior) newgraph.push_back(biasprior)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
# Calculate with correct initial velocity # Calculate with correct initial velocity
n_velocity = vector3(0, angular_velocity * radius, 0) 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) newgraph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity) initialEstimate.insert(V(0), n_velocity)
@ -141,7 +146,7 @@ def IMU_example():
# Add Bias variables periodically # Add Bias variables periodically
if i % 5 == 0: if i % 5 == 0:
biasKey += 1 biasKey += 1
factor = gtsam.BetweenFactorConstantBias( factor = BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
newgraph.add(factor) newgraph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
@ -154,8 +159,7 @@ def IMU_example():
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
# Add Imu Factor # Add Imu Factor
imufac = gtsam.ImuFactor( imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
newgraph.add(imufac) newgraph.add(imufac)
# insert new velocity, which is wrong # insert new velocity, which is wrong
@ -168,7 +172,7 @@ def IMU_example():
ISAM2_plot(result) ISAM2_plot(result)
# reset # reset
newgraph = gtsam.NonlinearFactorGraph() newgraph = NonlinearFactorGraph()
initialEstimate.clear() initialEstimate.clear()

43
cython/gtsam/utils/visual_data_generator.py
View File

@ -1,8 +1,9 @@
from __future__ import print_function from __future__ import print_function
import numpy as np import numpy as np
from math import pi, cos, sin
import gtsam import gtsam
from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3
class Options: class Options:
@ -10,7 +11,7 @@ class Options:
Options to generate test scenario 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 Options to generate test scenario
@param triangle: generate a triangle scene with 3 points if True, otherwise @param triangle: generate a triangle scene with 3 points if True, otherwise
@ -27,10 +28,10 @@ class GroundTruth:
Object holding generated ground-truth data 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.K = K
self.cameras = [gtsam.Pose3()] * nrCameras self.cameras = [Pose3()] * nrCameras
self.points = [gtsam.Point3()] * nrPoints self.points = [Point3()] * nrPoints
def print_(self, s=""): def print_(self, s=""):
print(s) print(s)
@ -52,11 +53,11 @@ class Data:
class NoiseModels: class NoiseModels:
pass 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.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.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
self.odometry = [gtsam.Pose3()] * nrCameras self.odometry = [Pose3()] * nrCameras
# Set Noise parameters # Set Noise parameters
self.noiseModels = Data.NoiseModels() self.noiseModels = Data.NoiseModels()
@ -73,7 +74,7 @@ class Data:
def generate_data(options): def generate_data(options):
""" Generate ground-truth and measurement data. """ """ 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 nrPoints = 3 if options.triangle else 8
truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints) truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints)
@ -83,26 +84,26 @@ def generate_data(options):
if options.triangle: # Create a triangle target, just 3 points on a plane if options.triangle: # Create a triangle target, just 3 points on a plane
r = 10 r = 10
for j in range(len(truth.points)): for j in range(len(truth.points)):
theta = j * 2 * pi / nrPoints theta = j * 2 * np.pi / nrPoints
truth.points[j] = gtsam.Point3(r * cos(theta), r * sin(theta), 0) truth.points[j] = Point3(r * np.cos(theta), r * np.sin(theta), 0)
else: # 3D landmarks as vertices of a cube else: # 3D landmarks as vertices of a cube
truth.points = [ truth.points = [
gtsam.Point3(10, 10, 10), gtsam.Point3(-10, 10, 10), Point3(10, 10, 10), Point3(-10, 10, 10),
gtsam.Point3(-10, -10, 10), gtsam.Point3(10, -10, 10), Point3(-10, -10, 10), Point3(10, -10, 10),
gtsam.Point3(10, 10, -10), gtsam.Point3(-10, 10, -10), Point3(10, 10, -10), Point3(-10, 10, -10),
gtsam.Point3(-10, -10, -10), gtsam.Point3(10, -10, -10) Point3(-10, -10, -10), Point3(10, -10, -10)
] ]
# Create camera cameras on a circle around the triangle # Create camera cameras on a circle around the triangle
height = 10 height = 10
r = 40 r = 40
for i in range(options.nrCameras): for i in range(options.nrCameras):
theta = i * 2 * pi / options.nrCameras theta = i * 2 * np.pi / options.nrCameras
t = gtsam.Point3(r * cos(theta), r * sin(theta), height) t = Point3(r * np.cos(theta), r * np.sin(theta), height)
truth.cameras[i] = gtsam.PinholeCameraCal3_S2.Lookat(t, truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
gtsam.Point3(), Point3(),
gtsam.Point3(0, 0, 1), Point3(0, 0, 1),
truth.K) truth.K)
# Create measurements # Create measurements
for j in range(nrPoints): for j in range(nrPoints):
# All landmarks seen in every frame # All landmarks seen in every frame