resolve overloads via type checking, simplify Values's insert and update, more friendly Matrix and Vector utils
Keyword arguments are not needed anymorerelease/4.3a0
parent
f4e745ff0f
commit
5958b2397c
|
@ -1846,48 +1846,48 @@ class Values {
|
|||
// void update(size_t j, const gtsam::Value& val);
|
||||
// gtsam::Value at(size_t j) const;
|
||||
|
||||
template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
|
||||
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
|
||||
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
|
||||
gtsam::SimpleCamera, gtsam::imuBias::ConstantBias,
|
||||
Vector, Matrix}>
|
||||
void insert(size_t j, const T& t);
|
||||
// template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
|
||||
// gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
|
||||
// gtsam::Cal3Bundler, gtsam::EssentialMatrix,
|
||||
// gtsam::SimpleCamera, gtsam::imuBias::ConstantBias,
|
||||
// Vector, Matrix}>
|
||||
// void insert(size_t j, const T& t);
|
||||
|
||||
template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
|
||||
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
|
||||
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
|
||||
gtsam::SimpleCamera, gtsam::imuBias::ConstantBias,
|
||||
Vector, Matrix}>
|
||||
void update(size_t j, const T& t);
|
||||
// template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
|
||||
// gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
|
||||
// gtsam::Cal3Bundler, gtsam::EssentialMatrix,
|
||||
// gtsam::SimpleCamera, gtsam::imuBias::ConstantBias,
|
||||
// Vector, Matrix}>
|
||||
// void update(size_t j, const T& t);
|
||||
|
||||
// void insert(size_t j, const gtsam::Point2& t);
|
||||
// void insert(size_t j, const gtsam::Point3& t);
|
||||
// void insert(size_t j, const gtsam::Rot2& t);
|
||||
// void insert(size_t j, const gtsam::Pose2& t);
|
||||
// void insert(size_t j, const gtsam::Rot3& t);
|
||||
// void insert(size_t j, const gtsam::Pose3& t);
|
||||
// void insert(size_t j, const gtsam::Cal3_S2& t);
|
||||
// void insert(size_t j, const gtsam::Cal3DS2& t);
|
||||
// void insert(size_t j, const gtsam::Cal3Bundler& t);
|
||||
// void insert(size_t j, const gtsam::EssentialMatrix& t);
|
||||
// void insert(size_t j, const gtsam::SimpleCamera& t);
|
||||
// void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
|
||||
// void insert(size_t j, Vector t);
|
||||
// void insert(size_t j, Matrix t);
|
||||
void insert(size_t j, const gtsam::Point2& t);
|
||||
void insert(size_t j, const gtsam::Point3& t);
|
||||
void insert(size_t j, const gtsam::Rot2& t);
|
||||
void insert(size_t j, const gtsam::Pose2& t);
|
||||
void insert(size_t j, const gtsam::Rot3& t);
|
||||
void insert(size_t j, const gtsam::Pose3& t);
|
||||
void insert(size_t j, const gtsam::Cal3_S2& t);
|
||||
void insert(size_t j, const gtsam::Cal3DS2& t);
|
||||
void insert(size_t j, const gtsam::Cal3Bundler& t);
|
||||
void insert(size_t j, const gtsam::EssentialMatrix& t);
|
||||
void insert(size_t j, const gtsam::SimpleCamera& t);
|
||||
void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
|
||||
void insert(size_t j, Vector t);
|
||||
void insert(size_t j, Matrix t);
|
||||
|
||||
// void update(size_t j, const gtsam::Point2& t);
|
||||
// void update(size_t j, const gtsam::Point3& t);
|
||||
// void update(size_t j, const gtsam::Rot2& t);
|
||||
// void update(size_t j, const gtsam::Pose2& t);
|
||||
// void update(size_t j, const gtsam::Rot3& t);
|
||||
// void update(size_t j, const gtsam::Pose3& t);
|
||||
// void update(size_t j, const gtsam::Cal3_S2& t);
|
||||
// void update(size_t j, const gtsam::Cal3DS2& t);
|
||||
// void update(size_t j, const gtsam::Cal3Bundler& t);
|
||||
// void update(size_t j, const gtsam::EssentialMatrix& t);
|
||||
// void update(size_t j, const gtsam::imuBias::ConstantBias& t);
|
||||
// void update(size_t j, Vector t);
|
||||
// void update(size_t j, Matrix t);
|
||||
void update(size_t j, const gtsam::Point2& t);
|
||||
void update(size_t j, const gtsam::Point3& t);
|
||||
void update(size_t j, const gtsam::Rot2& t);
|
||||
void update(size_t j, const gtsam::Pose2& t);
|
||||
void update(size_t j, const gtsam::Rot3& t);
|
||||
void update(size_t j, const gtsam::Pose3& t);
|
||||
void update(size_t j, const gtsam::Cal3_S2& t);
|
||||
void update(size_t j, const gtsam::Cal3DS2& t);
|
||||
void update(size_t j, const gtsam::Cal3Bundler& t);
|
||||
void update(size_t j, const gtsam::EssentialMatrix& t);
|
||||
void update(size_t j, const gtsam::imuBias::ConstantBias& t);
|
||||
void update(size_t j, Vector t);
|
||||
void update(size_t j, Matrix t);
|
||||
|
||||
template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
|
||||
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
|
||||
|
|
|
@ -23,12 +23,12 @@ def circlePose3(numPoses = 8, radius = 1.0, 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.]]))
|
||||
gRo = gtsam.Rot3(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)
|
||||
values.insert(key, gTi)
|
||||
theta = theta + dtheta
|
||||
return values
|
|
@ -1,4 +1,27 @@
|
|||
import numpy as np
|
||||
|
||||
def Vector(list1d): return np.array(list1d, dtype = 'float')
|
||||
def Matrix(list2d): return np.array(list2d, dtype = 'float', order = 'F')
|
||||
|
||||
def Vector(*args):
|
||||
"""
|
||||
Convenient function to create numpy vector to use with gtsam cython wrapper
|
||||
Usage: Vector(1), Vector(1,2,3), Vector(3,2,4)
|
||||
"""
|
||||
ret = np.asarray(args, dtype='float')
|
||||
while ret.ndim >= 2:
|
||||
ret = ret[0, :]
|
||||
return ret
|
||||
|
||||
|
||||
def Matrix(*args):
|
||||
"""
|
||||
Convenient function to create numpy matrix to use with gtsam cython wrapper
|
||||
Usage: Matrix([1])
|
||||
Matrix([1,2,3])
|
||||
Matrix((3,2,4))
|
||||
Matrix([1,2,3],[2,3,4])
|
||||
Matrix((1,2,3),(2,3,4))
|
||||
"""
|
||||
ret = np.asarray(args, dtype='float', order='F')
|
||||
if ret.ndim == 1:
|
||||
ret = np.expand_dims(ret, axis=0)
|
||||
return ret
|
||||
|
|
|
@ -87,23 +87,23 @@ def generate_data(options):
|
|||
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]))]
|
||||
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)]
|
||||
|
||||
# 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]))
|
||||
t = Point3(Vector(r * cos(theta), r * sin(theta), height))
|
||||
truth.cameras[i] = SimpleCamera.Lookat(
|
||||
t, Point3(), Point3(v=Vector([0, 0, 1])), truth.K)
|
||||
t, Point3(), Point3(Vector(0, 0, 1)), truth.K)
|
||||
# Create measurements
|
||||
for j in range(nrPoints):
|
||||
# All landmarks seen in every frame
|
||||
|
|
|
@ -35,7 +35,7 @@ def initialize(data, truth, options):
|
|||
else:
|
||||
newFactors.add(PriorFactorPose3(
|
||||
ii, truth.cameras[i].pose(), data.noiseModels.posePrior))
|
||||
initialEstimates.insertPose3(ii, truth.cameras[i].pose())
|
||||
initialEstimates.insert(ii, truth.cameras[i].pose())
|
||||
|
||||
nextPoseIndex = 2
|
||||
|
||||
|
@ -50,7 +50,7 @@ def initialize(data, truth, options):
|
|||
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])
|
||||
initialEstimates.insert(jj, truth.points[j])
|
||||
if options.pointPriors: # add point priors
|
||||
newFactors.add(PriorFactorPoint3(
|
||||
jj, truth.points[j], data.noiseModels.pointPrior))
|
||||
|
@ -112,7 +112,7 @@ def step(data, isam, result, truth, currPoseIndex):
|
|||
|
||||
# Initial estimates for the new pose.
|
||||
prevPose = result.atPose3(symbol(ord('x'), prevPoseIndex))
|
||||
initialEstimates.insertPose3(symbol(ord('x'), currPoseIndex),
|
||||
initialEstimates.insert(symbol(ord('x'), currPoseIndex),
|
||||
prevPose.compose(odometry))
|
||||
|
||||
# Update ISAM
|
||||
|
|
|
@ -7,34 +7,33 @@ from gtsam_utils import Matrix, Vector
|
|||
class TestJacobianFactor(unittest.TestCase):
|
||||
|
||||
def test_eliminate(self):
|
||||
Ax2 = Matrix([
|
||||
Ax2 = Matrix(
|
||||
[-5., 0.],
|
||||
[+0., -5.],
|
||||
[10., 0.],
|
||||
[+0., 10.]])
|
||||
[+0., 10.])
|
||||
|
||||
Al1 = Matrix([
|
||||
Al1 = Matrix(
|
||||
[5., 0.],
|
||||
[0., 5.],
|
||||
[0., 0.],
|
||||
[0., 0.]])
|
||||
[0., 0.])
|
||||
|
||||
Ax1 = Matrix([
|
||||
Ax1 = Matrix(
|
||||
[0.00, 0.], # f4
|
||||
[0.00, 0.], # f4
|
||||
[-10., 0.], # f2
|
||||
[0.00, -10.]]) # 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.])
|
||||
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)
|
||||
combined = JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4)
|
||||
|
||||
# eliminate the first variable (x2) in the combined factor, destructive
|
||||
# !
|
||||
|
@ -43,30 +42,29 @@ class TestJacobianFactor(unittest.TestCase):
|
|||
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])
|
||||
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]])
|
||||
Bl1 = Matrix([4.47214, 0.00],
|
||||
[0.00, 4.47214])
|
||||
|
||||
Bx1 = Matrix([
|
||||
Bx1 = Matrix(
|
||||
# x1
|
||||
[-4.47214, 0.00],
|
||||
[+0.00, -4.47214]])
|
||||
[+0.00, -4.47214])
|
||||
|
||||
# the RHS
|
||||
b1 = Vector([0.0, 0.894427])
|
||||
b1 = Vector(0.0, 0.894427)
|
||||
|
||||
model2 = noiseModel_Diagonal.Sigmas(np.array([1., 1.]))
|
||||
expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2)
|
||||
|
|
|
@ -1,5 +1,6 @@
|
|||
import unittest
|
||||
from gtsam import *
|
||||
from gtsam_utils import *
|
||||
from math import *
|
||||
import numpy as np
|
||||
|
||||
|
@ -21,18 +22,18 @@ class TestLocalizationExample(unittest.TestCase):
|
|||
# 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))
|
||||
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.]))
|
||||
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))
|
||||
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))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
|
|
|
@ -27,9 +27,9 @@ class TestOdometryExample(unittest.TestCase):
|
|||
|
||||
# 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))
|
||||
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))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
|
|
|
@ -40,11 +40,11 @@ class TestPose2SLAMExample(unittest.TestCase):
|
|||
|
||||
# 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))
|
||||
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))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
|
|
|
@ -40,11 +40,11 @@ class TestPose2SLAMExample(unittest.TestCase):
|
|||
|
||||
# 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))
|
||||
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))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
|
|
|
@ -28,12 +28,12 @@ class TestPose3SLAMExample(unittest.TestCase):
|
|||
# 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)))
|
||||
initial.insert(0, p0)
|
||||
initial.insert(1, hexagon.atPose3(1).retract(s * np.random.randn(6, 1)))
|
||||
initial.insert(2, hexagon.atPose3(2).retract(s * np.random.randn(6, 1)))
|
||||
initial.insert(3, hexagon.atPose3(3).retract(s * np.random.randn(6, 1)))
|
||||
initial.insert(4, hexagon.atPose3(4).retract(s * np.random.randn(6, 1)))
|
||||
initial.insert(5, hexagon.atPose3(5).retract(s * np.random.randn(6, 1)))
|
||||
|
||||
# optimize
|
||||
optimizer = LevenbergMarquardtOptimizer(fg, initial)
|
||||
|
|
|
@ -12,14 +12,14 @@ class TestPriorFactor(unittest.TestCase):
|
|||
priorPose3 = Pose3()
|
||||
model = noiseModel_Unit.Create(6)
|
||||
factor = PriorFactorPose3(key, priorPose3, model)
|
||||
values.insertPose3(key, priorPose3)
|
||||
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)
|
||||
values.insertVector(key, priorVector)
|
||||
values.insert(key, priorVector)
|
||||
self.assertEqual(factor.error(values), 0)
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
@ -41,10 +41,10 @@ class TestSFMExample(unittest.TestCase):
|
|||
initialEstimate = Values()
|
||||
for i in range(len(truth.cameras)):
|
||||
pose_i = truth.cameras[i].pose()
|
||||
initialEstimate.insertPose3(symbol(ord('x'), i), pose_i)
|
||||
initialEstimate.insert(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)
|
||||
initialEstimate.insert(symbol(ord('p'), j), point_j)
|
||||
|
||||
# Optimization
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
|
|
|
@ -47,13 +47,13 @@ class TestStereoVOExample(unittest.TestCase):
|
|||
|
||||
## Create initial estimate for camera poses and landmarks
|
||||
initialEstimate = Values()
|
||||
initialEstimate.insertPose3(x1, first_pose)
|
||||
initialEstimate.insert(x1, first_pose)
|
||||
# noisy estimate for pose 2
|
||||
initialEstimate.insertPose3(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1)))
|
||||
initialEstimate.insert(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))
|
||||
initialEstimate.insert(l1, expected_l1)
|
||||
initialEstimate.insert(l2, Point3(-1, 1, 5))
|
||||
initialEstimate.insert(l3, Point3( 0,-.5, 5))
|
||||
|
||||
## optimize
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
|
|
|
@ -7,26 +7,26 @@ class TestValues(unittest.TestCase):
|
|||
|
||||
def test_values(self):
|
||||
values = Values()
|
||||
E = EssentialMatrix(Rot3, Unit3)
|
||||
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())
|
||||
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(9, E)
|
||||
values.insert(10, imuBias_ConstantBias())
|
||||
|
||||
# special cases for Vector and Matrix:
|
||||
vec = np.array([1., 2., 3.])
|
||||
values.insertVector(11, vec)
|
||||
values.insert(11, vec)
|
||||
mat = np.array([[1., 2.], [3., 4.]], order='F')
|
||||
values.insertMatrix(12, mat)
|
||||
values.insert(12, mat)
|
||||
|
||||
self.assertTrue(values.atPoint2(0).equals(Point2(), tol))
|
||||
self.assertTrue(values.atPoint3(1).equals(Point3(), tol))
|
||||
|
|
|
@ -827,7 +827,6 @@ void Class::emit_cython_pyx(FileWriter& pyxFile, const std::vector<Class>& allCl
|
|||
"\t\tself." << shared_pxd_obj_in_pyx() << " = "
|
||||
<< shared_pxd_class_in_pyx() << "()\n";
|
||||
|
||||
pyxFile.oss << constructor.pyx_checkDuplicateNargsKwArgs();
|
||||
for (size_t i = 0; i<constructor.nrOverloads(); ++i) {
|
||||
pyxFile.oss << "\t\t" << (i == 0 ? "if" : "elif") << " self."
|
||||
<< pyxClassName() << "_" << i
|
||||
|
|
|
@ -185,7 +185,6 @@ void GlobalFunction::emit_cython_pyx(FileWriter& file) const {
|
|||
|
||||
// Dealing with overloads..
|
||||
file.oss << "def " << funcName << "(*args, **kwargs):\n";
|
||||
file.oss << pyx_checkDuplicateNargsKwArgs(1);
|
||||
for (size_t i = 0; i < N; ++i) {
|
||||
file.oss << "\tsuccess, results = " << funcName << "_" << i
|
||||
<< "(*args, **kwargs)\n";
|
||||
|
|
|
@ -143,7 +143,6 @@ void Method::emit_cython_pyx(FileWriter& file, const Class& cls) const {
|
|||
|
||||
// Dealing with overloads..
|
||||
file.oss << "\tdef " << instantiatedName << "(self, *args, **kwargs):\n";
|
||||
file.oss << pyx_checkDuplicateNargsKwArgs();
|
||||
for (size_t i = 0; i < N; ++i) {
|
||||
file.oss << "\t\tsuccess, results = self." << instantiatedName << "_" << i
|
||||
<< "(*args, **kwargs)\n";
|
||||
|
|
|
@ -83,6 +83,18 @@ public:
|
|||
s += indent + "__names = [" + args.pyx_paramsList() + "]\n";
|
||||
s += indent + "for i in range(len(args)):\n";
|
||||
s += indent + "\t__params[__names[i]] = args[i]\n";
|
||||
for (size_t i = 0; i<args.size(); ++i) {
|
||||
if (args[i].type.isNonBasicType() || args[i].type.isEigen()) {
|
||||
std::string param = "__params[__names[" + std::to_string(i) + "]]";
|
||||
s += indent + "if not isinstance(" + param + ", " +
|
||||
args[i].type.pyxArgumentType() + ")";
|
||||
if (args[i].type.isEigen())
|
||||
s += " or not " + param + ".ndim == " +
|
||||
((args[i].type.pyxClassName() == "Vector") ? "1" : "2");
|
||||
s += ":\n";
|
||||
s += indent + "\treturn False" + ((isVoid) ? "" : ", None") + "\n";
|
||||
}
|
||||
}
|
||||
s += indent + "try:\n";
|
||||
s += args.pyx_castParamsToPythonType();
|
||||
s += indent + "except:\n";
|
||||
|
@ -91,36 +103,6 @@ public:
|
|||
}
|
||||
return s;
|
||||
}
|
||||
|
||||
/// if two overloading methods have the same number of arguments, they have
|
||||
/// to be resolved via keyword args
|
||||
std::string pyx_checkDuplicateNargsKwArgs(size_t indentLevel = 2) const {
|
||||
std::string indent;
|
||||
for (size_t i = 0; i<indentLevel; ++i) indent += "\t";
|
||||
std::unordered_set<size_t> nargsSet;
|
||||
std::vector<size_t> nargsDuplicates;
|
||||
for (size_t i = 0; i < nrOverloads(); ++i) {
|
||||
size_t nargs = argumentList(i).size();
|
||||
if (nargsSet.find(nargs) != nargsSet.end())
|
||||
nargsDuplicates.push_back(nargs);
|
||||
else
|
||||
nargsSet.insert(nargs);
|
||||
}
|
||||
|
||||
std::string s;
|
||||
if (nargsDuplicates.size() > 0) {
|
||||
s += indent + "if len(kwargs)==0 and len(args)+len(kwargs) in [";
|
||||
for (size_t i = 0; i < nargsDuplicates.size(); ++i) {
|
||||
s += std::to_string(nargsDuplicates[i]);
|
||||
if (i < nargsDuplicates.size() - 1) s += ",";
|
||||
}
|
||||
s += "]:\n";
|
||||
s += indent + "\traise TypeError('Overloads with the same number of "
|
||||
"arguments exist. Please use keyword arguments to "
|
||||
"differentiate them!')\n";
|
||||
}
|
||||
return s;
|
||||
}
|
||||
};
|
||||
|
||||
class OverloadedFunction : public Function, public ArgumentOverloads {
|
||||
|
|
|
@ -99,7 +99,6 @@ void StaticMethod::emit_cython_pyx(FileWriter& file, const Class& cls) const {
|
|||
// Dealing with overloads..
|
||||
file.oss << "\t@staticmethod\n";
|
||||
file.oss << "\tdef " << name_ << "(*args, **kwargs):\n";
|
||||
file.oss << pyx_checkDuplicateNargsKwArgs();
|
||||
for (size_t i = 0; i < N; ++i) {
|
||||
string funcName = name_ + "_" + to_string(i);
|
||||
file.oss << "\t\tsuccess, results = " << cls.pyxClassName() << "."
|
||||
|
|
Loading…
Reference in New Issue