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

Fixed overloaded methods/constructors

release/4.3a0
Frank Dellaert 2017-12-02 18:43:18 -08:00
parent 0dd7dcdc9f
commit 4188a739ec
7 changed files with 80 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cython/README.md
View File

@ -17,7 +17,7 @@ by default: <your CMAKE_INSTALL_PREFIX>/cython
- Modify your PYTHONPATH to include the GTSAM_CYTHON_INSTALL_PATH:
```bash
export PYTHONPATH = $PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH>
export PYTHONPATH=$PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH>
```
UNIT TESTS

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

@ -2,7 +2,8 @@ import gtsam
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):
"""
circlePose3 generates a set of poses in a circle. This function
returns those poses inside a gtsam.Values object, with sequential
@ -18,16 +19,19 @@ def circlePose3(numPoses = 8, radius = 1.0, symbolChar = 0):
"""
# Force symbolChar to be a single character
if type(symbolChar) is str: symbolChar = ord(symbolChar[0])
if type(symbolChar) is str:
symbolChar = ord(symbolChar[0])
values = gtsam.Values()
theta = 0.0
dtheta = 2*pi/numPoses
gRo = gtsam.Rot3(np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F'))
dtheta = 2 * pi / numPoses
gRo = gtsam.Rot3(
np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F'))
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.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.insert(key, gTi)
theta = theta + dtheta

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

@ -1,7 +1,9 @@
from __future__ import print_function
import numpy as np
from math import pi, cos, sin
import gtsam
from gtsam import symbol
class Options:
"""
@ -31,14 +33,14 @@ class GroundTruth:
self.points = [gtsam.Point3()] * nrPoints
def print_(self, s=""):
print s
print "K = ", self.K
print "Cameras: ", len(self.cameras)
print(s)
print("K = ", self.K)
print("Cameras: ", len(self.cameras))
for camera in self.cameras:
print "\t", camera
print "Points: ", len(self.points)
print("\t", camera)
print("Points: ", len(self.points))
for point in self.points:
print "\t", point
print("\t", point)
pass
@ -69,9 +71,7 @@ class Data:
def generate_data(options):
"""
Generate ground-truth and measurement data
"""
""" Generate ground-truth and measurement data. """
K = gtsam.Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
nrPoints = 3 if options.triangle else 8
@ -86,14 +86,12 @@ def generate_data(options):
theta = j * 2 * pi / nrPoints
truth.points[j] = gtsam.Point3(r * cos(theta), r * sin(theta), 0)
else: # 3D landmarks as vertices of a cube
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)]
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
@ -101,8 +99,10 @@ def generate_data(options):
for i in range(options.nrCameras):
theta = i * 2 * pi / options.nrCameras
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)
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
@ -111,7 +111,7 @@ def generate_data(options):
# Calculate odometry between cameras
for i in range(1, options.nrCameras):
data.odometry[i] = truth.cameras[
i - 1].pose().between(truth.cameras[i].pose())
data.odometry[i] = truth.cameras[i - 1].pose().between(
truth.cameras[i].pose())
return data, truth

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

@ -1,10 +1,10 @@
import gtsam
from gtsam import symbol
class Options:
"""
Options for visual isam example
"""
""" Options for visual isam example. """
def __init__(self):
self.hardConstraint = False
self.pointPriors = False
@ -18,7 +18,7 @@ def initialize(data, truth, options):
params = gtsam.ISAM2Params()
if options.alwaysRelinearize:
params.setRelinearizeSkip(1)
isam = gtsam.ISAM2(params = params)
isam = gtsam.ISAM2(params=params)
# Add constraints/priors
# TODO: should not be from ground truth!
@ -28,11 +28,12 @@ def initialize(data, truth, options):
ii = symbol(ord('x'), i)
if i == 0:
if options.hardConstraint: # add hard constraint
newFactors.add(gtsam.NonlinearEqualityPose3(
ii, truth.cameras[0].pose()))
newFactors.add(
gtsam.NonlinearEqualityPose3(ii, truth.cameras[0].pose()))
else:
newFactors.add(gtsam.PriorFactorPose3(
ii, truth.cameras[i].pose(), data.noiseModels.posePrior))
newFactors.add(
gtsam.PriorFactorPose3(ii, truth.cameras[i].pose(),
data.noiseModels.posePrior))
initialEstimates.insert(ii, truth.cameras[i].pose())
nextPoseIndex = 2
@ -44,23 +45,27 @@ 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(gtsam.GenericProjectionFactorCal3_S2(
data.Z[i][k], data.noiseModels.measurement, ii, jj, data.K))
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(gtsam.PriorFactorPoint3(
jj, truth.points[j], data.noiseModels.pointPrior))
newFactors.add(
gtsam.PriorFactorPoint3(jj, truth.points[j],
data.noiseModels.pointPrior))
# Add odometry between frames 0 and 1
newFactors.add(gtsam.BetweenFactorPose3(symbol(ord('x'), 0), symbol(
ord('x'), 1), data.odometry[1], data.noiseModels.odometry))
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 = gtsam.LevenbergMarquardtOptimizer(
newFactors, initialEstimates)
batchOptimizer = gtsam.LevenbergMarquardtOptimizer(newFactors,
initialEstimates)
fullyOptimized = batchOptimizer.optimize()
isam.update(newFactors, fullyOptimized)
else:
@ -71,18 +76,18 @@ def initialize(data, truth, options):
result = isam.calculateEstimate()
# t=toc plot(frame_i,t,'g.')
return isam, result, nextPoseIndex
return isam, result, nextPoseIndex
def step(data, isam, result, truth, currPoseIndex):
"""
'''
Do one step isam update
@param[in] data: measurement data (odometry and visual measurements and their noiseModels)
@param[in/out] isam: current isam object, will be updated
@param[in/out] result: current result object, will be updated
@param[in] truth: ground truth data, used to initialize new variables
@param[currPoseIndex]: index of the current pose
"""
'''
# iSAM expects us to give it a new set of factors
# along with initial estimates for any new variables introduced.
newFactors = gtsam.NonlinearFactorGraph()
@ -91,18 +96,21 @@ def step(data, isam, result, truth, currPoseIndex):
# Add odometry
prevPoseIndex = currPoseIndex - 1
odometry = data.odometry[prevPoseIndex]
newFactors.add(gtsam.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(gtsam.GenericProjectionFactorCal3_S2(
zij, data.noiseModels.measurement,
symbol(ord('x'), currPoseIndex), jj, data.K))
newFactors.add(
gtsam.GenericProjectionFactorCal3_S2(
zij, data.noiseModels.measurement,
symbol(ord('x'), currPoseIndex), jj, data.K))
# TODO: initialize with something other than truth
if not result.exists(jj) and not initialEstimates.exists(jj):
lmInit = truth.points[j]
@ -110,8 +118,8 @@ def step(data, isam, result, truth, currPoseIndex):
# Initial estimates for the new pose.
prevPose = result.atPose3(symbol(ord('x'), prevPoseIndex))
initialEstimates.insert(symbol(ord('x'), currPoseIndex),
prevPose.compose(odometry))
initialEstimates.insert(
symbol(ord('x'), currPoseIndex), prevPose.compose(odometry))
# Update ISAM
# figure(1)tic

2
wrap/Constructor.cpp
View File

@ -152,7 +152,7 @@ void Constructor::emit_cython_pyx(FileWriter& pyxFile, const Class& cls) const {
pyxFile.oss << " self." << cls.shared_pxd_obj_in_pyx() << " = "
<< cls.shared_pxd_class_in_pyx() << "(new " << cls.pxd_class_in_pyx()
<< "(" << args.pyx_asParams() << "))\n";
pyxFile.oss << " except AssertionError:\n";
pyxFile.oss << " except (AssertionError, ValueError):\n";
pyxFile.oss << " pass\n";
}
}

2
wrap/Method.cpp
View File

@ -196,7 +196,7 @@ void Method::emit_cython_pyx(FileWriter& file, const Class& cls) const {
file.oss << " " << call << "\n";
file.oss << " return\n";
}
file.oss << " except AssertionError:\n";
file.oss << " except (AssertionError, ValueError):\n";
file.oss << " pass\n";
}
file.oss

21
wrap/tests/expected-cython/geometry.pyx
View File

@ -31,14 +31,14 @@ cdef class Point2:
try:
__params = process_args([], args, kwargs)
self.CPoint2_ = shared_ptr[CPoint2](new CPoint2())
except AssertionError:
except (AssertionError, ValueError):
pass
try:
__params = process_args(['x', 'y'], args, kwargs)
x = <double>(__params[0])
y = <double>(__params[1])
self.CPoint2_ = shared_ptr[CPoint2](new CPoint2(x, y))
except AssertionError:
except (AssertionError, ValueError):
pass
if (self.CPoint2_.use_count()==0):
raise TypeError('Point2 construction failed!')
@ -88,7 +88,7 @@ cdef class Point3:
y = <double>(__params[1])
z = <double>(__params[2])
self.CPoint3_ = shared_ptr[CPoint3](new CPoint3(x, y, z))
except AssertionError:
except (AssertionError, ValueError):
pass
if (self.CPoint3_.use_count()==0):
raise TypeError('Point3 construction failed!')
@ -124,7 +124,7 @@ cdef class Test:
try:
__params = process_args([], args, kwargs)
self.CTest_ = shared_ptr[CTest](new CTest())
except AssertionError:
except (AssertionError, ValueError):
pass
try:
__params = process_args(['a', 'b'], args, kwargs)
@ -133,7 +133,7 @@ cdef class Test:
assert isinstance(b, np.ndarray) and b.ndim == 2
b = b.astype(float, order='F', copy=False)
self.CTest_ = shared_ptr[CTest](new CTest(a, <MatrixXd>(Map[MatrixXd](b))))
except AssertionError:
except (AssertionError, ValueError):
pass
if (self.CTest_.use_count()==0):
raise TypeError('Test construction failed!')
@ -242,7 +242,7 @@ cdef class MyTemplatePoint2(MyBase):
try:
__params = process_args([], args, kwargs)
self.CMyTemplatePoint2_ = shared_ptr[CMyTemplatePoint2](new CMyTemplatePoint2())
except AssertionError:
except (AssertionError, ValueError):
pass
if (self.CMyTemplatePoint2_.use_count()==0):
raise TypeError('MyTemplatePoint2 construction failed!')
@ -306,7 +306,7 @@ cdef class MyTemplateMatrix(MyBase):
try:
__params = process_args([], args, kwargs)
self.CMyTemplateMatrix_ = shared_ptr[CMyTemplateMatrix](new CMyTemplateMatrix())
except AssertionError:
except (AssertionError, ValueError):
pass
if (self.CMyTemplateMatrix_.use_count()==0):
raise TypeError('MyTemplateMatrix construction failed!')
@ -376,7 +376,7 @@ cdef class MyVector3:
try:
__params = process_args([], args, kwargs)
self.CMyVector3_ = shared_ptr[CMyVector3](new CMyVector3())
except AssertionError:
except (AssertionError, ValueError):
pass
if (self.CMyVector3_.use_count()==0):
raise TypeError('MyVector3 construction failed!')
@ -400,7 +400,7 @@ cdef class MyVector12:
try:
__params = process_args([], args, kwargs)
self.CMyVector12_ = shared_ptr[CMyVector12](new CMyVector12())
except AssertionError:
except (AssertionError, ValueError):
pass
if (self.CMyVector12_.use_count()==0):
raise TypeError('MyVector12 construction failed!')
@ -429,7 +429,7 @@ cdef class MyFactorPosePoint2:
noiseModel = <noiseModel_Base>(__params[3])
assert isinstance(noiseModel, noiseModel_Base)
self.CMyFactorPosePoint2_ = shared_ptr[CMyFactorPosePoint2](new CMyFactorPosePoint2(key1, key2, measured, noiseModel.CnoiseModel_Base_))
except AssertionError:
except (AssertionError, ValueError):
pass
if (self.CMyFactorPosePoint2_.use_count()==0):
raise TypeError('MyFactorPosePoint2 construction failed!')
@ -478,4 +478,3 @@ def overloadedGlobalFunction_1(args, kwargs):
return True, ndarray_copy(return_value).squeeze()
except:
return False, None