diff --git a/cython/README.md b/cython/README.md index 2b1fa28d1..368d2a76d 100644 --- a/cython/README.md +++ b/cython/README.md @@ -17,7 +17,7 @@ by default: /cython - Modify your PYTHONPATH to include the GTSAM_CYTHON_INSTALL_PATH: ```bash -export PYTHONPATH = $PYTHONPATH: +export PYTHONPATH=$PYTHONPATH: ``` UNIT TESTS diff --git a/cython/gtsam/utils/circlePose3.py b/cython/gtsam/utils/circlePose3.py index cdeb41c45..7012548f4 100644 --- a/cython/gtsam/utils/circlePose3.py +++ b/cython/gtsam/utils/circlePose3.py @@ -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 diff --git a/cython/gtsam/utils/visual_data_generator.py b/cython/gtsam/utils/visual_data_generator.py index 3f6391164..91194c565 100644 --- a/cython/gtsam/utils/visual_data_generator.py +++ b/cython/gtsam/utils/visual_data_generator.py @@ -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 diff --git a/cython/gtsam/utils/visual_isam.py b/cython/gtsam/utils/visual_isam.py index c23ad194d..b0ebe68c3 100644 --- a/cython/gtsam/utils/visual_isam.py +++ b/cython/gtsam/utils/visual_isam.py @@ -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 diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 429164e1b..74719b289 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -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"; } } diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 4c2cd2518..2637275d1 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -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 diff --git a/wrap/tests/expected-cython/geometry.pyx b/wrap/tests/expected-cython/geometry.pyx index e402af387..ebe66b99a 100644 --- a/wrap/tests/expected-cython/geometry.pyx +++ b/wrap/tests/expected-cython/geometry.pyx @@ -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 = (__params[0]) y = (__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 = (__params[1]) z = (__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, (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 = (__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 -