Merge pull request strawlab#57 from larsmans/fix-eigen-transpose

nzjrs · nzjrs · commit 8507941c4dc3 · 2014-10-23T15:31:06.000+02:00
BUG: Eigen uses column-major storage!
diff --git a/pcl/_pcl.pyx b/pcl/_pcl.pyx
@@ -135,7 +135,7 @@ cdef class PointCloud:
     To load a point cloud from disk, use pcl.load.
     """
     def __cinit__(self, init=None):
-        cdef BasePointCloud other
+        cdef PointCloud other
 
         sp_assign(self.thisptr_shared, new cpp.PointCloud[cpp.PointXYZ]())
 
diff --git a/pcl/registration.pyx b/pcl/registration.pyx
@@ -54,10 +54,10 @@ cdef object run(Registration[cpp.PointXYZ, cpp.PointXYZ] &reg,
     # Get transformation matrix and convert from Eigen to NumPy format.
     cdef Registration[cpp.PointXYZ, cpp.PointXYZ].Matrix4 mat
     mat = reg.getFinalTransformation()
-    cdef np.ndarray[dtype=np.float32_t, ndim=2, mode='c'] transf
+    cdef np.ndarray[dtype=np.float32_t, ndim=2, mode='fortran'] transf
     cdef np.float32_t *transf_data
 
-    transf = np.empty((4, 4), dtype=np.float32, order='c')
+    transf = np.empty((4, 4), dtype=np.float32, order='fortran')
     transf_data = <np.float32_t *>np.PyArray_DATA(transf)
 
     for i in range(16):
diff --git a/tests/test_registration.py b/tests/test_registration.py
@@ -2,6 +2,7 @@
 
 import numpy as np
 from numpy import cos, sin
+from numpy.testing import assert_equal
 import unittest
 
 import pcl
@@ -38,13 +39,15 @@ def check_algo(self, algo):
         self.assertTrue(isinstance(transf, np.ndarray))
         self.assertEqual(transf.shape, (4, 4))
 
+        self.assertFalse(np.any(transf[:3] == 0))
+        assert_equal(transf[3], [0, 0, 0, 1])
+
         # XXX I think I misunderstand fitness, it's not equal to the following
         # MSS.
         # mss = (np.linalg.norm(estimate.to_array()
         #                       - self.source.to_array(), axis=1) ** 2).mean()
         # self.assertLess(mss, 1)
 
-        # TODO check the actual transformation matrix.
         #print("------", algo)
         #print("Converged: ", converged, "Estimate: ", estimate,
         #      "Fitness: ", fitness)
