diff --git a/cython/README.md b/cython/README.md index 53c04d1e4..35e1008a5 100644 --- a/cython/README.md +++ b/cython/README.md @@ -2,7 +2,7 @@ This is the Cython/Python wrapper around the GTSAM C++ library. INSTALL ======= -- This wrapper needs Cython(>=0.25), numpy and eigency, which can be installed +- This wrapper needs Cython(>=0.25), numpy and eigency, which can be installed as follows: ```bash @@ -35,21 +35,27 @@ See the tests for examples. ## Some important notes: -- Vector/Matrix: Due to a design choice of eigency, numpy.array matrices with the default order='A' -will always be transposed in C++ no matter how you transpose it in Python. Use order='F', or use -two functions Vector and Matrix in cython/gtsam/utils/np_utils.py for your conveniences. These two functions -also help to avoid a common but very subtle bug of using integers when creating numpy arrays, -e.g. np.array([1,2,3]). These can't be an input for gtsam functions as they only accept floating-point arrays. -For more details, see: https://github.com/wouterboomsma/eigency#storage-layout---why-arrays-are-sometimes-transposed +- Vector/Matrix: + + GTSAM expects double-precision floating point vectors and matrices. + Hence, you should pass numpy matrices with dtype=float, or 'float64'. + + Also, GTSAM expects *column-major* matrices, unlike the default storage + scheme in numpy. Hence, you should pass column-major matrices to gtsam using + the flag order='F'. And you always get column-major matrices back. + For more details, see: https://github.com/wouterboomsma/eigency#storage-layout---why-arrays-are-sometimes-transposed + + Passing row-major matrices of different dtype, e.g. 'int', will also work + as the wrapper converts them to column-major and dtype float for you, + using numpy.array.astype(float, order='F', copy=False). + However, this will result a copy if your matrix is not in the expected type + and storage order. - Inner namespace: Classes in inner namespace will be prefixed by _ in Python. Examples: noiseModel_Gaussian, noiseModel_mEstimator_Tukey - Casting from a base class to a derive class must be done explicitly. -Examples: +Examples: ```Python noiseBase = factor.get_noiseModel() - noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase) + noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase) ``` WRAPPING YOUR OWN PROJECT THAT USES GTSAM @@ -102,7 +108,7 @@ KNOWN ISSUES - size-related issue: can only wrap up to a certain number of classes: up to mEstimator! - Guess: 64 vs 32b? disutils Compiler flags? - Bug with Cython 0.24: instantiated factor classes return FastVector for keys(), which can't be casted to FastVector - - Upgrading to 0.25 solves the problem + - Upgrading to 0.25 solves the problem - Need default constructor and default copy constructor for almost every classes... :( - support these constructors by default and declare "delete" for special classes? @@ -110,12 +116,20 @@ KNOWN ISSUES TODO ===== ☐ Unify cython/gtsam.h and the original gtsam.h + ✔ 06-03-17: manage to remove the requirements for default and copy constructors - 25-11-16: - Try to unify but failed. Main reasons are: Key/size_t, std containers, KeyVector/KeyList/KeySet. + Try to unify but failed. Main reasons are: Key/size_t, std containers, KeyVector/KeyList/KeySet. Matlab doesn't need to know about Key, but I can't make Cython to ignore Key as it couldn't cast KeyVector, i.e. FastVector, - to FastVector. + to FastVector. +☐ Unit tests for cython wrappers +☐ Fix Python tests: don't use " import * ": Bad style!!! +☐ Marginal and JointMarginal: revert changes +- add doc for generate +- matlab 6 arguments? Completed/Cancelled: +✔ Convert input numpy Matrix/Vector to float dtype and storage order 'F' automatically, cannot crash! @done (15-03-17 13:00) +✔ Remove requirements.txt - Frank: don't bother with only 2 packages and a special case for eigency! @done (08-03-17 10:30) ✔ CMake install script @done (25-11-16 02:30) ✘ [REFACTOR] better name for uninstantiateClass: very vague!! @cancelled (25-11-16 02:30) -- lazy ✘ forward declaration? @cancelled (23-11-16 13:00) - nothing to do, seem to work? @@ -150,7 +164,7 @@ Completed/Cancelled: - what's the purpose of "virtual" ?? Installation: - ☐ Prerequisite: + ☐ Prerequisite: - Users create venv and pip install requirements before compiling - Wrap cython script in gtsam/cython folder ☐ Install built module into venv? diff --git a/cython/gtsam/tests/experiments.py b/cython/gtsam/tests/experiments.py index 1437aed7b..8f7dab5cf 100644 --- a/cython/gtsam/tests/experiments.py +++ b/cython/gtsam/tests/experiments.py @@ -3,9 +3,9 @@ This file contains small experiments to test the wrapper with gtsam_short, not real unittests. Its name convention is different from other tests so it won't be discovered. """ -from gtsam.gtsam import * +from gtsam import * import numpy as np -from gtsam.utils import Vector, Matrix +from utils import Vector, Matrix r = Rot3() print(r) diff --git a/cython/gtsam/tests/test_JacobianFactor.py b/cython/gtsam/tests/test_JacobianFactor.py index 7e6b0faaa..ebca30b04 100644 --- a/cython/gtsam/tests/test_JacobianFactor.py +++ b/cython/gtsam/tests/test_JacobianFactor.py @@ -2,36 +2,39 @@ import unittest from gtsam import * from math import * import numpy as np -from gtsam.utils import Matrix, Vector class TestJacobianFactor(unittest.TestCase): def test_eliminate(self): - Ax2 = Matrix( - [-5., 0.], + # Recommended way to specify a matrix (see cython/README) + Ax2 = np.array( + [[-5., 0.], [+0., -5.], [10., 0.], - [+0., 10.]) + [+0., 10.]], order='F') - Al1 = Matrix( - [5., 0.], - [0., 5.], - [0., 0.], - [0., 0.]) + # This is good too + Al1 = np.array( + [[5, 0], + [0, 5], + [0, 0], + [0, 0]], dtype=float, order = 'F') - Ax1 = Matrix( - [0.00, 0.], # f4 - [0.00, 0.], # f4 - [-10., 0.], # f2 - [0.00, -10.]) # f2 + # Not recommended for performance reasons, but should still work + # as the wrapper should convert it to the correct type and storage order + Ax1 = np.array( + [[0, 0], # f4 + [0, 0], # f4 + [-10, 0], # f2 + [0, -10]]) # f2 x2 = 1 l1 = 2 x1 = 3 # the RHS - b2 = Vector(-1., 1.5, 2., -1.) - sigmas = Vector(1., 1., 1., 1.) + b2 = np.array([-1., 1.5, 2., -1.]) + sigmas = np.array([1., 1., 1., 1.]) model4 = noiseModel_Diagonal.Sigmas(sigmas) combined = JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4) @@ -42,29 +45,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 = np.array([[11.1803, 0.00], + [0.00, 11.1803]]) + S12 = np.array([[-2.23607, 0.00], + [+0.00, -2.23607]]) + S13 = np.array([[-8.94427, 0.00], + [+0.00, -8.94427]]) + d = np.array([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 = np.array([[4.47214, 0.00], + [0.00, 4.47214]]) - Bx1 = Matrix( + Bx1 = np.array( # x1 - [-4.47214, 0.00], - [+0.00, -4.47214]) + [[-4.47214, 0.00], + [+0.00, -4.47214]]) # the RHS - b1 = Vector(0.0, 0.894427) + b1 = np.array([0.0, 0.894427]) model2 = noiseModel_Diagonal.Sigmas(np.array([1., 1.])) expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2) diff --git a/cython/gtsam/tests/test_SFMExample.py b/cython/gtsam/tests/test_SFMExample.py index 2df72382a..8954a831e 100644 --- a/cython/gtsam/tests/test_SFMExample.py +++ b/cython/gtsam/tests/test_SFMExample.py @@ -2,7 +2,6 @@ import unittest from gtsam import * from math import * import numpy as np -from gtsam.utils import Vector, Matrix import gtsam.utils.visual_data_generator as generator @@ -17,7 +16,7 @@ class TestSFMExample(unittest.TestCase): measurementNoiseSigma = 1.0 pointNoiseSigma = 0.1 - poseNoiseSigmas = Vector([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]) + poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]) graph = NonlinearFactorGraph() diff --git a/cython/gtsam/tests/test_StereoVOExample.py b/cython/gtsam/tests/test_StereoVOExample.py index 65ca7f3b7..9dc282349 100644 --- a/cython/gtsam/tests/test_StereoVOExample.py +++ b/cython/gtsam/tests/test_StereoVOExample.py @@ -2,7 +2,6 @@ import unittest from gtsam import * from math import * import numpy as np -from gtsam.utils import Vector, Matrix class TestStereoVOExample(unittest.TestCase): @@ -32,7 +31,7 @@ class TestStereoVOExample(unittest.TestCase): ## Create realistic calibration and measurement noise model # format: fx fy skew cx cy baseline K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2) - stereo_model = noiseModel_Diagonal.Sigmas(Vector([1.0, 1.0, 1.0])) + stereo_model = noiseModel_Diagonal.Sigmas(np.array([1.0, 1.0, 1.0])) ## Add measurements # pose 1 diff --git a/cython/gtsam/tests/test_Values.py b/cython/gtsam/tests/test_Values.py index 422da3bf1..6d5d8e077 100644 --- a/cython/gtsam/tests/test_Values.py +++ b/cython/gtsam/tests/test_Values.py @@ -22,11 +22,25 @@ class TestValues(unittest.TestCase): values.insert(9, E) values.insert(10, imuBias_ConstantBias()) - # special cases for Vector and Matrix: - vec = np.array([1., 2., 3.]) + # Special cases for Vectors and Matrices + # Note that gtsam's Eigen Vectors and Matrices requires double-precision + # floating point numbers in column-major (Fortran style) storage order, + # whereas by default, numpy.array is in row-major order and the type is + # in whatever the number input type is, e.g. np.array([1,2,3]) + # will have 'int' type. + # + # The wrapper will automatically fix the type and storage order for you, + # but for performace reasons, it's recommended to specify the correct + # type and storage order. + vec = np.array([1., 2., 3.]) # for vectors, the order is not important, but dtype still is values.insert(11, vec) mat = np.array([[1., 2.], [3., 4.]], order='F') values.insert(12, mat) + # Test with dtype int and the default order='C' + # This still works as the wrapper converts to the correct type and order for you + # but is nornally not recommended! + mat2 = np.array([[1,2,],[3,5]]) + values.insert(13, mat2) self.assertTrue(values.atPoint2(0).equals(Point2(), tol)) self.assertTrue(values.atPoint3(1).equals(Point3(), tol)) @@ -46,6 +60,8 @@ class TestValues(unittest.TestCase): self.assertTrue(np.allclose(vec, actualVector, tol)) actualMatrix = values.atMatrix(12) self.assertTrue(np.allclose(mat, actualMatrix, tol)) + actualMatrix2 = values.atMatrix(13) + self.assertTrue(np.allclose(mat2, actualMatrix2, tol)) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_VisualISAMExample.py b/cython/gtsam/tests/test_VisualISAMExample.py index c2c4a98ac..bc4346780 100644 --- a/cython/gtsam/tests/test_VisualISAMExample.py +++ b/cython/gtsam/tests/test_VisualISAMExample.py @@ -2,7 +2,6 @@ import unittest from gtsam import * from math import * import numpy as np -from gtsam.utils import Vector, Matrix import gtsam.utils.visual_data_generator as generator import gtsam.utils.visual_isam as visual_isam diff --git a/cython/gtsam/utils/__init__.py b/cython/gtsam/utils/__init__.py index 3e9ea71a6..e69de29bb 100644 --- a/cython/gtsam/utils/__init__.py +++ b/cython/gtsam/utils/__init__.py @@ -1 +0,0 @@ -from .np_utils import * \ No newline at end of file diff --git a/cython/gtsam/utils/circlePose3.py b/cython/gtsam/utils/circlePose3.py index bdc4b7631..49b64d0b7 100644 --- a/cython/gtsam/utils/circlePose3.py +++ b/cython/gtsam/utils/circlePose3.py @@ -1,6 +1,6 @@ from gtsam import * from math import * -from np_utils import * +import numpy as np def circlePose3(numPoses = 8, radius = 1.0, symbolChar = 0): """ @@ -23,7 +23,7 @@ def circlePose3(numPoses = 8, radius = 1.0, symbolChar = 0): values = gtsam.Values() theta = 0.0 dtheta = 2*pi/numPoses - gRo = gtsam.Rot3(Matrix([0., 1., 0.], [1., 0., 0.], [0., 0., -1.])) + 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) @@ -31,4 +31,4 @@ def circlePose3(numPoses = 8, radius = 1.0, symbolChar = 0): gTi = gtsam.Pose3(gRo.compose(oRi), gti) values.insert(key, gTi) theta = theta + dtheta - return values \ No newline at end of file + return values diff --git a/cython/gtsam/utils/np_utils.py b/cython/gtsam/utils/np_utils.py deleted file mode 100644 index 84940eb8e..000000000 --- a/cython/gtsam/utils/np_utils.py +++ /dev/null @@ -1,27 +0,0 @@ -import numpy as np - - -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.squeeze(np.asarray(args, dtype='float')) - if ret.ndim == 0: - ret = np.expand_dims(ret, axis=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 5db6840c6..6f404fcc0 100644 --- a/cython/gtsam/utils/visual_data_generator.py +++ b/cython/gtsam/utils/visual_data_generator.py @@ -59,11 +59,11 @@ class Data: # Set Noise parameters self.noiseModels = Data.NoiseModels() self.noiseModels.posePrior = noiseModel_Diagonal.Sigmas( - Vector([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])) + np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])) # noiseModels.odometry = noiseModel_Diagonal.Sigmas( - # Vector([0.001,0.001,0.001,0.1,0.1,0.1])) + # np.array([0.001,0.001,0.001,0.1,0.1,0.1])) self.noiseModels.odometry = noiseModel_Diagonal.Sigmas( - Vector([0.05, 0.05, 0.05, 0.2, 0.2, 0.2])) + np.array([0.05, 0.05, 0.05, 0.2, 0.2, 0.2])) self.noiseModels.pointPrior = noiseModel_Isotropic.Sigma(3, 0.1) self.noiseModels.measurement = noiseModel_Isotropic.Sigma(2, 1.0) @@ -84,8 +84,7 @@ def generate_data(options): r = 10 for j in range(len(truth.points)): theta = j * 2 * pi / nrPoints - truth.points[j] = Point3( - v=Vector([r * cos(theta), r * sin(theta), 0])) + truth.points[j] = Point3(r * cos(theta), r * sin(theta), 0) else: # 3D landmarks as vertices of a cube truth.points = [Point3(10, 10, 10), Point3(-10, 10, 10), @@ -101,9 +100,9 @@ def generate_data(options): r = 40 for i in range(options.nrCameras): theta = i * 2 * pi / options.nrCameras - t = Point3(Vector(r * cos(theta), r * sin(theta), height)) + t = Point3(r * cos(theta), r * sin(theta), height) truth.cameras[i] = SimpleCamera.Lookat( - t, Point3(), Point3(Vector(0, 0, 1)), truth.K) + t, Point3(), Point3(0, 0, 1), truth.K) # Create measurements for j in range(nrPoints): # All landmarks seen in every frame