diff --git a/cython/gtsam.h b/cython/gtsam.h index 22387093a..d7e523be8 100644 --- a/cython/gtsam.h +++ b/cython/gtsam.h @@ -1846,48 +1846,48 @@ class Values { // void update(size_t j, const gtsam::Value& val); // gtsam::Value at(size_t j) const; - template - void insert(size_t j, const T& t); + // template + // void insert(size_t j, const T& t); - template - void update(size_t j, const T& t); + // template + // 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 = 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 diff --git a/cython/gtsam_utils/visual_data_generator.py b/cython/gtsam_utils/visual_data_generator.py index 180f74b47..5db6840c6 100644 --- a/cython/gtsam_utils/visual_data_generator.py +++ b/cython/gtsam_utils/visual_data_generator.py @@ -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 diff --git a/cython/gtsam_utils/visual_isam.py b/cython/gtsam_utils/visual_isam.py index 222a468a9..bd1fef8c3 100644 --- a/cython/gtsam_utils/visual_isam.py +++ b/cython/gtsam_utils/visual_isam.py @@ -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 diff --git a/cython/test/test_JacobianFactor.py b/cython/test/test_JacobianFactor.py index fe8c7cbfa..4abc35bfd 100644 --- a/cython/test/test_JacobianFactor.py +++ b/cython/test/test_JacobianFactor.py @@ -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) diff --git a/cython/test/test_LocalizationExample.py b/cython/test/test_LocalizationExample.py index 61f2ff995..2431a064a 100644 --- a/cython/test/test_LocalizationExample.py +++ b/cython/test/test_LocalizationExample.py @@ -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 diff --git a/cython/test/test_OdometryExample.py b/cython/test/test_OdometryExample.py index 895866385..f38709e9f 100644 --- a/cython/test/test_OdometryExample.py +++ b/cython/test/test_OdometryExample.py @@ -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 diff --git a/cython/test/test_PlanarSLAMExample.py b/cython/test/test_PlanarSLAMExample.py index 22c1f19bd..9053f3fb3 100644 --- a/cython/test/test_PlanarSLAMExample.py +++ b/cython/test/test_PlanarSLAMExample.py @@ -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 diff --git a/cython/test/test_Pose2SLAMExample.py b/cython/test/test_Pose2SLAMExample.py index 536e8d782..dce96d7b1 100644 --- a/cython/test/test_Pose2SLAMExample.py +++ b/cython/test/test_Pose2SLAMExample.py @@ -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 diff --git a/cython/test/test_Pose3SLAMExample.py b/cython/test/test_Pose3SLAMExample.py index 779b3d68e..f6cb2ff51 100644 --- a/cython/test/test_Pose3SLAMExample.py +++ b/cython/test/test_Pose3SLAMExample.py @@ -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) diff --git a/cython/test/test_PriorFactor.py b/cython/test/test_PriorFactor.py index 38c5cd622..80e3622c0 100644 --- a/cython/test/test_PriorFactor.py +++ b/cython/test/test_PriorFactor.py @@ -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__": diff --git a/cython/test/test_SFMExample.py b/cython/test/test_SFMExample.py index 112c4b9d4..f67b65433 100644 --- a/cython/test/test_SFMExample.py +++ b/cython/test/test_SFMExample.py @@ -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) diff --git a/cython/test/test_StereoVOExample.py b/cython/test/test_StereoVOExample.py index 71e2674f5..964ac43a0 100644 --- a/cython/test/test_StereoVOExample.py +++ b/cython/test/test_StereoVOExample.py @@ -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) diff --git a/cython/test/test_Values.py b/cython/test/test_Values.py index 350642988..c31fed577 100644 --- a/cython/test/test_Values.py +++ b/cython/test/test_Values.py @@ -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)) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 85bdc0022..e14d57ad5 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -827,7 +827,6 @@ void Class::emit_cython_pyx(FileWriter& pyxFile, const std::vector& 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 nargsSet; - std::vector 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 { diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index cfdafe5a6..b058848f7 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -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() << "."