Port all matlab tests to python. All passed.
Duy-Nguyen Ta
parent
9fe804bc1a
commit
a4f28bc7fb
|
|
@ -0,0 +1 @@
|
|||
from .np_utils import *
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
from gtsam import *
|
||||
from math import *
|
||||
from np_utils import *
|
||||
|
||||
def circlePose3(numPoses = 8, radius = 1.0, symbolChar = 0):
|
||||
"""
|
||||
circlePose3 generates a set of poses in a circle. This function
|
||||
returns those poses inside a gtsam.Values object, with sequential
|
||||
keys starting from 0. An optional character may be provided, which
|
||||
will be stored in the msb of each key (i.e. gtsam.Symbol).
|
||||
|
||||
We use aerospace/navlab convention, X forward, Y right, Z down
|
||||
First pose will be at (R,0,0)
|
||||
^y ^ X
|
||||
| |
|
||||
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(R = Matrix([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]]))
|
||||
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.insertPose3(key, gTi)
|
||||
theta = theta + dtheta
|
||||
return values
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
import numpy as np
|
||||
|
||||
def Vector(list1d): return np.array(list1d, dtype = 'float')
|
||||
def Matrix(list2d): return np.array(list2d, dtype = 'float', order = 'F')
|
||||
|
|
@ -0,0 +1,118 @@
|
|||
from np_utils import *
|
||||
from math import *
|
||||
from gtsam import *
|
||||
|
||||
|
||||
class Options:
|
||||
"""
|
||||
Options to generate test scenario
|
||||
"""
|
||||
|
||||
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
|
||||
a cube with 8 points
|
||||
@param nrCameras: number of cameras to generate
|
||||
@param K: camera calibration object
|
||||
"""
|
||||
self.triangle = triangle
|
||||
self.nrCameras = nrCameras
|
||||
|
||||
|
||||
class GroundTruth:
|
||||
"""
|
||||
Object holding generated ground-truth data
|
||||
"""
|
||||
|
||||
def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
|
||||
self.K = K
|
||||
self.cameras = [Pose3()] * nrCameras
|
||||
self.points = [Point3()] * nrPoints
|
||||
|
||||
def print_(self, s=""):
|
||||
print s
|
||||
print "K = ", self.K
|
||||
print "Cameras: ", len(self.cameras)
|
||||
for camera in self.cameras:
|
||||
print "\t", camera
|
||||
print "Points: ", len(self.points)
|
||||
for point in self.points:
|
||||
print "\t", point
|
||||
pass
|
||||
|
||||
|
||||
class Data:
|
||||
"""
|
||||
Object holding generated measurement data
|
||||
"""
|
||||
|
||||
class NoiseModels:
|
||||
pass
|
||||
|
||||
def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
|
||||
self.K = K
|
||||
self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras]
|
||||
self.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
|
||||
self.odometry = [Pose3()] * nrCameras
|
||||
|
||||
# Set Noise parameters
|
||||
self.noiseModels = Data.NoiseModels()
|
||||
self.noiseModels.posePrior = noiseModel_Diagonal.Sigmas(
|
||||
Vector([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]))
|
||||
# noiseModels.odometry = noiseModel_Diagonal.Sigmas(
|
||||
# Vector([0.001,0.001,0.001,0.1,0.1,0.1]))
|
||||
self.noiseModels.odometry = noiseModel_Diagonal.Sigmas(
|
||||
Vector([0.05, 0.05, 0.05, 0.2, 0.2, 0.2]))
|
||||
self.noiseModels.pointPrior = noiseModel_Isotropic.Sigma(3, 0.1)
|
||||
self.noiseModels.measurement = noiseModel_Isotropic.Sigma(2, 1.0)
|
||||
|
||||
|
||||
def generate_data(options):
|
||||
"""
|
||||
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(
|
||||
v=Vector([r * cos(theta), r * sin(theta), 0]))
|
||||
else: # 3D landmarks as vertices of a cube
|
||||
truth.points = [Point3(v=Vector([10, 10, 10])),
|
||||
Point3(v=Vector([-10, 10, 10])),
|
||||
Point3(v=Vector([-10, -10, 10])),
|
||||
Point3(v=Vector([10, -10, 10])),
|
||||
Point3(v=Vector([10, 10, -10])),
|
||||
Point3(v=Vector([-10, 10, -10])),
|
||||
Point3(v=Vector([-10, -10, -10])),
|
||||
Point3(v=Vector([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(v=Vector([r * cos(theta), r * sin(theta), height]))
|
||||
truth.cameras[i] = SimpleCamera.Lookat(
|
||||
t, Point3(), Point3(v=Vector([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
|
||||
|
|
@ -0,0 +1,125 @@
|
|||
from np_utils import *
|
||||
from math import *
|
||||
from gtsam import *
|
||||
|
||||
|
||||
class Options:
|
||||
"""
|
||||
Options for visual isam example
|
||||
"""
|
||||
def __init__(self):
|
||||
self.hardConstraint = False
|
||||
self.pointPriors = False
|
||||
self.batchInitialization = True
|
||||
self.reorderInterval = 10
|
||||
self.alwaysRelinearize = False
|
||||
|
||||
|
||||
def initialize(data, truth, options):
|
||||
# Initialize iSAM
|
||||
params = gtsam.ISAM2Params()
|
||||
if options.alwaysRelinearize:
|
||||
params.setRelinearizeSkip(1)
|
||||
isam = ISAM2(params = params)
|
||||
|
||||
# Add constraints/priors
|
||||
# TODO: should not be from ground truth!
|
||||
newFactors = NonlinearFactorGraph()
|
||||
initialEstimates = Values()
|
||||
for i in range(2):
|
||||
ii = symbol(ord('x'), i)
|
||||
if i == 0:
|
||||
if options.hardConstraint: # add hard constraint
|
||||
newFactors.add(NonlinearEqualityPose3(
|
||||
ii, truth.cameras[0].pose()))
|
||||
else:
|
||||
newFactors.add(PriorFactorPose3(
|
||||
ii, truth.cameras[i].pose(), data.noiseModels.posePrior))
|
||||
initialEstimates.insertPose3(ii, truth.cameras[i].pose())
|
||||
|
||||
nextPoseIndex = 2
|
||||
|
||||
# Add visual measurement factors from two first poses and initialize
|
||||
# observed landmarks
|
||||
for i in range(2):
|
||||
ii = symbol(ord('x'), i)
|
||||
for k in range(len(data.Z[i])):
|
||||
j = data.J[i][k]
|
||||
jj = symbol(ord('l'), j)
|
||||
newFactors.add(GenericProjectionFactorCal3_S2(
|
||||
data.Z[i][k], data.noiseModels.measurement, ii, jj, data.K))
|
||||
# TODO: initial estimates should not be from ground truth!
|
||||
if not initialEstimates.exists(jj):
|
||||
initialEstimates.insertPoint3(jj, truth.points[j])
|
||||
if options.pointPriors: # add point priors
|
||||
newFactors.add(PriorFactorPoint3(
|
||||
jj, truth.points[j], data.noiseModels.pointPrior))
|
||||
|
||||
# Add odometry between frames 0 and 1
|
||||
newFactors.add(BetweenFactorPose3(symbol(ord('x'), 0), symbol(
|
||||
ord('x'), 1), data.odometry[1], data.noiseModels.odometry))
|
||||
|
||||
# Update ISAM
|
||||
if options.batchInitialization: # Do a full optimize for first two poses
|
||||
batchOptimizer = LevenbergMarquardtOptimizer(
|
||||
newFactors, initialEstimates)
|
||||
fullyOptimized = batchOptimizer.optimize()
|
||||
isam.update(newFactors, fullyOptimized)
|
||||
else:
|
||||
isam.update(newFactors, initialEstimates)
|
||||
|
||||
# figure(1)tic
|
||||
# t=toc plot(frame_i,t,'r.') tic
|
||||
result = isam.calculateEstimate()
|
||||
# t=toc plot(frame_i,t,'g.')
|
||||
|
||||
return isam, result, nextPoseIndex
|
||||
|
||||
|
||||
def step(data, isam, result, truth, currPoseIndex):
|
||||
"""
|
||||
Do one step isam update
|
||||
@param[in] data: measurement data (odometry and visual measurements and their noiseModels)
|
||||
@param[in/out] isam: current isam object, will be updated
|
||||
@param[in/out] result: current result object, will be updated
|
||||
@param[in] truth: ground truth data, used to initialize new variables
|
||||
@param[currPoseIndex]: index of the current pose
|
||||
"""
|
||||
# iSAM expects us to give it a new set of factors
|
||||
# along with initial estimates for any new variables introduced.
|
||||
newFactors = NonlinearFactorGraph()
|
||||
initialEstimates = Values()
|
||||
|
||||
# Add odometry
|
||||
prevPoseIndex = currPoseIndex - 1
|
||||
odometry = data.odometry[prevPoseIndex]
|
||||
newFactors.add(BetweenFactorPose3(symbol(ord('x'), prevPoseIndex),
|
||||
symbol(ord('x'), currPoseIndex),
|
||||
odometry, data.noiseModels.odometry))
|
||||
|
||||
# Add visual measurement factors and initializations as necessary
|
||||
for k in range(len(data.Z[currPoseIndex])):
|
||||
zij = data.Z[currPoseIndex][k]
|
||||
j = data.J[currPoseIndex][k]
|
||||
jj = symbol(ord('l'), j)
|
||||
newFactors.add(GenericProjectionFactorCal3_S2(
|
||||
zij, data.noiseModels.measurement,
|
||||
symbol(ord('x'), currPoseIndex), jj, data.K))
|
||||
# TODO: initialize with something other than truth
|
||||
if not result.exists(jj) and not initialEstimates.exists(jj):
|
||||
lmInit = truth.points[j]
|
||||
initialEstimates.insert(jj, lmInit)
|
||||
|
||||
# Initial estimates for the new pose.
|
||||
prevPose = result.atPose3(symbol(ord('x'), prevPoseIndex))
|
||||
initialEstimates.insertPose3(symbol(ord('x'), currPoseIndex),
|
||||
prevPose.compose(odometry))
|
||||
|
||||
# Update ISAM
|
||||
# figure(1)tic
|
||||
isam.update(newFactors, initialEstimates)
|
||||
# t=toc plot(frame_i,t,'r.') tic
|
||||
newResult = isam.calculateEstimate()
|
||||
# t=toc plot(frame_i,t,'g.')
|
||||
|
||||
return isam, newResult
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
|
||||
|
||||
class TestCal3Unified(unittest.TestCase):
|
||||
|
||||
def test_Cal3Unified(self):
|
||||
K = Cal3Unified()
|
||||
self.assertEqual(K.fx(), 1.)
|
||||
self.assertEqual(K.fx(), 1.)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,78 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
from gtsam_utils import Matrix, Vector
|
||||
|
||||
class TestJacobianFactor(unittest.TestCase):
|
||||
|
||||
def test_eliminate(self):
|
||||
Ax2 = Matrix([
|
||||
[-5., 0.],
|
||||
[+0., -5.],
|
||||
[10., 0.],
|
||||
[+0., 10.]])
|
||||
|
||||
Al1 = Matrix([
|
||||
[5., 0.],
|
||||
[0., 5.],
|
||||
[0., 0.],
|
||||
[0., 0.]])
|
||||
|
||||
Ax1 = Matrix([
|
||||
[0.00, 0.], # f4
|
||||
[0.00, 0.], # f4
|
||||
[-10., 0.], # f2
|
||||
[0.00, -10.]]) # f2
|
||||
|
||||
x2 = 1
|
||||
l1 = 2
|
||||
x1 = 3
|
||||
|
||||
# the RHS
|
||||
b2 = Vector([-1., 1.5, 2., -1.])
|
||||
sigmas = Vector([1., 1., 1., 1.])
|
||||
model4 = noiseModel_Diagonal.Sigmas(sigmas)
|
||||
combined = JacobianFactor(x2, np.transpose(
|
||||
Ax2), l1, Al1, x1, Ax1, b2, model4)
|
||||
|
||||
# eliminate the first variable (x2) in the combined factor, destructive
|
||||
# !
|
||||
ord = Ordering()
|
||||
ord.push_back(x2)
|
||||
actualCG, lf = combined.eliminate(ord)
|
||||
|
||||
# create expected Conditional Gaussian
|
||||
R11 = Matrix([[11.1803, 0.00],
|
||||
[0.00, 11.1803]])
|
||||
S12 = Matrix([[-2.23607, 0.00],
|
||||
[+0.00, -2.23607] ])
|
||||
S13 = Matrix([[-8.94427, 0.00],
|
||||
[+0.00, -8.94427]])
|
||||
d = Vector([2.23607, -1.56525])
|
||||
expectedCG = GaussianConditional(
|
||||
x2, d, R11, l1, S12, x1, S13, noiseModel_Unit.Create(2))
|
||||
# check if the result matches
|
||||
self.assertTrue(actualCG.equals(expectedCG, 1e-4))
|
||||
|
||||
# the expected linear factor
|
||||
Bl1 = Matrix([
|
||||
[4.47214, 0.00],
|
||||
[0.00, 4.47214]])
|
||||
|
||||
Bx1 = Matrix([
|
||||
# x1
|
||||
[-4.47214, 0.00],
|
||||
[+0.00, -4.47214]])
|
||||
|
||||
# the RHS
|
||||
b1 = Vector([0.0, 0.894427])
|
||||
|
||||
model2 = noiseModel_Diagonal.Sigmas(np.array([1., 1.]))
|
||||
expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2)
|
||||
|
||||
# check if the result matches the combined (reduced) factor
|
||||
self.assertTrue(lf.equals(expectedLF, 1e-4))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,70 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
|
||||
class TestKalmanFilter(unittest.TestCase):
|
||||
|
||||
def test_KalmanFilter(self):
|
||||
F = np.eye(2)
|
||||
B = np.eye(2)
|
||||
u = np.array([1.0, 0.0])
|
||||
modelQ = noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1]))
|
||||
Q = 0.01 * np.eye(2)
|
||||
H = np.eye(2)
|
||||
z1 = np.array([1.0, 0.0])
|
||||
z2 = np.array([2.0, 0.0])
|
||||
z3 = np.array([3.0, 0.0])
|
||||
modelR = noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1]))
|
||||
R = 0.01 * np.eye(2)
|
||||
|
||||
# Create the set of expected output TestValues
|
||||
expected0 = np.array([0.0, 0.0])
|
||||
P00 = 0.01 * np.eye(2)
|
||||
|
||||
expected1 = np.array([1.0, 0.0])
|
||||
P01 = P00 + Q
|
||||
I11 = np.linalg.inv(P01) + np.linalg.inv(R)
|
||||
|
||||
expected2 = np.array([2.0, 0.0])
|
||||
P12 = np.linalg.inv(I11) + Q
|
||||
I22 = np.linalg.inv(P12) + np.linalg.inv(R)
|
||||
|
||||
expected3 = np.array([3.0, 0.0])
|
||||
P23 = np.linalg.inv(I22) + Q
|
||||
I33 = np.linalg.inv(P23) + np.linalg.inv(R)
|
||||
|
||||
# Create an KalmanFilter object
|
||||
KF = KalmanFilter(n=2)
|
||||
|
||||
# Create the Kalman Filter initialization point
|
||||
x_initial = np.array([0.0, 0.0])
|
||||
P_initial = 0.01 * np.eye(2)
|
||||
|
||||
# Create an KF object
|
||||
state = KF.init(x_initial, P_initial)
|
||||
self.assertTrue(np.allclose(expected0, state.mean()))
|
||||
self.assertTrue(np.allclose(P00, state.covariance()))
|
||||
|
||||
# Run iteration 1
|
||||
state = KF.predict(state, F, B, u, modelQ)
|
||||
self.assertTrue(np.allclose(expected1, state.mean().ravel()))
|
||||
self.assertTrue(np.allclose(P01, state.covariance()))
|
||||
state = KF.update(state, H, z1, modelR)
|
||||
self.assertTrue(np.allclose(expected1, state.mean().ravel()))
|
||||
self.assertTrue(np.allclose(I11, state.information()))
|
||||
|
||||
# Run iteration 2
|
||||
state = KF.predict(state, F, B, u, modelQ)
|
||||
self.assertTrue(np.allclose(expected2, state.mean().ravel()))
|
||||
state = KF.update(state, H, z2, modelR)
|
||||
self.assertTrue(np.allclose(expected2, state.mean().ravel()))
|
||||
|
||||
# Run iteration 3
|
||||
state = KF.predict(state, F, B, u, modelQ)
|
||||
self.assertTrue(np.allclose(expected3, state.mean().ravel()))
|
||||
state = KF.update(state, H, z3, modelR)
|
||||
self.assertTrue(np.allclose(expected3, state.mean().ravel()))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,51 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
|
||||
class TestLocalizationExample(unittest.TestCase):
|
||||
|
||||
def test_LocalizationExample(self):
|
||||
# Create the graph (defined in pose2SLAM.h, derived from
|
||||
# NonlinearFactorGraph)
|
||||
graph = NonlinearFactorGraph()
|
||||
|
||||
# Add two odometry factors
|
||||
# create a measurement for both factors (the same in this case)
|
||||
odometry = Pose2(2.0, 0.0, 0.0)
|
||||
odometryNoise = noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta
|
||||
graph.add(BetweenFactorPose2(0, 1, odometry, odometryNoise))
|
||||
graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise))
|
||||
|
||||
# Add three "GPS" measurements
|
||||
# We use Pose2 Priors here with high variance on theta
|
||||
groundTruth = Values()
|
||||
groundTruth.insertPose2(0, Pose2(0.0, 0.0, 0.0))
|
||||
groundTruth.insertPose2(1, Pose2(2.0, 0.0, 0.0))
|
||||
groundTruth.insertPose2(2, Pose2(4.0, 0.0, 0.0))
|
||||
model = noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 10.]))
|
||||
for i in range(3):
|
||||
graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model))
|
||||
|
||||
# Initialize to noisy points
|
||||
initialEstimate = Values()
|
||||
initialEstimate.insertPose2(0, Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insertPose2(1, Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insertPose2(2, Pose2(4.1, 0.1, 0.1))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimizeSafely()
|
||||
|
||||
# Plot Covariance Ellipses
|
||||
marginals = Marginals(graph, result)
|
||||
P = [None] * result.size()
|
||||
for i in range(0, result.size()):
|
||||
pose_i = result.atPose2(i)
|
||||
self.assertTrue(pose_i.equals(groundTruth.atPose2(i), 1e-4))
|
||||
P[i] = marginals.marginalCovariance(i)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
|
||||
class TestOdometryExample(unittest.TestCase):
|
||||
|
||||
def test_OdometryExample(self):
|
||||
# Create the graph (defined in pose2SLAM.h, derived from
|
||||
# NonlinearFactorGraph)
|
||||
graph = NonlinearFactorGraph()
|
||||
|
||||
# Add a Gaussian prior on pose x_1
|
||||
priorMean = Pose2(0.0, 0.0, 0.0) # prior mean is at origin
|
||||
priorNoise = noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.3, 0.3, 0.1])) # 30cm std on x,y, 0.1 rad on theta
|
||||
# add directly to graph
|
||||
graph.add(PriorFactorPose2(1, priorMean, priorNoise))
|
||||
|
||||
# Add two odometry factors
|
||||
# create a measurement for both factors (the same in this case)
|
||||
odometry = Pose2(2.0, 0.0, 0.0)
|
||||
odometryNoise = noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta
|
||||
graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise))
|
||||
graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise))
|
||||
|
||||
# Initialize to noisy points
|
||||
initialEstimate = Values()
|
||||
initialEstimate.insertPose2(1, Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insertPose2(2, Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insertPose2(3, Pose2(4.1, 0.1, 0.1))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimizeSafely()
|
||||
marginals = Marginals(graph, result)
|
||||
marginals.marginalCovariance(1)
|
||||
|
||||
# Check first pose equality
|
||||
pose_1 = result.atPose2(1)
|
||||
self.assertTrue(pose_1.equals(Pose2(), 1e-4))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,64 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
|
||||
class TestPose2SLAMExample(unittest.TestCase):
|
||||
|
||||
def test_Pose2SLAMExample(self):
|
||||
# Assumptions
|
||||
# - All values are axis aligned
|
||||
# - Robot poses are facing along the X axis (horizontal, to the right in images)
|
||||
# - We have full odometry for measurements
|
||||
# - The robot is on a grid, moving 2 meters each step
|
||||
|
||||
# Create graph container and add factors to it
|
||||
graph = NonlinearFactorGraph()
|
||||
|
||||
# Add prior
|
||||
# gaussian for prior
|
||||
priorMean = Pose2(0.0, 0.0, 0.0) # prior at origin
|
||||
priorNoise = noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
# add directly to graph
|
||||
graph.add(PriorFactorPose2(1, priorMean, priorNoise))
|
||||
|
||||
# Add odometry
|
||||
# general noisemodel for odometry
|
||||
odometryNoise = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
graph.add(BetweenFactorPose2(
|
||||
1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise))
|
||||
graph.add(BetweenFactorPose2(
|
||||
2, 3, Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
graph.add(BetweenFactorPose2(
|
||||
3, 4, Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
graph.add(BetweenFactorPose2(
|
||||
4, 5, Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
|
||||
# Add pose constraint
|
||||
model = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi / 2), model))
|
||||
|
||||
# Initialize to noisy points
|
||||
initialEstimate = Values()
|
||||
initialEstimate.insertPose2(1, Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insertPose2(2, Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insertPose2(3, Pose2(4.1, 0.1, pi / 2))
|
||||
initialEstimate.insertPose2(4, Pose2(4.0, 2.0, pi))
|
||||
initialEstimate.insertPose2(5, Pose2(2.1, 2.1, -pi / 2))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimizeSafely()
|
||||
|
||||
# Plot Covariance Ellipses
|
||||
marginals = Marginals(graph, result)
|
||||
P = marginals.marginalCovariance(1)
|
||||
|
||||
pose_1 = result.atPose2(1)
|
||||
self.assertTrue(pose_1.equals(Pose2(), 1e-4))
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
|
||||
class TestPose2SLAMExample(unittest.TestCase):
|
||||
|
||||
def test_Pose2SLAMExample(self):
|
||||
# Assumptions
|
||||
# - All values are axis aligned
|
||||
# - Robot poses are facing along the X axis (horizontal, to the right in images)
|
||||
# - We have full odometry for measurements
|
||||
# - The robot is on a grid, moving 2 meters each step
|
||||
|
||||
# Create graph container and add factors to it
|
||||
graph = NonlinearFactorGraph()
|
||||
|
||||
# Add prior
|
||||
# gaussian for prior
|
||||
priorMean = Pose2(0.0, 0.0, 0.0) # prior at origin
|
||||
priorNoise = noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
# add directly to graph
|
||||
graph.add(PriorFactorPose2(1, priorMean, priorNoise))
|
||||
|
||||
# Add odometry
|
||||
# general noisemodel for odometry
|
||||
odometryNoise = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
graph.add(BetweenFactorPose2(
|
||||
1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise))
|
||||
graph.add(BetweenFactorPose2(
|
||||
2, 3, Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
graph.add(BetweenFactorPose2(
|
||||
3, 4, Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
graph.add(BetweenFactorPose2(
|
||||
4, 5, Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
|
||||
# Add pose constraint
|
||||
model = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi / 2), model))
|
||||
|
||||
# Initialize to noisy points
|
||||
initialEstimate = Values()
|
||||
initialEstimate.insertPose2(1, Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insertPose2(2, Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insertPose2(3, Pose2(4.1, 0.1, pi / 2))
|
||||
initialEstimate.insertPose2(4, Pose2(4.0, 2.0, pi))
|
||||
initialEstimate.insertPose2(5, Pose2(2.1, 2.1, -pi / 2))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimizeSafely()
|
||||
|
||||
# Plot Covariance Ellipses
|
||||
marginals = Marginals(graph, result)
|
||||
P = marginals.marginalCovariance(1)
|
||||
|
||||
pose_1 = result.atPose2(1)
|
||||
self.assertTrue(pose_1.equals(Pose2(), 1e-4))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
from gtsam_utils.circlePose3 import *
|
||||
|
||||
class TestPose3SLAMExample(unittest.TestCase):
|
||||
|
||||
def test_Pose3SLAMExample(self):
|
||||
# Create a hexagon of poses
|
||||
hexagon = circlePose3(6, 1.0)
|
||||
p0 = hexagon.atPose3(0)
|
||||
p1 = hexagon.atPose3(1)
|
||||
|
||||
# create a Pose graph with one equality constraint and one measurement
|
||||
fg = NonlinearFactorGraph()
|
||||
fg.add(NonlinearEqualityPose3(0, p0))
|
||||
delta = p0.between(p1)
|
||||
covariance = noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.05, 0.05, 0.05, 5. * pi / 180, 5. * pi / 180, 5. * pi / 180]))
|
||||
fg.add(BetweenFactorPose3(0, 1, delta, covariance))
|
||||
fg.add(BetweenFactorPose3(1, 2, delta, covariance))
|
||||
fg.add(BetweenFactorPose3(2, 3, delta, covariance))
|
||||
fg.add(BetweenFactorPose3(3, 4, delta, covariance))
|
||||
fg.add(BetweenFactorPose3(4, 5, delta, covariance))
|
||||
fg.add(BetweenFactorPose3(5, 0, delta, covariance))
|
||||
|
||||
# Create initial config
|
||||
initial = Values()
|
||||
s = 0.10
|
||||
initial.insertPose3(0, p0)
|
||||
initial.insertPose3(1, hexagon.atPose3(1).retract(s * np.random.randn(6, 1)))
|
||||
initial.insertPose3(2, hexagon.atPose3(2).retract(s * np.random.randn(6, 1)))
|
||||
initial.insertPose3(3, hexagon.atPose3(3).retract(s * np.random.randn(6, 1)))
|
||||
initial.insertPose3(4, hexagon.atPose3(4).retract(s * np.random.randn(6, 1)))
|
||||
initial.insertPose3(5, hexagon.atPose3(5).retract(s * np.random.randn(6, 1)))
|
||||
|
||||
# optimize
|
||||
optimizer = LevenbergMarquardtOptimizer(fg, initial)
|
||||
result = optimizer.optimizeSafely()
|
||||
|
||||
pose_1 = result.atPose3(1)
|
||||
self.assertTrue(pose_1.equals(p1, 1e-4))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
|
||||
class TestPriorFactor(unittest.TestCase):
|
||||
|
||||
def test_PriorFactor(self):
|
||||
values = Values()
|
||||
|
||||
key = 5
|
||||
priorPose3 = Pose3()
|
||||
model = noiseModel_Unit.Create(6)
|
||||
factor = PriorFactorPose3(key, priorPose3, model)
|
||||
values.insertPose3(key, priorPose3)
|
||||
self.assertEqual(factor.error(values), 0)
|
||||
|
||||
key = 3
|
||||
priorVector = np.array([0., 0., 0.])
|
||||
model = noiseModel_Unit.Create(3)
|
||||
factor = PriorFactorVector(key, priorVector, model)
|
||||
values.insertVector(key, priorVector)
|
||||
self.assertEqual(factor.error(values), 0)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,70 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
from gtsam_utils import Vector, Matrix
|
||||
import gtsam_utils.visual_data_generator as generator
|
||||
|
||||
|
||||
class TestSFMExample(unittest.TestCase):
|
||||
|
||||
def test_SFMExample(self):
|
||||
options = generator.Options()
|
||||
options.triangle = False
|
||||
options.nrCameras = 10
|
||||
|
||||
[data, truth] = generator.generate_data(options)
|
||||
|
||||
measurementNoiseSigma = 1.0
|
||||
pointNoiseSigma = 0.1
|
||||
poseNoiseSigmas = Vector([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])
|
||||
|
||||
graph = NonlinearFactorGraph()
|
||||
|
||||
# Add factors for all measurements
|
||||
measurementNoise = noiseModel_Isotropic.Sigma(2, measurementNoiseSigma)
|
||||
for i in range(len(data.Z)):
|
||||
for k in range(len(data.Z[i])):
|
||||
j = data.J[i][k]
|
||||
graph.add(GenericProjectionFactorCal3_S2(
|
||||
data.Z[i][k], measurementNoise,
|
||||
symbol(ord('x'), i), symbol(ord('p'), j), data.K))
|
||||
|
||||
posePriorNoise = noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
|
||||
graph.add(PriorFactorPose3(symbol(ord('x'), 0),
|
||||
truth.cameras[0].pose(), posePriorNoise))
|
||||
pointPriorNoise = noiseModel_Isotropic.Sigma(3, pointNoiseSigma)
|
||||
graph.add(PriorFactorPoint3(symbol(ord('p'), 0),
|
||||
truth.points[0], pointPriorNoise))
|
||||
|
||||
# Initial estimate
|
||||
initialEstimate = Values()
|
||||
for i in range(len(truth.cameras)):
|
||||
pose_i = truth.cameras[i].pose()
|
||||
initialEstimate.insertPose3(symbol(ord('x'), i), pose_i)
|
||||
for j in range(len(truth.points)):
|
||||
point_j = truth.points[j]
|
||||
initialEstimate.insertPoint3(symbol(ord('p'), j), point_j)
|
||||
|
||||
# Optimization
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
for i in range(5):
|
||||
optimizer.iterate()
|
||||
result = optimizer.values()
|
||||
|
||||
# Marginalization
|
||||
marginals = Marginals(graph, result)
|
||||
marginals.marginalCovariance(symbol(ord('p'), 0))
|
||||
marginals.marginalCovariance(symbol(ord('x'), 0))
|
||||
|
||||
# Check optimized results, should be equal to ground truth
|
||||
for i in range(len(truth.cameras)):
|
||||
pose_i = result.atPose3(symbol(ord('x'), i))
|
||||
self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5))
|
||||
|
||||
for j in range(len(truth.points)):
|
||||
point_j = result.atPoint3(symbol(ord('p'), j))
|
||||
self.assertTrue(point_j.equals(truth.points[j], 1e-5))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,70 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
from gtsam_utils import Vector, Matrix
|
||||
|
||||
|
||||
class TestStereoVOExample(unittest.TestCase):
|
||||
|
||||
def test_StereoVOExample(self):
|
||||
## Assumptions
|
||||
# - For simplicity this example is in the camera's coordinate frame
|
||||
# - X: right, Y: down, Z: forward
|
||||
# - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis)
|
||||
# - x1 is fixed with a constraint, x2 is initialized with noisy values
|
||||
# - No noise on measurements
|
||||
|
||||
## Create keys for variables
|
||||
x1 = symbol(ord('x'),1)
|
||||
x2 = symbol(ord('x'),2)
|
||||
l1 = symbol(ord('l'),1)
|
||||
l2 = symbol(ord('l'),2)
|
||||
l3 = symbol(ord('l'),3)
|
||||
|
||||
## Create graph container and add factors to it
|
||||
graph = NonlinearFactorGraph()
|
||||
|
||||
## add a constraint on the starting pose
|
||||
first_pose = Pose3()
|
||||
graph.add(NonlinearEqualityPose3(x1, first_pose))
|
||||
|
||||
## Create realistic calibration and measurement noise model
|
||||
# format: fx fy skew cx cy baseline
|
||||
K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
|
||||
stereo_model = noiseModel_Diagonal.Sigmas(Vector([1.0, 1.0, 1.0]))
|
||||
|
||||
## Add measurements
|
||||
# pose 1
|
||||
graph.add(GenericStereoFactor3D(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K))
|
||||
graph.add(GenericStereoFactor3D(StereoPoint2(120, 80, 440), stereo_model, x1, l2, K))
|
||||
graph.add(GenericStereoFactor3D(StereoPoint2(320, 280, 140), stereo_model, x1, l3, K))
|
||||
|
||||
#pose 2
|
||||
graph.add(GenericStereoFactor3D(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K))
|
||||
graph.add(GenericStereoFactor3D(StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K))
|
||||
graph.add(GenericStereoFactor3D(StereoPoint2(320, 270, 115), stereo_model, x2, l3, K))
|
||||
|
||||
## Create initial estimate for camera poses and landmarks
|
||||
initialEstimate = Values()
|
||||
initialEstimate.insertPose3(x1, first_pose)
|
||||
# noisy estimate for pose 2
|
||||
initialEstimate.insertPose3(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1)))
|
||||
expected_l1 = Point3( 1, 1, 5)
|
||||
initialEstimate.insertPoint3(l1, expected_l1)
|
||||
initialEstimate.insertPoint3(l2, Point3(-1, 1, 5))
|
||||
initialEstimate.insertPoint3(l3, Point3( 0,-.5, 5))
|
||||
|
||||
## optimize
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimize()
|
||||
|
||||
## check equality for the first pose and point
|
||||
pose_x1 = result.atPose3(x1)
|
||||
self.assertTrue(pose_x1.equals(first_pose,1e-4))
|
||||
|
||||
point_l1 = result.atPoint3(l1)
|
||||
self.assertTrue(point_l1.equals(expected_l1,1e-4))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,51 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
|
||||
class TestValues(unittest.TestCase):
|
||||
|
||||
def test_values(self):
|
||||
values = Values()
|
||||
E = EssentialMatrix(Rot3, Unit3)
|
||||
tol = 1e-9
|
||||
|
||||
values.insertPoint2(0, Point2())
|
||||
values.insertPoint3(1, Point3())
|
||||
values.insertRot2(2, Rot2())
|
||||
values.insertPose2(3, Pose2())
|
||||
values.insertRot3(4, Rot3())
|
||||
values.insertPose3(5, Pose3())
|
||||
values.insertCal3_S2(6, Cal3_S2())
|
||||
values.insertCal3DS2(7, Cal3DS2())
|
||||
values.insertCal3Bundler(8, Cal3Bundler())
|
||||
values.insertEssentialMatrix(9, E)
|
||||
values.insertimuBias_ConstantBias(10, imuBias_ConstantBias())
|
||||
|
||||
# special cases for Vector and Matrix:
|
||||
vec = np.array([1., 2., 3.])
|
||||
values.insertVector(11, vec)
|
||||
mat = np.array([[1., 2.], [3., 4.]], order='F')
|
||||
values.insertMatrix(12, mat)
|
||||
|
||||
self.assertTrue(values.atPoint2(0).equals(Point2(), tol))
|
||||
self.assertTrue(values.atPoint3(1).equals(Point3(), tol))
|
||||
self.assertTrue(values.atRot2(2).equals(Rot2(), tol))
|
||||
self.assertTrue(values.atPose2(3).equals(Pose2(), tol))
|
||||
self.assertTrue(values.atRot3(4).equals(Rot3(), tol))
|
||||
self.assertTrue(values.atPose3(5).equals(Pose3(), tol))
|
||||
self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol))
|
||||
self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol))
|
||||
self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol))
|
||||
self.assertTrue(values.atEssentialMatrix(9).equals(E, tol))
|
||||
self.assertTrue(values.atimuBias_ConstantBias(
|
||||
10).equals(imuBias_ConstantBias(), tol))
|
||||
|
||||
# special cases for Vector and Matrix:
|
||||
actualVector = values.atVector(11)
|
||||
self.assertTrue(np.allclose(vec, actualVector.ravel(), tol))
|
||||
actualMatrix = values.atMatrix(12)
|
||||
self.assertTrue(np.allclose(mat, actualMatrix, tol))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
from gtsam_utils import Vector, Matrix
|
||||
import gtsam_utils.visual_data_generator as generator
|
||||
import gtsam_utils.visual_isam as visual_isam
|
||||
|
||||
class TestVisualISAMExample(unittest.TestCase):
|
||||
|
||||
def test_VisualISAMExample(self):
|
||||
# Data Options
|
||||
options = generator.Options()
|
||||
options.triangle = False
|
||||
options.nrCameras = 20
|
||||
|
||||
# iSAM Options
|
||||
isamOptions = visual_isam.Options()
|
||||
isamOptions.hardConstraint = False
|
||||
isamOptions.pointPriors = False
|
||||
isamOptions.batchInitialization = True
|
||||
isamOptions.reorderInterval = 10
|
||||
isamOptions.alwaysRelinearize = False
|
||||
|
||||
# Generate data
|
||||
data, truth = generator.generate_data(options)
|
||||
|
||||
# Initialize iSAM with the first pose and points
|
||||
isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions)
|
||||
|
||||
# Main loop for iSAM: stepping through all poses
|
||||
for currentPose in range(nextPose, options.nrCameras):
|
||||
isam, result = visual_isam.step(data, isam, result, truth, currentPose)
|
||||
|
||||
for i in range(len(truth.cameras)):
|
||||
pose_i = result.atPose3(symbol(ord('x'), i))
|
||||
self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5))
|
||||
|
||||
for j in range(len(truth.points)):
|
||||
point_j = result.atPoint3(symbol(ord('l'), j))
|
||||
self.assertTrue(point_j.equals(truth.points[j], 1e-5))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Loading…
Reference in New Issue