fix bad import * style
Duy-Nguyen Ta
parent
d6c75b57f8
commit
3daf8d7351
|
|
@ -1,20 +1,19 @@
|
|||
"""
|
||||
This file contains small experiments to test the wrapper with gtsam_short,
|
||||
not real unittests. Its name convention is different from other tests so it
|
||||
won't be discovered.
|
||||
This file is not a real python unittest. It contains small experiments
|
||||
to test the wrapper with gtsam_test, a short version of gtsam.h.
|
||||
Its name convention is different from other tests so it won't be discovered.
|
||||
"""
|
||||
from gtsam import *
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from utils import Vector, Matrix
|
||||
|
||||
r = Rot3()
|
||||
r = gtsam.Rot3()
|
||||
print(r)
|
||||
print(r.pitch())
|
||||
r2 = Rot3()
|
||||
r2 = gtsam.Rot3()
|
||||
r3 = r.compose(r2)
|
||||
print("r3 pitch:", r3.pitch())
|
||||
|
||||
v = Vector(1, 1, 1)
|
||||
v = np.array([1, 1, 1])
|
||||
print("v = ", v)
|
||||
r4 = r3.retract(v)
|
||||
print("r4 pitch:", r4.pitch())
|
||||
|
|
@ -29,35 +28,35 @@ Rmat = np.array([
|
|||
[0.104218, 0.990074, -0.0942928],
|
||||
[-0.0942928, 0.104218, 0.990074]
|
||||
])
|
||||
r5 = Rot3(Rmat)
|
||||
r5 = gtsam.Rot3(Rmat)
|
||||
r5.print_(b"r5: ")
|
||||
|
||||
l = Rot3.Logmap(r5)
|
||||
l = gtsam.Rot3.Logmap(r5)
|
||||
print("l = ", l)
|
||||
|
||||
|
||||
noise = noiseModel_Gaussian.Covariance(Rmat)
|
||||
noise = gtsam.noiseModel_Gaussian.Covariance(Rmat)
|
||||
noise.print_(b"noise:")
|
||||
|
||||
D = np.array([1.,2.,3.])
|
||||
diag = noiseModel_Diagonal.Variances(D)
|
||||
diag = gtsam.noiseModel_Diagonal.Variances(D)
|
||||
print("diag:", diag)
|
||||
diag.print_(b"diag:")
|
||||
print("diag R:", diag.R())
|
||||
|
||||
p = Point3()
|
||||
p = gtsam.Point3()
|
||||
p.print_("p:")
|
||||
factor = BetweenFactorPoint3(1,2,p, noise)
|
||||
factor = gtsam.BetweenFactorPoint3(1,2,p, noise)
|
||||
factor.print_(b"factor:")
|
||||
|
||||
vv = VectorValues()
|
||||
vv = gtsam.VectorValues()
|
||||
vv.print_(b"vv:")
|
||||
vv.insert(1, np.array([1.,2.,3.]))
|
||||
vv.insert(2, np.array([3.,4.]))
|
||||
vv.insert(3, np.array([5.,6.,7.,8.]))
|
||||
vv.print_(b"vv:")
|
||||
|
||||
vv2 = VectorValues(vv)
|
||||
vv2 = gtsam.VectorValues(vv)
|
||||
vv2.insert(4, np.array([4.,2.,1]))
|
||||
vv2.print_(b"vv2:")
|
||||
vv.print_(b"vv:")
|
||||
|
|
@ -68,17 +67,17 @@ vv3 = vv.add(vv2)
|
|||
|
||||
vv3.print_(b"vv3:")
|
||||
|
||||
values = Values()
|
||||
values.insert(1, Point3())
|
||||
values.insert(2, Rot3())
|
||||
values = gtsam.Values()
|
||||
values.insert(1, gtsam.Point3())
|
||||
values.insert(2, gtsam.Rot3())
|
||||
values.print_(b"values:")
|
||||
|
||||
factor = PriorFactorVector(1, np.array([1.,2.,3.]), diag)
|
||||
factor = gtsam.PriorFactorVector(1, np.array([1.,2.,3.]), diag)
|
||||
print "Prior factor vector: ", factor
|
||||
|
||||
|
||||
|
||||
keys = KeyVector()
|
||||
keys = gtsam.KeyVector()
|
||||
|
||||
keys.push_back(1)
|
||||
keys.push_back(2)
|
||||
|
|
@ -86,28 +85,28 @@ print 'size: ', keys.size()
|
|||
print keys.at(0)
|
||||
print keys.at(1)
|
||||
|
||||
noise = noiseModel_Isotropic.Precision(2, 3.0)
|
||||
noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0)
|
||||
noise.print_('noise:')
|
||||
print 'noise print:', noise
|
||||
f = JacobianFactor(7, np.ones([2,2]), model=noise, b=np.ones(2))
|
||||
f = gtsam.JacobianFactor(7, np.ones([2,2]), model=noise, b=np.ones(2))
|
||||
print 'JacobianFactor(7):\n', f
|
||||
print "A = ", f.getA()
|
||||
print "b = ", f.getb()
|
||||
|
||||
f = JacobianFactor(np.ones(2))
|
||||
f = gtsam.JacobianFactor(np.ones(2))
|
||||
f.print_('jacoboian b_in:')
|
||||
|
||||
|
||||
print "JacobianFactor initalized with b_in:", f
|
||||
|
||||
diag = noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.]))
|
||||
fv = PriorFactorVector(1, np.array([4.,5.,6.]), diag)
|
||||
diag = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.]))
|
||||
fv = gtsam.PriorFactorVector(1, np.array([4.,5.,6.]), diag)
|
||||
print "priorfactorvector: ", fv
|
||||
|
||||
print "base noise: ", fv.get_noiseModel()
|
||||
print "casted to gaussian2: ", dynamic_cast_noiseModel_Diagonal_noiseModel_Base(fv.get_noiseModel())
|
||||
print "casted to gaussian2: ", gtsam.dynamic_cast_noiseModel_Diagonal_noiseModel_Base(fv.get_noiseModel())
|
||||
|
||||
X = symbol(65, 19)
|
||||
X = gtsam.symbol(65, 19)
|
||||
print X
|
||||
print symbolChr(X)
|
||||
print symbolIndex(X)
|
||||
print gtsam.symbolChr(X)
|
||||
print gtsam.symbolIndex(X)
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
import gtsam
|
||||
from math import *
|
||||
import numpy as np
|
||||
|
||||
|
|
@ -7,7 +7,7 @@ import numpy as np
|
|||
class TestCal3Unified(unittest.TestCase):
|
||||
|
||||
def test_Cal3Unified(self):
|
||||
K = Cal3Unified()
|
||||
K = gtsam.Cal3Unified()
|
||||
self.assertEqual(K.fx(), 1.)
|
||||
self.assertEqual(K.fx(), 1.)
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,5 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import gtsam
|
||||
import numpy as np
|
||||
|
||||
class TestJacobianFactor(unittest.TestCase):
|
||||
|
|
@ -35,12 +34,12 @@ class TestJacobianFactor(unittest.TestCase):
|
|||
# the RHS
|
||||
b2 = np.array([-1., 1.5, 2., -1.])
|
||||
sigmas = np.array([1., 1., 1., 1.])
|
||||
model4 = noiseModel_Diagonal.Sigmas(sigmas)
|
||||
combined = JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4)
|
||||
model4 = gtsam.noiseModel_Diagonal.Sigmas(sigmas)
|
||||
combined = gtsam.JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4)
|
||||
|
||||
# eliminate the first variable (x2) in the combined factor, destructive
|
||||
# !
|
||||
ord = Ordering()
|
||||
ord = gtsam.Ordering()
|
||||
ord.push_back(x2)
|
||||
actualCG, lf = combined.eliminate(ord)
|
||||
|
||||
|
|
@ -52,8 +51,8 @@ class TestJacobianFactor(unittest.TestCase):
|
|||
S13 = np.array([[-8.94427, 0.00],
|
||||
[+0.00, -8.94427]])
|
||||
d = np.array([2.23607, -1.56525])
|
||||
expectedCG = GaussianConditional(
|
||||
x2, d, R11, l1, S12, x1, S13, noiseModel_Unit.Create(2))
|
||||
expectedCG = gtsam.GaussianConditional(
|
||||
x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2))
|
||||
# check if the result matches
|
||||
self.assertTrue(actualCG.equals(expectedCG, 1e-4))
|
||||
|
||||
|
|
@ -69,8 +68,8 @@ class TestJacobianFactor(unittest.TestCase):
|
|||
# the RHS
|
||||
b1 = np.array([0.0, 0.894427])
|
||||
|
||||
model2 = noiseModel_Diagonal.Sigmas(np.array([1., 1.]))
|
||||
expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2)
|
||||
model2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([1., 1.]))
|
||||
expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2)
|
||||
|
||||
# check if the result matches the combined (reduced) factor
|
||||
self.assertTrue(lf.equals(expectedLF, 1e-4))
|
||||
|
|
|
|||
|
|
@ -1,6 +1,5 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import gtsam
|
||||
import numpy as np
|
||||
|
||||
class TestKalmanFilter(unittest.TestCase):
|
||||
|
|
@ -9,13 +8,13 @@ class TestKalmanFilter(unittest.TestCase):
|
|||
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]))
|
||||
modelQ = gtsam.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]))
|
||||
modelR = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1]))
|
||||
R = 0.01 * np.eye(2)
|
||||
|
||||
# Create the set of expected output TestValues
|
||||
|
|
@ -35,7 +34,7 @@ class TestKalmanFilter(unittest.TestCase):
|
|||
I33 = np.linalg.inv(P23) + np.linalg.inv(R)
|
||||
|
||||
# Create an KalmanFilter object
|
||||
KF = KalmanFilter(n=2)
|
||||
KF = gtsam.KalmanFilter(n=2)
|
||||
|
||||
# Create the Kalman Filter initialization point
|
||||
x_initial = np.array([0.0, 0.0])
|
||||
|
|
|
|||
|
|
@ -1,7 +1,5 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from gtsam.utils import *
|
||||
from math import *
|
||||
import gtsam
|
||||
import numpy as np
|
||||
|
||||
class TestLocalizationExample(unittest.TestCase):
|
||||
|
|
@ -9,39 +7,39 @@ class TestLocalizationExample(unittest.TestCase):
|
|||
def test_LocalizationExample(self):
|
||||
# Create the graph (defined in pose2SLAM.h, derived from
|
||||
# NonlinearFactorGraph)
|
||||
graph = NonlinearFactorGraph()
|
||||
graph = gtsam.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(
|
||||
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
|
||||
odometryNoise = gtsam.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))
|
||||
graph.add(gtsam.BetweenFactorPose2(0, 1, odometry, odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
|
||||
|
||||
# Add three "GPS" measurements
|
||||
# We use Pose2 Priors here with high variance on theta
|
||||
groundTruth = Values()
|
||||
groundTruth.insert(0, Pose2(0.0, 0.0, 0.0))
|
||||
groundTruth.insert(1, Pose2(2.0, 0.0, 0.0))
|
||||
groundTruth.insert(2, Pose2(4.0, 0.0, 0.0))
|
||||
model = noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 10.]))
|
||||
groundTruth = gtsam.Values()
|
||||
groundTruth.insert(0, gtsam.Pose2(0.0, 0.0, 0.0))
|
||||
groundTruth.insert(1, gtsam.Pose2(2.0, 0.0, 0.0))
|
||||
groundTruth.insert(2, gtsam.Pose2(4.0, 0.0, 0.0))
|
||||
model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 10.]))
|
||||
for i in range(3):
|
||||
graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model))
|
||||
graph.add(gtsam.PriorFactorPose2(i, groundTruth.atPose2(i), model))
|
||||
|
||||
# Initialize to noisy points
|
||||
initialEstimate = Values()
|
||||
initialEstimate.insert(0, Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insert(1, Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insert(2, Pose2(4.1, 0.1, 0.1))
|
||||
initialEstimate = gtsam.Values()
|
||||
initialEstimate.insert(0, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insert(1, gtsam.Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insert(2, gtsam.Pose2(4.1, 0.1, 0.1))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimizeSafely()
|
||||
|
||||
# Plot Covariance Ellipses
|
||||
marginals = Marginals(graph, result)
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
P = [None] * result.size()
|
||||
for i in range(0, result.size()):
|
||||
pose_i = result.atPose2(i)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,5 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import gtsam
|
||||
import numpy as np
|
||||
|
||||
class TestOdometryExample(unittest.TestCase):
|
||||
|
|
@ -8,39 +7,39 @@ class TestOdometryExample(unittest.TestCase):
|
|||
def test_OdometryExample(self):
|
||||
# Create the graph (defined in pose2SLAM.h, derived from
|
||||
# NonlinearFactorGraph)
|
||||
graph = NonlinearFactorGraph()
|
||||
graph = gtsam.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(
|
||||
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior mean is at origin
|
||||
priorNoise = gtsam.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))
|
||||
graph.add(gtsam.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(
|
||||
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
|
||||
odometryNoise = gtsam.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))
|
||||
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))
|
||||
|
||||
# Initialize to noisy points
|
||||
initialEstimate = Values()
|
||||
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1))
|
||||
initialEstimate = gtsam.Values()
|
||||
initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimizeSafely()
|
||||
marginals = Marginals(graph, result)
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
marginals.marginalCovariance(1)
|
||||
|
||||
# Check first pose equality
|
||||
pose_1 = result.atPose2(1)
|
||||
self.assertTrue(pose_1.equals(Pose2(), 1e-4))
|
||||
self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import gtsam
|
||||
from math import pi
|
||||
import numpy as np
|
||||
|
||||
class TestPose2SLAMExample(unittest.TestCase):
|
||||
|
|
@ -13,50 +13,50 @@ class TestPose2SLAMExample(unittest.TestCase):
|
|||
# - The robot is on a grid, moving 2 meters each step
|
||||
|
||||
# Create graph container and add factors to it
|
||||
graph = NonlinearFactorGraph()
|
||||
graph = gtsam.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]))
|
||||
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
|
||||
priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
# add directly to graph
|
||||
graph.add(PriorFactorPose2(1, priorMean, priorNoise))
|
||||
graph.add(gtsam.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))
|
||||
odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
4, 5, gtsam.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))
|
||||
model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model))
|
||||
|
||||
# Initialize to noisy points
|
||||
initialEstimate = Values()
|
||||
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insert(3, Pose2(4.1, 0.1, pi / 2))
|
||||
initialEstimate.insert(4, Pose2(4.0, 2.0, pi))
|
||||
initialEstimate.insert(5, Pose2(2.1, 2.1, -pi / 2))
|
||||
initialEstimate = gtsam.Values()
|
||||
initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2))
|
||||
initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi))
|
||||
initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimizeSafely()
|
||||
|
||||
# Plot Covariance Ellipses
|
||||
marginals = Marginals(graph, result)
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
P = marginals.marginalCovariance(1)
|
||||
|
||||
pose_1 = result.atPose2(1)
|
||||
self.assertTrue(pose_1.equals(Pose2(), 1e-4))
|
||||
self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4))
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import gtsam
|
||||
from math import pi
|
||||
import numpy as np
|
||||
|
||||
class TestPose2SLAMExample(unittest.TestCase):
|
||||
|
|
@ -13,50 +13,50 @@ class TestPose2SLAMExample(unittest.TestCase):
|
|||
# - The robot is on a grid, moving 2 meters each step
|
||||
|
||||
# Create graph container and add factors to it
|
||||
graph = NonlinearFactorGraph()
|
||||
graph = gtsam.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]))
|
||||
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
|
||||
priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
# add directly to graph
|
||||
graph.add(PriorFactorPose2(1, priorMean, priorNoise))
|
||||
graph.add(gtsam.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))
|
||||
odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
4, 5, gtsam.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))
|
||||
model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model))
|
||||
|
||||
# Initialize to noisy points
|
||||
initialEstimate = Values()
|
||||
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insert(3, Pose2(4.1, 0.1, pi / 2))
|
||||
initialEstimate.insert(4, Pose2(4.0, 2.0, pi))
|
||||
initialEstimate.insert(5, Pose2(2.1, 2.1, -pi / 2))
|
||||
initialEstimate = gtsam.Values()
|
||||
initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2))
|
||||
initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi))
|
||||
initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimizeSafely()
|
||||
|
||||
# Plot Covariance Ellipses
|
||||
marginals = Marginals(graph, result)
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
P = marginals.marginalCovariance(1)
|
||||
|
||||
pose_1 = result.atPose2(1)
|
||||
self.assertTrue(pose_1.equals(Pose2(), 1e-4))
|
||||
self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
import gtsam
|
||||
from math import pi
|
||||
from gtsam.utils.circlePose3 import *
|
||||
|
||||
class TestPose3SLAMExample(unittest.TestCase):
|
||||
|
|
@ -13,20 +13,20 @@ class TestPose3SLAMExample(unittest.TestCase):
|
|||
p1 = hexagon.atPose3(1)
|
||||
|
||||
# create a Pose graph with one equality constraint and one measurement
|
||||
fg = NonlinearFactorGraph()
|
||||
fg.add(NonlinearEqualityPose3(0, p0))
|
||||
fg = gtsam.NonlinearFactorGraph()
|
||||
fg.add(gtsam.NonlinearEqualityPose3(0, p0))
|
||||
delta = p0.between(p1)
|
||||
covariance = noiseModel_Diagonal.Sigmas(
|
||||
covariance = gtsam.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))
|
||||
fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance))
|
||||
fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance))
|
||||
fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance))
|
||||
fg.add(gtsam.BetweenFactorPose3(3, 4, delta, covariance))
|
||||
fg.add(gtsam.BetweenFactorPose3(4, 5, delta, covariance))
|
||||
fg.add(gtsam.BetweenFactorPose3(5, 0, delta, covariance))
|
||||
|
||||
# Create initial config
|
||||
initial = Values()
|
||||
initial = gtsam.Values()
|
||||
s = 0.10
|
||||
initial.insert(0, p0)
|
||||
initial.insert(1, hexagon.atPose3(1).retract(s * np.random.randn(6, 1)))
|
||||
|
|
@ -36,7 +36,7 @@ class TestPose3SLAMExample(unittest.TestCase):
|
|||
initial.insert(5, hexagon.atPose3(5).retract(s * np.random.randn(6, 1)))
|
||||
|
||||
# optimize
|
||||
optimizer = LevenbergMarquardtOptimizer(fg, initial)
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(fg, initial)
|
||||
result = optimizer.optimizeSafely()
|
||||
|
||||
pose_1 = result.atPose3(1)
|
||||
|
|
|
|||
|
|
@ -1,24 +1,23 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import gtsam
|
||||
import numpy as np
|
||||
|
||||
class TestPriorFactor(unittest.TestCase):
|
||||
|
||||
def test_PriorFactor(self):
|
||||
values = Values()
|
||||
values = gtsam.Values()
|
||||
|
||||
key = 5
|
||||
priorPose3 = Pose3()
|
||||
model = noiseModel_Unit.Create(6)
|
||||
factor = PriorFactorPose3(key, priorPose3, model)
|
||||
priorPose3 = gtsam.Pose3()
|
||||
model = gtsam.noiseModel_Unit.Create(6)
|
||||
factor = gtsam.PriorFactorPose3(key, priorPose3, model)
|
||||
values.insert(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)
|
||||
model = gtsam.noiseModel_Unit.Create(3)
|
||||
factor = gtsam.PriorFactorVector(key, priorVector, model)
|
||||
values.insert(key, priorVector)
|
||||
self.assertEqual(factor.error(values), 0)
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import gtsam
|
||||
from gtsam import symbol
|
||||
import numpy as np
|
||||
import gtsam.utils.visual_data_generator as generator
|
||||
|
||||
|
|
@ -18,26 +18,26 @@ class TestSFMExample(unittest.TestCase):
|
|||
pointNoiseSigma = 0.1
|
||||
poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])
|
||||
|
||||
graph = NonlinearFactorGraph()
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# Add factors for all measurements
|
||||
measurementNoise = noiseModel_Isotropic.Sigma(2, measurementNoiseSigma)
|
||||
measurementNoise = gtsam.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(
|
||||
graph.add(gtsam.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),
|
||||
posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
|
||||
graph.add(gtsam.PriorFactorPose3(symbol(ord('x'), 0),
|
||||
truth.cameras[0].pose(), posePriorNoise))
|
||||
pointPriorNoise = noiseModel_Isotropic.Sigma(3, pointNoiseSigma)
|
||||
graph.add(PriorFactorPoint3(symbol(ord('p'), 0),
|
||||
pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, pointNoiseSigma)
|
||||
graph.add(gtsam.PriorFactorPoint3(symbol(ord('p'), 0),
|
||||
truth.points[0], pointPriorNoise))
|
||||
|
||||
# Initial estimate
|
||||
initialEstimate = Values()
|
||||
initialEstimate = gtsam.Values()
|
||||
for i in range(len(truth.cameras)):
|
||||
pose_i = truth.cameras[i].pose()
|
||||
initialEstimate.insert(symbol(ord('x'), i), pose_i)
|
||||
|
|
@ -46,13 +46,13 @@ class TestSFMExample(unittest.TestCase):
|
|||
initialEstimate.insert(symbol(ord('p'), j), point_j)
|
||||
|
||||
# Optimization
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
for i in range(5):
|
||||
optimizer.iterate()
|
||||
result = optimizer.values()
|
||||
|
||||
# Marginalization
|
||||
marginals = Marginals(graph, result)
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
marginals.marginalCovariance(symbol(ord('p'), 0))
|
||||
marginals.marginalCovariance(symbol(ord('x'), 0))
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import gtsam
|
||||
from gtsam import symbol
|
||||
import numpy as np
|
||||
|
||||
|
||||
|
|
@ -22,40 +22,40 @@ class TestStereoVOExample(unittest.TestCase):
|
|||
l3 = symbol(ord('l'),3)
|
||||
|
||||
## Create graph container and add factors to it
|
||||
graph = NonlinearFactorGraph()
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
## add a constraint on the starting pose
|
||||
first_pose = Pose3()
|
||||
graph.add(NonlinearEqualityPose3(x1, first_pose))
|
||||
first_pose = gtsam.Pose3()
|
||||
graph.add(gtsam.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(np.array([1.0, 1.0, 1.0]))
|
||||
K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
|
||||
stereo_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([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))
|
||||
graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(520, 480, 440), stereo_model, x1, l1, K))
|
||||
graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(120, 80, 440), stereo_model, x1, l2, K))
|
||||
graph.add(gtsam.GenericStereoFactor3D(gtsam.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))
|
||||
graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(570, 520, 490), stereo_model, x2, l1, K))
|
||||
graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K))
|
||||
graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 270, 115), stereo_model, x2, l3, K))
|
||||
|
||||
## Create initial estimate for camera poses and landmarks
|
||||
initialEstimate = Values()
|
||||
initialEstimate = gtsam.Values()
|
||||
initialEstimate.insert(x1, first_pose)
|
||||
# noisy estimate for pose 2
|
||||
initialEstimate.insert(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1)))
|
||||
expected_l1 = Point3( 1, 1, 5)
|
||||
initialEstimate.insert(x2, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1,-.1,1.1)))
|
||||
expected_l1 = gtsam.Point3( 1, 1, 5)
|
||||
initialEstimate.insert(l1, expected_l1)
|
||||
initialEstimate.insert(l2, Point3(-1, 1, 5))
|
||||
initialEstimate.insert(l3, Point3( 0,-.5, 5))
|
||||
initialEstimate.insert(l2, gtsam.Point3(-1, 1, 5))
|
||||
initialEstimate.insert(l3, gtsam.Point3( 0,-.5, 5))
|
||||
|
||||
## optimize
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimize()
|
||||
|
||||
## check equality for the first pose and point
|
||||
|
|
|
|||
|
|
@ -1,26 +1,25 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import gtsam
|
||||
import numpy as np
|
||||
|
||||
class TestValues(unittest.TestCase):
|
||||
|
||||
def test_values(self):
|
||||
values = Values()
|
||||
E = EssentialMatrix(Rot3(), Unit3())
|
||||
values = gtsam.Values()
|
||||
E = gtsam.EssentialMatrix(gtsam.Rot3(), gtsam.Unit3())
|
||||
tol = 1e-9
|
||||
|
||||
values.insert(0, Point2())
|
||||
values.insert(1, Point3())
|
||||
values.insert(2, Rot2())
|
||||
values.insert(3, Pose2())
|
||||
values.insert(4, Rot3())
|
||||
values.insert(5, Pose3())
|
||||
values.insert(6, Cal3_S2())
|
||||
values.insert(7, Cal3DS2())
|
||||
values.insert(8, Cal3Bundler())
|
||||
values.insert(0, gtsam.Point2())
|
||||
values.insert(1, gtsam.Point3())
|
||||
values.insert(2, gtsam.Rot2())
|
||||
values.insert(3, gtsam.Pose2())
|
||||
values.insert(4, gtsam.Rot3())
|
||||
values.insert(5, gtsam.Pose3())
|
||||
values.insert(6, gtsam.Cal3_S2())
|
||||
values.insert(7, gtsam.Cal3DS2())
|
||||
values.insert(8, gtsam.Cal3Bundler())
|
||||
values.insert(9, E)
|
||||
values.insert(10, imuBias_ConstantBias())
|
||||
values.insert(10, gtsam.imuBias_ConstantBias())
|
||||
|
||||
# Special cases for Vectors and Matrices
|
||||
# Note that gtsam's Eigen Vectors and Matrices requires double-precision
|
||||
|
|
@ -42,18 +41,18 @@ class TestValues(unittest.TestCase):
|
|||
mat2 = np.array([[1,2,],[3,5]])
|
||||
values.insert(13, mat2)
|
||||
|
||||
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.atPoint2(0).equals(gtsam.Point2(), tol))
|
||||
self.assertTrue(values.atPoint3(1).equals(gtsam.Point3(), tol))
|
||||
self.assertTrue(values.atRot2(2).equals(gtsam.Rot2(), tol))
|
||||
self.assertTrue(values.atPose2(3).equals(gtsam.Pose2(), tol))
|
||||
self.assertTrue(values.atRot3(4).equals(gtsam.Rot3(), tol))
|
||||
self.assertTrue(values.atPose3(5).equals(gtsam.Pose3(), tol))
|
||||
self.assertTrue(values.atCal3_S2(6).equals(gtsam.Cal3_S2(), tol))
|
||||
self.assertTrue(values.atCal3DS2(7).equals(gtsam.Cal3DS2(), tol))
|
||||
self.assertTrue(values.atCal3Bundler(8).equals(gtsam.Cal3Bundler(), tol))
|
||||
self.assertTrue(values.atEssentialMatrix(9).equals(E, tol))
|
||||
self.assertTrue(values.atimuBias_ConstantBias(
|
||||
10).equals(imuBias_ConstantBias(), tol))
|
||||
10).equals(gtsam.imuBias_ConstantBias(), tol))
|
||||
|
||||
# special cases for Vector and Matrix:
|
||||
actualVector = values.atVector(11)
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
from gtsam import symbol
|
||||
import gtsam.utils.visual_data_generator as generator
|
||||
import gtsam.utils.visual_isam as visual_isam
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
from gtsam import *
|
||||
from math import *
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from math import pi, cos, sin
|
||||
|
||||
def circlePose3(numPoses = 8, radius = 1.0, symbolChar = 0):
|
||||
"""
|
||||
|
|
|
|||
|
|
@ -1,14 +1,14 @@
|
|||
from np_utils import *
|
||||
from math import *
|
||||
from gtsam import *
|
||||
|
||||
import numpy as np
|
||||
from math import pi, cos, sin
|
||||
import gtsam
|
||||
from gtsam import symbol
|
||||
|
||||
class Options:
|
||||
"""
|
||||
Options to generate test scenario
|
||||
"""
|
||||
|
||||
def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()):
|
||||
def __init__(self, triangle=False, nrCameras=3, K=gtsam.Cal3_S2()):
|
||||
"""
|
||||
Options to generate test scenario
|
||||
@param triangle: generate a triangle scene with 3 points if True, otherwise
|
||||
|
|
@ -25,10 +25,10 @@ class GroundTruth:
|
|||
Object holding generated ground-truth data
|
||||
"""
|
||||
|
||||
def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
|
||||
def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
|
||||
self.K = K
|
||||
self.cameras = [Pose3()] * nrCameras
|
||||
self.points = [Point3()] * nrPoints
|
||||
self.cameras = [gtsam.Pose3()] * nrCameras
|
||||
self.points = [gtsam.Point3()] * nrPoints
|
||||
|
||||
def print_(self, s=""):
|
||||
print s
|
||||
|
|
@ -50,22 +50,22 @@ class Data:
|
|||
class NoiseModels:
|
||||
pass
|
||||
|
||||
def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
|
||||
def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
|
||||
self.K = K
|
||||
self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras]
|
||||
self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras]
|
||||
self.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
|
||||
self.odometry = [Pose3()] * nrCameras
|
||||
self.odometry = [gtsam.Pose3()] * nrCameras
|
||||
|
||||
# Set Noise parameters
|
||||
self.noiseModels = Data.NoiseModels()
|
||||
self.noiseModels.posePrior = noiseModel_Diagonal.Sigmas(
|
||||
self.noiseModels.posePrior = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]))
|
||||
# noiseModels.odometry = noiseModel_Diagonal.Sigmas(
|
||||
# noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
# np.array([0.001,0.001,0.001,0.1,0.1,0.1]))
|
||||
self.noiseModels.odometry = noiseModel_Diagonal.Sigmas(
|
||||
self.noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([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)
|
||||
self.noiseModels.pointPrior = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
|
||||
self.noiseModels.measurement = gtsam.noiseModel_Isotropic.Sigma(2, 1.0)
|
||||
|
||||
|
||||
def generate_data(options):
|
||||
|
|
@ -73,7 +73,7 @@ def generate_data(options):
|
|||
Generate ground-truth and measurement data
|
||||
"""
|
||||
|
||||
K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
|
||||
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)
|
||||
|
|
@ -84,25 +84,25 @@ def generate_data(options):
|
|||
r = 10
|
||||
for j in range(len(truth.points)):
|
||||
theta = j * 2 * pi / nrPoints
|
||||
truth.points[j] = Point3(r * cos(theta), r * sin(theta), 0)
|
||||
truth.points[j] = gtsam.Point3(r * cos(theta), r * sin(theta), 0)
|
||||
else: # 3D landmarks as vertices of a cube
|
||||
truth.points = [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)]
|
||||
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 = Point3(r * cos(theta), r * sin(theta), height)
|
||||
truth.cameras[i] = SimpleCamera.Lookat(
|
||||
t, Point3(), Point3(0, 0, 1), truth.K)
|
||||
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
|
||||
for j in range(nrPoints):
|
||||
# All landmarks seen in every frame
|
||||
|
|
|
|||
|
|
@ -1,7 +1,5 @@
|
|||
from np_utils import *
|
||||
from math import *
|
||||
from gtsam import *
|
||||
|
||||
import gtsam
|
||||
from gtsam import symbol
|
||||
|
||||
class Options:
|
||||
"""
|
||||
|
|
@ -20,20 +18,20 @@ def initialize(data, truth, options):
|
|||
params = gtsam.ISAM2Params()
|
||||
if options.alwaysRelinearize:
|
||||
params.setRelinearizeSkip(1)
|
||||
isam = ISAM2(params = params)
|
||||
isam = gtsam.ISAM2(params = params)
|
||||
|
||||
# Add constraints/priors
|
||||
# TODO: should not be from ground truth!
|
||||
newFactors = NonlinearFactorGraph()
|
||||
initialEstimates = Values()
|
||||
newFactors = gtsam.NonlinearFactorGraph()
|
||||
initialEstimates = gtsam.Values()
|
||||
for i in range(2):
|
||||
ii = symbol(ord('x'), i)
|
||||
if i == 0:
|
||||
if options.hardConstraint: # add hard constraint
|
||||
newFactors.add(NonlinearEqualityPose3(
|
||||
newFactors.add(gtsam.NonlinearEqualityPose3(
|
||||
ii, truth.cameras[0].pose()))
|
||||
else:
|
||||
newFactors.add(PriorFactorPose3(
|
||||
newFactors.add(gtsam.PriorFactorPose3(
|
||||
ii, truth.cameras[i].pose(), data.noiseModels.posePrior))
|
||||
initialEstimates.insert(ii, truth.cameras[i].pose())
|
||||
|
||||
|
|
@ -46,22 +44,22 @@ def initialize(data, truth, options):
|
|||
for k in range(len(data.Z[i])):
|
||||
j = data.J[i][k]
|
||||
jj = symbol(ord('l'), j)
|
||||
newFactors.add(GenericProjectionFactorCal3_S2(
|
||||
newFactors.add(gtsam.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.insert(jj, truth.points[j])
|
||||
if options.pointPriors: # add point priors
|
||||
newFactors.add(PriorFactorPoint3(
|
||||
newFactors.add(gtsam.PriorFactorPoint3(
|
||||
jj, truth.points[j], data.noiseModels.pointPrior))
|
||||
|
||||
# Add odometry between frames 0 and 1
|
||||
newFactors.add(BetweenFactorPose3(symbol(ord('x'), 0), symbol(
|
||||
newFactors.add(gtsam.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(
|
||||
batchOptimizer = gtsam.LevenbergMarquardtOptimizer(
|
||||
newFactors, initialEstimates)
|
||||
fullyOptimized = batchOptimizer.optimize()
|
||||
isam.update(newFactors, fullyOptimized)
|
||||
|
|
@ -87,22 +85,22 @@ def step(data, isam, result, truth, currPoseIndex):
|
|||
"""
|
||||
# iSAM expects us to give it a new set of factors
|
||||
# along with initial estimates for any new variables introduced.
|
||||
newFactors = NonlinearFactorGraph()
|
||||
initialEstimates = Values()
|
||||
newFactors = gtsam.NonlinearFactorGraph()
|
||||
initialEstimates = gtsam.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))
|
||||
newFactors.add(gtsam.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(
|
||||
newFactors.add(gtsam.GenericProjectionFactorCal3_S2(
|
||||
zij, data.noiseModels.measurement,
|
||||
symbol(ord('x'), currPoseIndex), jj, data.K))
|
||||
# TODO: initialize with something other than truth
|
||||
|
|
|
|||
Loading…
Reference in New Issue