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: - Modify your PYTHONPATH to include the GTSAM_CYTHON_INSTALL_PATH:
```bash ```bash
export PYTHONPATH = $PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH> export PYTHONPATH=$PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH>
``` ```
UNIT TESTS UNIT TESTS

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

@ -2,7 +2,8 @@ import gtsam
import numpy as np import numpy as np
from math import pi, cos, sin 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 circlePose3 generates a set of poses in a circle. This function
returns those poses inside a gtsam.Values object, with sequential 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 # 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() values = gtsam.Values()
theta = 0.0 theta = 0.0
dtheta = 2*pi/numPoses dtheta = 2 * pi / numPoses
gRo = gtsam.Rot3(np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F')) gRo = gtsam.Rot3(
np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F'))
for i in range(numPoses): for i in range(numPoses):
key = gtsam.symbol(symbolChar, i) key = gtsam.symbol(symbolChar, i)
gti = gtsam.Point3(radius*cos(theta), radius*sin(theta), 0) gti = gtsam.Point3(radius * cos(theta), radius * sin(theta), 0)
oRi = gtsam.Rot3.Yaw(-theta) # negative yaw goes counterclockwise, with Z down ! oRi = gtsam.Rot3.Yaw(
-theta) # negative yaw goes counterclockwise, with Z down !
gTi = gtsam.Pose3(gRo.compose(oRi), gti) gTi = gtsam.Pose3(gRo.compose(oRi), gti)
values.insert(key, gTi) values.insert(key, gTi)
theta = theta + dtheta 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 import numpy as np
from math import pi, cos, sin from math import pi, cos, sin
import gtsam import gtsam
from gtsam import symbol
class Options: class Options:
""" """
@ -31,14 +33,14 @@ class GroundTruth:
self.points = [gtsam.Point3()] * nrPoints self.points = [gtsam.Point3()] * nrPoints
def print_(self, s=""): def print_(self, s=""):
print s print(s)
print "K = ", self.K print("K = ", self.K)
print "Cameras: ", len(self.cameras) print("Cameras: ", len(self.cameras))
for camera in self.cameras: for camera in self.cameras:
print "\t", camera print("\t", camera)
print "Points: ", len(self.points) print("Points: ", len(self.points))
for point in self.points: for point in self.points:
print "\t", point print("\t", point)
pass pass
@ -69,9 +71,7 @@ class Data:
def generate_data(options): 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.) 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
@ -86,14 +86,12 @@ def generate_data(options):
theta = j * 2 * pi / nrPoints theta = j * 2 * pi / nrPoints
truth.points[j] = gtsam.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 = [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), 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 # Create camera cameras on a circle around the triangle
height = 10 height = 10
@ -101,8 +99,10 @@ def generate_data(options):
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 = gtsam.Point3(r * cos(theta), r * sin(theta), height) t = gtsam.Point3(r * cos(theta), r * sin(theta), height)
truth.cameras[i] = gtsam.SimpleCamera.Lookat( truth.cameras[i] = gtsam.SimpleCamera.Lookat(t,
t, gtsam.Point3(), gtsam.Point3(0, 0, 1), truth.K) 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
@ -111,7 +111,7 @@ def generate_data(options):
# Calculate odometry between cameras # Calculate odometry between cameras
for i in range(1, options.nrCameras): for i in range(1, options.nrCameras):
data.odometry[i] = truth.cameras[ data.odometry[i] = truth.cameras[i - 1].pose().between(
i - 1].pose().between(truth.cameras[i].pose()) truth.cameras[i].pose())
return data, truth return data, truth

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

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

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

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