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

fix bad import * style

release/4.3a0
Duy-Nguyen Ta 2017-03-18 19:50:35 -04:00
parent d6c75b57f8
commit 3daf8d7351
17 changed files with 252 additions and 263 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
cython/gtsam/tests/experiments.py
View File

@ -1,20 +1,19 @@
""" """
This file contains small experiments to test the wrapper with gtsam_short, This file is not a real python unittest. It contains small experiments
not real unittests. Its name convention is different from other tests so it to test the wrapper with gtsam_test, a short version of gtsam.h.
won't be discovered. Its name convention is different from other tests so it won't be discovered.
""" """
from gtsam import * import gtsam
import numpy as np import numpy as np
from utils import Vector, Matrix
r = Rot3() r = gtsam.Rot3()
print(r) print(r)
print(r.pitch()) print(r.pitch())
r2 = Rot3() r2 = gtsam.Rot3()
r3 = r.compose(r2) r3 = r.compose(r2)
print("r3 pitch:", r3.pitch()) print("r3 pitch:", r3.pitch())
v = Vector(1, 1, 1) v = np.array([1, 1, 1])
print("v = ", v) print("v = ", v)
r4 = r3.retract(v) r4 = r3.retract(v)
print("r4 pitch:", r4.pitch()) print("r4 pitch:", r4.pitch())
@ -29,35 +28,35 @@ Rmat = np.array([
[0.104218, 0.990074, -0.0942928], [0.104218, 0.990074, -0.0942928],
[-0.0942928, 0.104218, 0.990074] [-0.0942928, 0.104218, 0.990074]
]) ])
r5 = Rot3(Rmat) r5 = gtsam.Rot3(Rmat)
r5.print_(b"r5: ") r5.print_(b"r5: ")
l = Rot3.Logmap(r5) l = gtsam.Rot3.Logmap(r5)
print("l = ", l) print("l = ", l)
noise = noiseModel_Gaussian.Covariance(Rmat) noise = gtsam.noiseModel_Gaussian.Covariance(Rmat)
noise.print_(b"noise:") noise.print_(b"noise:")
D = np.array([1.,2.,3.]) D = np.array([1.,2.,3.])
diag = noiseModel_Diagonal.Variances(D) diag = gtsam.noiseModel_Diagonal.Variances(D)
print("diag:", diag) print("diag:", diag)
diag.print_(b"diag:") diag.print_(b"diag:")
print("diag R:", diag.R()) print("diag R:", diag.R())
p = Point3() p = gtsam.Point3()
p.print_("p:") p.print_("p:")
factor = BetweenFactorPoint3(1,2,p, noise) factor = gtsam.BetweenFactorPoint3(1,2,p, noise)
factor.print_(b"factor:") factor.print_(b"factor:")
vv = VectorValues() vv = gtsam.VectorValues()
vv.print_(b"vv:") vv.print_(b"vv:")
vv.insert(1, np.array([1.,2.,3.])) vv.insert(1, np.array([1.,2.,3.]))
vv.insert(2, np.array([3.,4.])) vv.insert(2, np.array([3.,4.]))
vv.insert(3, np.array([5.,6.,7.,8.])) vv.insert(3, np.array([5.,6.,7.,8.]))
vv.print_(b"vv:") vv.print_(b"vv:")
vv2 = VectorValues(vv) vv2 = gtsam.VectorValues(vv)
vv2.insert(4, np.array([4.,2.,1])) vv2.insert(4, np.array([4.,2.,1]))
vv2.print_(b"vv2:") vv2.print_(b"vv2:")
vv.print_(b"vv:") vv.print_(b"vv:")
@ -68,17 +67,17 @@ vv3 = vv.add(vv2)
vv3.print_(b"vv3:") vv3.print_(b"vv3:")
values = Values() values = gtsam.Values()
values.insert(1, Point3()) values.insert(1, gtsam.Point3())
values.insert(2, Rot3()) values.insert(2, gtsam.Rot3())
values.print_(b"values:") 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 print "Prior factor vector: ", factor
keys = KeyVector() keys = gtsam.KeyVector()
keys.push_back(1) keys.push_back(1)
keys.push_back(2) keys.push_back(2)
@ -86,28 +85,28 @@ print 'size: ', keys.size()
print keys.at(0) print keys.at(0)
print keys.at(1) print keys.at(1)
noise = noiseModel_Isotropic.Precision(2, 3.0) noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0)
noise.print_('noise:') noise.print_('noise:')
print '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 'JacobianFactor(7):\n', f
print "A = ", f.getA() print "A = ", f.getA()
print "b = ", f.getb() print "b = ", f.getb()
f = JacobianFactor(np.ones(2)) f = gtsam.JacobianFactor(np.ones(2))
f.print_('jacoboian b_in:') f.print_('jacoboian b_in:')
print "JacobianFactor initalized with b_in:", f print "JacobianFactor initalized with b_in:", f
diag = noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.])) diag = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.]))
fv = PriorFactorVector(1, np.array([4.,5.,6.]), diag) fv = gtsam.PriorFactorVector(1, np.array([4.,5.,6.]), diag)
print "priorfactorvector: ", fv print "priorfactorvector: ", fv
print "base noise: ", fv.get_noiseModel() 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 X
print symbolChr(X) print gtsam.symbolChr(X)
print symbolIndex(X) print gtsam.symbolIndex(X)

4
cython/gtsam/tests/test_Cal3Unified.py
View File

@ -1,5 +1,5 @@
import unittest import unittest
from gtsam import * import gtsam
from math import * from math import *
import numpy as np import numpy as np
@ -7,7 +7,7 @@ import numpy as np
class TestCal3Unified(unittest.TestCase): class TestCal3Unified(unittest.TestCase):
def test_Cal3Unified(self): def test_Cal3Unified(self):
K = Cal3Unified() K = gtsam.Cal3Unified()
self.assertEqual(K.fx(), 1.) self.assertEqual(K.fx(), 1.)
self.assertEqual(K.fx(), 1.) self.assertEqual(K.fx(), 1.)

17
cython/gtsam/tests/test_JacobianFactor.py
View File

@ -1,6 +1,5 @@
import unittest import unittest
from gtsam import * import gtsam
from math import *
import numpy as np import numpy as np
class TestJacobianFactor(unittest.TestCase): class TestJacobianFactor(unittest.TestCase):
@ -35,12 +34,12 @@ class TestJacobianFactor(unittest.TestCase):
# the RHS # the RHS
b2 = np.array([-1., 1.5, 2., -1.]) b2 = np.array([-1., 1.5, 2., -1.])
sigmas = np.array([1., 1., 1., 1.]) sigmas = np.array([1., 1., 1., 1.])
model4 = noiseModel_Diagonal.Sigmas(sigmas) model4 = gtsam.noiseModel_Diagonal.Sigmas(sigmas)
combined = JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4) combined = gtsam.JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4)
# eliminate the first variable (x2) in the combined factor, destructive # eliminate the first variable (x2) in the combined factor, destructive
# ! # !
ord = Ordering() ord = gtsam.Ordering()
ord.push_back(x2) ord.push_back(x2)
actualCG, lf = combined.eliminate(ord) actualCG, lf = combined.eliminate(ord)
@ -52,8 +51,8 @@ class TestJacobianFactor(unittest.TestCase):
S13 = np.array([[-8.94427, 0.00], S13 = np.array([[-8.94427, 0.00],
[+0.00, -8.94427]]) [+0.00, -8.94427]])
d = np.array([2.23607, -1.56525]) d = np.array([2.23607, -1.56525])
expectedCG = GaussianConditional( expectedCG = gtsam.GaussianConditional(
x2, d, R11, l1, S12, x1, S13, noiseModel_Unit.Create(2)) x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2))
# check if the result matches # check if the result matches
self.assertTrue(actualCG.equals(expectedCG, 1e-4)) self.assertTrue(actualCG.equals(expectedCG, 1e-4))
@ -69,8 +68,8 @@ class TestJacobianFactor(unittest.TestCase):
# the RHS # the RHS
b1 = np.array([0.0, 0.894427]) b1 = np.array([0.0, 0.894427])
model2 = noiseModel_Diagonal.Sigmas(np.array([1., 1.])) model2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([1., 1.]))
expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2) expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2)
# check if the result matches the combined (reduced) factor # check if the result matches the combined (reduced) factor
self.assertTrue(lf.equals(expectedLF, 1e-4)) self.assertTrue(lf.equals(expectedLF, 1e-4))

9
cython/gtsam/tests/test_KalmanFilter.py
View File

@ -1,6 +1,5 @@
import unittest import unittest
from gtsam import * import gtsam
from math import *
import numpy as np import numpy as np
class TestKalmanFilter(unittest.TestCase): class TestKalmanFilter(unittest.TestCase):
@ -9,13 +8,13 @@ class TestKalmanFilter(unittest.TestCase):
F = np.eye(2) F = np.eye(2)
B = np.eye(2) B = np.eye(2)
u = np.array([1.0, 0.0]) 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) Q = 0.01 * np.eye(2)
H = np.eye(2) H = np.eye(2)
z1 = np.array([1.0, 0.0]) z1 = np.array([1.0, 0.0])
z2 = np.array([2.0, 0.0]) z2 = np.array([2.0, 0.0])
z3 = np.array([3.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) R = 0.01 * np.eye(2)
# Create the set of expected output TestValues # Create the set of expected output TestValues
@ -35,7 +34,7 @@ class TestKalmanFilter(unittest.TestCase):
I33 = np.linalg.inv(P23) + np.linalg.inv(R) I33 = np.linalg.inv(P23) + np.linalg.inv(R)
# Create an KalmanFilter object # Create an KalmanFilter object
KF = KalmanFilter(n=2) KF = gtsam.KalmanFilter(n=2)
# Create the Kalman Filter initialization point # Create the Kalman Filter initialization point
x_initial = np.array([0.0, 0.0]) x_initial = np.array([0.0, 0.0])

38
cython/gtsam/tests/test_LocalizationExample.py
View File

@ -1,7 +1,5 @@
import unittest import unittest
from gtsam import * import gtsam
from gtsam.utils import *
from math import *
import numpy as np import numpy as np
class TestLocalizationExample(unittest.TestCase): class TestLocalizationExample(unittest.TestCase):
@ -9,39 +7,39 @@ class TestLocalizationExample(unittest.TestCase):
def test_LocalizationExample(self): def test_LocalizationExample(self):
# Create the graph (defined in pose2SLAM.h, derived from # Create the graph (defined in pose2SLAM.h, derived from
# NonlinearFactorGraph) # NonlinearFactorGraph)
graph = NonlinearFactorGraph() graph = gtsam.NonlinearFactorGraph()
# Add two odometry factors # Add two odometry factors
# create a measurement for both factors (the same in this case) # create a measurement for both factors (the same in this case)
odometry = Pose2(2.0, 0.0, 0.0) odometry = gtsam.Pose2(2.0, 0.0, 0.0)
odometryNoise = noiseModel_Diagonal.Sigmas( odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta 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(gtsam.BetweenFactorPose2(0, 1, odometry, odometryNoise))
graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)) graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
# Add three "GPS" measurements # Add three "GPS" measurements
# We use Pose2 Priors here with high variance on theta # We use Pose2 Priors here with high variance on theta
groundTruth = Values() groundTruth = gtsam.Values()
groundTruth.insert(0, Pose2(0.0, 0.0, 0.0)) groundTruth.insert(0, gtsam.Pose2(0.0, 0.0, 0.0))
groundTruth.insert(1, Pose2(2.0, 0.0, 0.0)) groundTruth.insert(1, gtsam.Pose2(2.0, 0.0, 0.0))
groundTruth.insert(2, Pose2(4.0, 0.0, 0.0)) groundTruth.insert(2, gtsam.Pose2(4.0, 0.0, 0.0))
model = noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 10.])) model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 10.]))
for i in range(3): 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 # Initialize to noisy points
initialEstimate = Values() initialEstimate = gtsam.Values()
initialEstimate.insert(0, Pose2(0.5, 0.0, 0.2)) initialEstimate.insert(0, gtsam.Pose2(0.5, 0.0, 0.2))
initialEstimate.insert(1, Pose2(2.3, 0.1, -0.2)) initialEstimate.insert(1, gtsam.Pose2(2.3, 0.1, -0.2))
initialEstimate.insert(2, Pose2(4.1, 0.1, 0.1)) initialEstimate.insert(2, gtsam.Pose2(4.1, 0.1, 0.1))
# Optimize using Levenberg-Marquardt optimization with an ordering from # Optimize using Levenberg-Marquardt optimization with an ordering from
# colamd # colamd
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
result = optimizer.optimizeSafely() result = optimizer.optimizeSafely()
# Plot Covariance Ellipses # Plot Covariance Ellipses
marginals = Marginals(graph, result) marginals = gtsam.Marginals(graph, result)
P = [None] * result.size() P = [None] * result.size()
for i in range(0, result.size()): for i in range(0, result.size()):
pose_i = result.atPose2(i) pose_i = result.atPose2(i)

33
cython/gtsam/tests/test_OdometryExample.py
View File

@ -1,6 +1,5 @@
import unittest import unittest
from gtsam import * import gtsam
from math import *
import numpy as np import numpy as np
class TestOdometryExample(unittest.TestCase): class TestOdometryExample(unittest.TestCase):
@ -8,39 +7,39 @@ class TestOdometryExample(unittest.TestCase):
def test_OdometryExample(self): def test_OdometryExample(self):
# Create the graph (defined in pose2SLAM.h, derived from # Create the graph (defined in pose2SLAM.h, derived from
# NonlinearFactorGraph) # NonlinearFactorGraph)
graph = NonlinearFactorGraph() graph = gtsam.NonlinearFactorGraph()
# Add a Gaussian prior on pose x_1 # Add a Gaussian prior on pose x_1
priorMean = Pose2(0.0, 0.0, 0.0) # prior mean is at origin priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior mean is at origin
priorNoise = noiseModel_Diagonal.Sigmas( priorNoise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.3, 0.3, 0.1])) # 30cm std on x,y, 0.1 rad on theta np.array([0.3, 0.3, 0.1])) # 30cm std on x,y, 0.1 rad on theta
# add directly to graph # add directly to graph
graph.add(PriorFactorPose2(1, priorMean, priorNoise)) graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
# Add two odometry factors # Add two odometry factors
# create a measurement for both factors (the same in this case) # create a measurement for both factors (the same in this case)
odometry = Pose2(2.0, 0.0, 0.0) odometry = gtsam.Pose2(2.0, 0.0, 0.0)
odometryNoise = noiseModel_Diagonal.Sigmas( odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta 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(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)) graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))
# Initialize to noisy points # Initialize to noisy points
initialEstimate = Values() initialEstimate = gtsam.Values()
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)) initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)) initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)) initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
# Optimize using Levenberg-Marquardt optimization with an ordering from # Optimize using Levenberg-Marquardt optimization with an ordering from
# colamd # colamd
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
result = optimizer.optimizeSafely() result = optimizer.optimizeSafely()
marginals = Marginals(graph, result) marginals = gtsam.Marginals(graph, result)
marginals.marginalCovariance(1) marginals.marginalCovariance(1)
# Check first pose equality # Check first pose equality
pose_1 = result.atPose2(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__": if __name__ == "__main__":
unittest.main() unittest.main()

52
cython/gtsam/tests/test_PlanarSLAMExample.py
View File

@ -1,6 +1,6 @@
import unittest import unittest
from gtsam import * import gtsam
from math import * from math import pi
import numpy as np import numpy as np
class TestPose2SLAMExample(unittest.TestCase): class TestPose2SLAMExample(unittest.TestCase):
@ -13,50 +13,50 @@ class TestPose2SLAMExample(unittest.TestCase):
# - The robot is on a grid, moving 2 meters each step # - The robot is on a grid, moving 2 meters each step
# Create graph container and add factors to it # Create graph container and add factors to it
graph = NonlinearFactorGraph() graph = gtsam.NonlinearFactorGraph()
# Add prior # Add prior
# gaussian for prior # gaussian for prior
priorMean = Pose2(0.0, 0.0, 0.0) # prior at origin priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
priorNoise = noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
# add directly to graph # add directly to graph
graph.add(PriorFactorPose2(1, priorMean, priorNoise)) graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
# Add odometry # Add odometry
# general noisemodel for odometry # general noisemodel for odometry
odometryNoise = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph.add(BetweenFactorPose2( graph.add(gtsam.BetweenFactorPose2(
1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)) 1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise))
graph.add(BetweenFactorPose2( graph.add(gtsam.BetweenFactorPose2(
2, 3, Pose2(2.0, 0.0, pi / 2), odometryNoise)) 2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
graph.add(BetweenFactorPose2( graph.add(gtsam.BetweenFactorPose2(
3, 4, Pose2(2.0, 0.0, pi / 2), odometryNoise)) 3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
graph.add(BetweenFactorPose2( graph.add(gtsam.BetweenFactorPose2(
4, 5, Pose2(2.0, 0.0, pi / 2), odometryNoise)) 4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
# Add pose constraint # Add pose constraint
model = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi / 2), model)) graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model))
# Initialize to noisy points # Initialize to noisy points
initialEstimate = Values() initialEstimate = gtsam.Values()
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)) initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)) initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
initialEstimate.insert(3, Pose2(4.1, 0.1, pi / 2)) initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2))
initialEstimate.insert(4, Pose2(4.0, 2.0, pi)) initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi))
initialEstimate.insert(5, Pose2(2.1, 2.1, -pi / 2)) initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2))
# Optimize using Levenberg-Marquardt optimization with an ordering from # Optimize using Levenberg-Marquardt optimization with an ordering from
# colamd # colamd
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
result = optimizer.optimizeSafely() result = optimizer.optimizeSafely()
# Plot Covariance Ellipses # Plot Covariance Ellipses
marginals = Marginals(graph, result) marginals = gtsam.Marginals(graph, result)
P = marginals.marginalCovariance(1) P = marginals.marginalCovariance(1)
pose_1 = result.atPose2(1) pose_1 = result.atPose2(1)
self.assertTrue(pose_1.equals(Pose2(), 1e-4)) self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4))

52
cython/gtsam/tests/test_Pose2SLAMExample.py
View File

@ -1,6 +1,6 @@
import unittest import unittest
from gtsam import * import gtsam
from math import * from math import pi
import numpy as np import numpy as np
class TestPose2SLAMExample(unittest.TestCase): class TestPose2SLAMExample(unittest.TestCase):
@ -13,50 +13,50 @@ class TestPose2SLAMExample(unittest.TestCase):
# - The robot is on a grid, moving 2 meters each step # - The robot is on a grid, moving 2 meters each step
# Create graph container and add factors to it # Create graph container and add factors to it
graph = NonlinearFactorGraph() graph = gtsam.NonlinearFactorGraph()
# Add prior # Add prior
# gaussian for prior # gaussian for prior
priorMean = Pose2(0.0, 0.0, 0.0) # prior at origin priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
priorNoise = noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
# add directly to graph # add directly to graph
graph.add(PriorFactorPose2(1, priorMean, priorNoise)) graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
# Add odometry # Add odometry
# general noisemodel for odometry # general noisemodel for odometry
odometryNoise = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph.add(BetweenFactorPose2( graph.add(gtsam.BetweenFactorPose2(
1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)) 1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise))
graph.add(BetweenFactorPose2( graph.add(gtsam.BetweenFactorPose2(
2, 3, Pose2(2.0, 0.0, pi / 2), odometryNoise)) 2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
graph.add(BetweenFactorPose2( graph.add(gtsam.BetweenFactorPose2(
3, 4, Pose2(2.0, 0.0, pi / 2), odometryNoise)) 3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
graph.add(BetweenFactorPose2( graph.add(gtsam.BetweenFactorPose2(
4, 5, Pose2(2.0, 0.0, pi / 2), odometryNoise)) 4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
# Add pose constraint # Add pose constraint
model = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi / 2), model)) graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model))
# Initialize to noisy points # Initialize to noisy points
initialEstimate = Values() initialEstimate = gtsam.Values()
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)) initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)) initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
initialEstimate.insert(3, Pose2(4.1, 0.1, pi / 2)) initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2))
initialEstimate.insert(4, Pose2(4.0, 2.0, pi)) initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi))
initialEstimate.insert(5, Pose2(2.1, 2.1, -pi / 2)) initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2))
# Optimize using Levenberg-Marquardt optimization with an ordering from # Optimize using Levenberg-Marquardt optimization with an ordering from
# colamd # colamd
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
result = optimizer.optimizeSafely() result = optimizer.optimizeSafely()
# Plot Covariance Ellipses # Plot Covariance Ellipses
marginals = Marginals(graph, result) marginals = gtsam.Marginals(graph, result)
P = marginals.marginalCovariance(1) P = marginals.marginalCovariance(1)
pose_1 = result.atPose2(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__": if __name__ == "__main__":
unittest.main() unittest.main()

26
cython/gtsam/tests/test_Pose3SLAMExample.py
View File

@ -1,7 +1,7 @@
import unittest import unittest
from gtsam import *
from math import *
import numpy as np import numpy as np
import gtsam
from math import pi
from gtsam.utils.circlePose3 import * from gtsam.utils.circlePose3 import *
class TestPose3SLAMExample(unittest.TestCase): class TestPose3SLAMExample(unittest.TestCase):
@ -13,20 +13,20 @@ class TestPose3SLAMExample(unittest.TestCase):
p1 = hexagon.atPose3(1) p1 = hexagon.atPose3(1)
# create a Pose graph with one equality constraint and one measurement # create a Pose graph with one equality constraint and one measurement
fg = NonlinearFactorGraph() fg = gtsam.NonlinearFactorGraph()
fg.add(NonlinearEqualityPose3(0, p0)) fg.add(gtsam.NonlinearEqualityPose3(0, p0))
delta = p0.between(p1) 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])) 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(gtsam.BetweenFactorPose3(0, 1, delta, covariance))
fg.add(BetweenFactorPose3(1, 2, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance))
fg.add(BetweenFactorPose3(2, 3, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance))
fg.add(BetweenFactorPose3(3, 4, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(3, 4, delta, covariance))
fg.add(BetweenFactorPose3(4, 5, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(4, 5, delta, covariance))
fg.add(BetweenFactorPose3(5, 0, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(5, 0, delta, covariance))
# Create initial config # Create initial config
initial = Values() initial = gtsam.Values()
s = 0.10 s = 0.10
initial.insert(0, p0) initial.insert(0, p0)
initial.insert(1, hexagon.atPose3(1).retract(s * np.random.randn(6, 1))) 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))) initial.insert(5, hexagon.atPose3(5).retract(s * np.random.randn(6, 1)))
# optimize # optimize
optimizer = LevenbergMarquardtOptimizer(fg, initial) optimizer = gtsam.LevenbergMarquardtOptimizer(fg, initial)
result = optimizer.optimizeSafely() result = optimizer.optimizeSafely()
pose_1 = result.atPose3(1) pose_1 = result.atPose3(1)

15
cython/gtsam/tests/test_PriorFactor.py
View File

@ -1,24 +1,23 @@
import unittest import unittest
from gtsam import * import gtsam
from math import *
import numpy as np import numpy as np
class TestPriorFactor(unittest.TestCase): class TestPriorFactor(unittest.TestCase):
def test_PriorFactor(self): def test_PriorFactor(self):
values = Values() values = gtsam.Values()
key = 5 key = 5
priorPose3 = Pose3() priorPose3 = gtsam.Pose3()
model = noiseModel_Unit.Create(6) model = gtsam.noiseModel_Unit.Create(6)
factor = PriorFactorPose3(key, priorPose3, model) factor = gtsam.PriorFactorPose3(key, priorPose3, model)
values.insert(key, priorPose3) values.insert(key, priorPose3)
self.assertEqual(factor.error(values), 0) self.assertEqual(factor.error(values), 0)
key = 3 key = 3
priorVector = np.array([0., 0., 0.]) priorVector = np.array([0., 0., 0.])
model = noiseModel_Unit.Create(3) model = gtsam.noiseModel_Unit.Create(3)
factor = PriorFactorVector(key, priorVector, model) factor = gtsam.PriorFactorVector(key, priorVector, model)
values.insert(key, priorVector) values.insert(key, priorVector)
self.assertEqual(factor.error(values), 0) self.assertEqual(factor.error(values), 0)

24
cython/gtsam/tests/test_SFMExample.py
View File

@ -1,6 +1,6 @@
import unittest import unittest
from gtsam import * import gtsam
from math import * from gtsam import symbol
import numpy as np import numpy as np
import gtsam.utils.visual_data_generator as generator import gtsam.utils.visual_data_generator as generator
@ -18,26 +18,26 @@ class TestSFMExample(unittest.TestCase):
pointNoiseSigma = 0.1 pointNoiseSigma = 0.1
poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 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 # 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 i in range(len(data.Z)):
for k in range(len(data.Z[i])): for k in range(len(data.Z[i])):
j = data.J[i][k] j = data.J[i][k]
graph.add(GenericProjectionFactorCal3_S2( graph.add(gtsam.GenericProjectionFactorCal3_S2(
data.Z[i][k], measurementNoise, data.Z[i][k], measurementNoise,
symbol(ord('x'), i), symbol(ord('p'), j), data.K)) symbol(ord('x'), i), symbol(ord('p'), j), data.K))
posePriorNoise = noiseModel_Diagonal.Sigmas(poseNoiseSigmas) posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
graph.add(PriorFactorPose3(symbol(ord('x'), 0), graph.add(gtsam.PriorFactorPose3(symbol(ord('x'), 0),
truth.cameras[0].pose(), posePriorNoise)) truth.cameras[0].pose(), posePriorNoise))
pointPriorNoise = noiseModel_Isotropic.Sigma(3, pointNoiseSigma) pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, pointNoiseSigma)
graph.add(PriorFactorPoint3(symbol(ord('p'), 0), graph.add(gtsam.PriorFactorPoint3(symbol(ord('p'), 0),
truth.points[0], pointPriorNoise)) truth.points[0], pointPriorNoise))
# Initial estimate # Initial estimate
initialEstimate = Values() initialEstimate = gtsam.Values()
for i in range(len(truth.cameras)): for i in range(len(truth.cameras)):
pose_i = truth.cameras[i].pose() pose_i = truth.cameras[i].pose()
initialEstimate.insert(symbol(ord('x'), i), pose_i) initialEstimate.insert(symbol(ord('x'), i), pose_i)
@ -46,13 +46,13 @@ class TestSFMExample(unittest.TestCase):
initialEstimate.insert(symbol(ord('p'), j), point_j) initialEstimate.insert(symbol(ord('p'), j), point_j)
# Optimization # Optimization
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
for i in range(5): for i in range(5):
optimizer.iterate() optimizer.iterate()
result = optimizer.values() result = optimizer.values()
# Marginalization # Marginalization
marginals = Marginals(graph, result) marginals = gtsam.Marginals(graph, result)
marginals.marginalCovariance(symbol(ord('p'), 0)) marginals.marginalCovariance(symbol(ord('p'), 0))
marginals.marginalCovariance(symbol(ord('x'), 0)) marginals.marginalCovariance(symbol(ord('x'), 0))

38
cython/gtsam/tests/test_StereoVOExample.py
View File

@ -1,6 +1,6 @@
import unittest import unittest
from gtsam import * import gtsam
from math import * from gtsam import symbol
import numpy as np import numpy as np
@ -22,40 +22,40 @@ class TestStereoVOExample(unittest.TestCase):
l3 = symbol(ord('l'),3) l3 = symbol(ord('l'),3)
## Create graph container and add factors to it ## Create graph container and add factors to it
graph = NonlinearFactorGraph() graph = gtsam.NonlinearFactorGraph()
## add a constraint on the starting pose ## add a constraint on the starting pose
first_pose = Pose3() first_pose = gtsam.Pose3()
graph.add(NonlinearEqualityPose3(x1, first_pose)) graph.add(gtsam.NonlinearEqualityPose3(x1, first_pose))
## Create realistic calibration and measurement noise model ## Create realistic calibration and measurement noise model
# format: fx fy skew cx cy baseline # format: fx fy skew cx cy baseline
K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2) K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
stereo_model = noiseModel_Diagonal.Sigmas(np.array([1.0, 1.0, 1.0])) stereo_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.0, 1.0, 1.0]))
## Add measurements ## Add measurements
# pose 1 # pose 1
graph.add(GenericStereoFactor3D(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K)) graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(520, 480, 440), stereo_model, x1, l1, K))
graph.add(GenericStereoFactor3D(StereoPoint2(120, 80, 440), stereo_model, x1, l2, K)) graph.add(gtsam.GenericStereoFactor3D(gtsam.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(320, 280, 140), stereo_model, x1, l3, K))
#pose 2 #pose 2
graph.add(GenericStereoFactor3D(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K)) graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(570, 520, 490), stereo_model, x2, l1, K))
graph.add(GenericStereoFactor3D(StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K)) graph.add(gtsam.GenericStereoFactor3D(gtsam.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(320, 270, 115), stereo_model, x2, l3, K))
## Create initial estimate for camera poses and landmarks ## Create initial estimate for camera poses and landmarks
initialEstimate = Values() initialEstimate = gtsam.Values()
initialEstimate.insert(x1, first_pose) initialEstimate.insert(x1, first_pose)
# noisy estimate for pose 2 # noisy estimate for pose 2
initialEstimate.insert(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1))) initialEstimate.insert(x2, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1,-.1,1.1)))
expected_l1 = Point3( 1, 1, 5) expected_l1 = gtsam.Point3( 1, 1, 5)
initialEstimate.insert(l1, expected_l1) initialEstimate.insert(l1, expected_l1)
initialEstimate.insert(l2, Point3(-1, 1, 5)) initialEstimate.insert(l2, gtsam.Point3(-1, 1, 5))
initialEstimate.insert(l3, Point3( 0,-.5, 5)) initialEstimate.insert(l3, gtsam.Point3( 0,-.5, 5))
## optimize ## optimize
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
result = optimizer.optimize() result = optimizer.optimize()
## check equality for the first pose and point ## check equality for the first pose and point

47
cython/gtsam/tests/test_Values.py
View File

@ -1,26 +1,25 @@
import unittest import unittest
from gtsam import * import gtsam
from math import *
import numpy as np import numpy as np
class TestValues(unittest.TestCase): class TestValues(unittest.TestCase):
def test_values(self): def test_values(self):
values = Values() values = gtsam.Values()
E = EssentialMatrix(Rot3(), Unit3()) E = gtsam.EssentialMatrix(gtsam.Rot3(), gtsam.Unit3())
tol = 1e-9 tol = 1e-9
values.insert(0, Point2()) values.insert(0, gtsam.Point2())
values.insert(1, Point3()) values.insert(1, gtsam.Point3())
values.insert(2, Rot2()) values.insert(2, gtsam.Rot2())
values.insert(3, Pose2()) values.insert(3, gtsam.Pose2())
values.insert(4, Rot3()) values.insert(4, gtsam.Rot3())
values.insert(5, Pose3()) values.insert(5, gtsam.Pose3())
values.insert(6, Cal3_S2()) values.insert(6, gtsam.Cal3_S2())
values.insert(7, Cal3DS2()) values.insert(7, gtsam.Cal3DS2())
values.insert(8, Cal3Bundler()) values.insert(8, gtsam.Cal3Bundler())
values.insert(9, E) values.insert(9, E)
values.insert(10, imuBias_ConstantBias()) values.insert(10, gtsam.imuBias_ConstantBias())
# Special cases for Vectors and Matrices # Special cases for Vectors and Matrices
# Note that gtsam's Eigen Vectors and Matrices requires double-precision # 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]]) mat2 = np.array([[1,2,],[3,5]])
values.insert(13, mat2) values.insert(13, mat2)
self.assertTrue(values.atPoint2(0).equals(Point2(), tol)) self.assertTrue(values.atPoint2(0).equals(gtsam.Point2(), tol))
self.assertTrue(values.atPoint3(1).equals(Point3(), tol)) self.assertTrue(values.atPoint3(1).equals(gtsam.Point3(), tol))
self.assertTrue(values.atRot2(2).equals(Rot2(), tol)) self.assertTrue(values.atRot2(2).equals(gtsam.Rot2(), tol))
self.assertTrue(values.atPose2(3).equals(Pose2(), tol)) self.assertTrue(values.atPose2(3).equals(gtsam.Pose2(), tol))
self.assertTrue(values.atRot3(4).equals(Rot3(), tol)) self.assertTrue(values.atRot3(4).equals(gtsam.Rot3(), tol))
self.assertTrue(values.atPose3(5).equals(Pose3(), tol)) self.assertTrue(values.atPose3(5).equals(gtsam.Pose3(), tol))
self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol)) self.assertTrue(values.atCal3_S2(6).equals(gtsam.Cal3_S2(), tol))
self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol)) self.assertTrue(values.atCal3DS2(7).equals(gtsam.Cal3DS2(), tol))
self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol)) self.assertTrue(values.atCal3Bundler(8).equals(gtsam.Cal3Bundler(), tol))
self.assertTrue(values.atEssentialMatrix(9).equals(E, tol)) self.assertTrue(values.atEssentialMatrix(9).equals(E, tol))
self.assertTrue(values.atimuBias_ConstantBias( self.assertTrue(values.atimuBias_ConstantBias(
10).equals(imuBias_ConstantBias(), tol)) 10).equals(gtsam.imuBias_ConstantBias(), tol))
# special cases for Vector and Matrix: # special cases for Vector and Matrix:
actualVector = values.atVector(11) actualVector = values.atVector(11)

3
cython/gtsam/tests/test_VisualISAMExample.py
View File

@ -1,7 +1,6 @@
import unittest import unittest
from gtsam import *
from math import *
import numpy as np import numpy as np
from gtsam import symbol
import gtsam.utils.visual_data_generator as generator import gtsam.utils.visual_data_generator as generator
import gtsam.utils.visual_isam as visual_isam import gtsam.utils.visual_isam as visual_isam

4
cython/gtsam/utils/circlePose3.py
View File

@ -1,6 +1,6 @@
from gtsam import * import gtsam
from math import *
import numpy as np import numpy as np
from math import pi, cos, sin
def circlePose3(numPoses = 8, radius = 1.0, symbolChar = 0): def circlePose3(numPoses = 8, radius = 1.0, symbolChar = 0):
""" """

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

@ -1,14 +1,14 @@
from np_utils import * import numpy as np
from math import * from math import pi, cos, sin
from gtsam import * import gtsam
from gtsam import symbol
class Options: class Options:
""" """
Options to generate test scenario 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 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
@ -25,10 +25,10 @@ class GroundTruth:
Object holding generated ground-truth data 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.K = K
self.cameras = [Pose3()] * nrCameras self.cameras = [gtsam.Pose3()] * nrCameras
self.points = [Point3()] * nrPoints self.points = [gtsam.Point3()] * nrPoints
def print_(self, s=""): def print_(self, s=""):
print s print s
@ -50,22 +50,22 @@ class Data:
class NoiseModels: class NoiseModels:
pass 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.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.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
self.odometry = [Pose3()] * nrCameras self.odometry = [gtsam.Pose3()] * nrCameras
# Set Noise parameters # Set Noise parameters
self.noiseModels = Data.NoiseModels() 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])) 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])) # 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])) 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.pointPrior = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
self.noiseModels.measurement = noiseModel_Isotropic.Sigma(2, 1.0) self.noiseModels.measurement = gtsam.noiseModel_Isotropic.Sigma(2, 1.0)
def generate_data(options): def generate_data(options):
@ -73,7 +73,7 @@ def generate_data(options):
Generate ground-truth and measurement data 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 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)
@ -84,25 +84,25 @@ def generate_data(options):
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 * 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 else: # 3D landmarks as vertices of a cube
truth.points = [Point3(10, 10, 10), truth.points = [gtsam.Point3(10, 10, 10),
Point3(-10, 10, 10), gtsam.Point3(-10, 10, 10),
Point3(-10, -10, 10), gtsam.Point3(-10, -10, 10),
Point3(10, -10, 10), gtsam.Point3(10, -10, 10),
Point3(10, 10, -10), gtsam.Point3(10, 10, -10),
Point3(-10, 10, -10), gtsam.Point3(-10, 10, -10),
Point3(-10, -10, -10), gtsam.Point3(-10, -10, -10),
Point3(10, -10, -10)] gtsam.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 * pi / options.nrCameras
t = Point3(r * cos(theta), r * sin(theta), height) t = gtsam.Point3(r * cos(theta), r * sin(theta), height)
truth.cameras[i] = SimpleCamera.Lookat( truth.cameras[i] = gtsam.SimpleCamera.Lookat(
t, Point3(), Point3(0, 0, 1), truth.K) t, gtsam.Point3(), gtsam.Point3(0, 0, 1), 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

36
cython/gtsam/utils/visual_isam.py
View File

@ -1,7 +1,5 @@
from np_utils import * import gtsam
from math import * from gtsam import symbol
from gtsam import *
class Options: class Options:
""" """
@ -20,20 +18,20 @@ def initialize(data, truth, options):
params = gtsam.ISAM2Params() params = gtsam.ISAM2Params()
if options.alwaysRelinearize: if options.alwaysRelinearize:
params.setRelinearizeSkip(1) params.setRelinearizeSkip(1)
isam = ISAM2(params = params) isam = gtsam.ISAM2(params = params)
# Add constraints/priors # Add constraints/priors
# TODO: should not be from ground truth! # TODO: should not be from ground truth!
newFactors = NonlinearFactorGraph() newFactors = gtsam.NonlinearFactorGraph()
initialEstimates = Values() initialEstimates = gtsam.Values()
for i in range(2): for i in range(2):
ii = symbol(ord('x'), i) ii = symbol(ord('x'), i)
if i == 0: if i == 0:
if options.hardConstraint: # add hard constraint if options.hardConstraint: # add hard constraint
newFactors.add(NonlinearEqualityPose3( newFactors.add(gtsam.NonlinearEqualityPose3(
ii, truth.cameras[0].pose())) ii, truth.cameras[0].pose()))
else: else:
newFactors.add(PriorFactorPose3( newFactors.add(gtsam.PriorFactorPose3(
ii, truth.cameras[i].pose(), data.noiseModels.posePrior)) ii, truth.cameras[i].pose(), data.noiseModels.posePrior))
initialEstimates.insert(ii, truth.cameras[i].pose()) initialEstimates.insert(ii, truth.cameras[i].pose())
@ -46,22 +44,22 @@ def initialize(data, truth, options):
for k in range(len(data.Z[i])): for k in range(len(data.Z[i])):
j = data.J[i][k] j = data.J[i][k]
jj = symbol(ord('l'), j) 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)) data.Z[i][k], data.noiseModels.measurement, ii, jj, data.K))
# TODO: initial estimates should not be from ground truth! # TODO: initial estimates should not be from ground truth!
if not initialEstimates.exists(jj): if not initialEstimates.exists(jj):
initialEstimates.insert(jj, truth.points[j]) initialEstimates.insert(jj, truth.points[j])
if options.pointPriors: # add point priors if options.pointPriors: # add point priors
newFactors.add(PriorFactorPoint3( newFactors.add(gtsam.PriorFactorPoint3(
jj, truth.points[j], data.noiseModels.pointPrior)) jj, truth.points[j], data.noiseModels.pointPrior))
# Add odometry between frames 0 and 1 # 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)) ord('x'), 1), data.odometry[1], data.noiseModels.odometry))
# Update ISAM # Update ISAM
if options.batchInitialization: # Do a full optimize for first two poses if options.batchInitialization: # Do a full optimize for first two poses
batchOptimizer = LevenbergMarquardtOptimizer( batchOptimizer = gtsam.LevenbergMarquardtOptimizer(
newFactors, initialEstimates) newFactors, initialEstimates)
fullyOptimized = batchOptimizer.optimize() fullyOptimized = batchOptimizer.optimize()
isam.update(newFactors, fullyOptimized) 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 # iSAM expects us to give it a new set of factors
# along with initial estimates for any new variables introduced. # along with initial estimates for any new variables introduced.
newFactors = NonlinearFactorGraph() newFactors = gtsam.NonlinearFactorGraph()
initialEstimates = Values() initialEstimates = gtsam.Values()
# Add odometry # Add odometry
prevPoseIndex = currPoseIndex - 1 prevPoseIndex = currPoseIndex - 1
odometry = data.odometry[prevPoseIndex] odometry = data.odometry[prevPoseIndex]
newFactors.add(BetweenFactorPose3(symbol(ord('x'), prevPoseIndex), newFactors.add(gtsam.BetweenFactorPose3(symbol(ord('x'), prevPoseIndex),
symbol(ord('x'), currPoseIndex), symbol(ord('x'), currPoseIndex),
odometry, data.noiseModels.odometry)) odometry, data.noiseModels.odometry))
# Add visual measurement factors and initializations as necessary # Add visual measurement factors and initializations as necessary
for k in range(len(data.Z[currPoseIndex])): for k in range(len(data.Z[currPoseIndex])):
zij = data.Z[currPoseIndex][k] zij = data.Z[currPoseIndex][k]
j = data.J[currPoseIndex][k] j = data.J[currPoseIndex][k]
jj = symbol(ord('l'), j) jj = symbol(ord('l'), j)
newFactors.add(GenericProjectionFactorCal3_S2( newFactors.add(gtsam.GenericProjectionFactorCal3_S2(
zij, data.noiseModels.measurement, zij, data.noiseModels.measurement,
symbol(ord('x'), currPoseIndex), jj, data.K)) symbol(ord('x'), currPoseIndex), jj, data.K))
# TODO: initialize with something other than truth # TODO: initialize with something other than truth