Merge pull request #79 from jcarpent/devel

Improve efficiency + fix bug in Eigen::MatrixBase

Author: jcarpent

CMakeLists.txt

@@ -103,6 +103,7 @@ SET(${PROJECT_NAME}_HEADERS
   include/eigenpy/quaternion.hpp
   include/eigenpy/stride.hpp
   include/eigenpy/ref.hpp
+  include/eigenpy/details/rvalue_from_python_data.hpp
 )
 
 INCLUDE_DIRECTORIES(${${PROJECT_NAME}_BINARY_DIR}/include)

cmake

@@ -6,6 +6,7 @@
 #ifndef __eigenpy_details_hpp__
 #define __eigenpy_details_hpp__
 
+#include "eigenpy/details/rvalue_from_python_data.hpp"
 #include "eigenpy/fwd.hpp"
 
 #include <patchlevel.h> // For PY_MAJOR_VERSION
@@ -18,7 +19,6 @@
 
 #define GET_PY_ARRAY_TYPE(array) PyArray_ObjectType(reinterpret_cast<PyObject *>(array), 0)
 
-
 namespace eigenpy
 {
   template <typename SCALAR>  struct NumpyEquivalentType {};
@@ -146,33 +146,75 @@ namespace eigenpy
       const int rows = (int)PyArray_DIMS(pyArray)[0];
       const int cols = (int)PyArray_DIMS(pyArray)[1];
 
-      Type * mat_ptr = new(storage) Type(rows,cols);
+      Type * mat_ptr = new (storage) Type(rows,cols);
+      
+      if(NumpyEquivalentType<Scalar>::type_code == GET_PY_ARRAY_TYPE(pyArray))
+      {
+        *mat_ptr = MapNumpy<MatType,Scalar>::map(pyArray); // avoid useless cast
+        return;
+      }
+      
       if(GET_PY_ARRAY_TYPE(pyArray) == NPY_INT)
+      {
         *mat_ptr = MapNumpy<MatType,int>::map(pyArray).template cast<Scalar>();
+        return;
+      }
 
       if(GET_PY_ARRAY_TYPE(pyArray) == NPY_LONG)
+      {
         *mat_ptr = MapNumpy<MatType,long>::map(pyArray).template cast<Scalar>();
+        return;
+      }
 
       if(GET_PY_ARRAY_TYPE(pyArray) == NPY_FLOAT)
+      {
         *mat_ptr = MapNumpy<MatType,float>::map(pyArray).template cast<Scalar>();
+        return;
+      }
 
       if(GET_PY_ARRAY_TYPE(pyArray) == NPY_DOUBLE)
+      {
         *mat_ptr = MapNumpy<MatType,double>::map(pyArray).template cast<Scalar>();
+        return;
+      }
     }
 
-    static void convert(Type const & mat , PyArrayObject * pyArray)
+    /// \brief Copy mat into the Python array using Eigen::Map
+    template<typename MatrixDerived>
+    static void convert(const Eigen::MatrixBase<MatrixDerived> & mat_,
+                        PyArrayObject * pyArray)
     {
+      const MatrixDerived & mat = const_cast<const MatrixDerived &>(mat_.derived());
+      
+      if(NumpyEquivalentType<Scalar>::type_code == GET_PY_ARRAY_TYPE(pyArray))
+      {
+        MapNumpy<MatType,Scalar>::map(pyArray) = mat; // no cast needed
+        return;
+      }
+      
       if(GET_PY_ARRAY_TYPE(pyArray) == NPY_INT)
+      {
         MapNumpy<MatType,int>::map(pyArray) = mat.template cast<int>();
+        return;
+      }
 
       if(GET_PY_ARRAY_TYPE(pyArray) == NPY_LONG)
+      {
         MapNumpy<MatType,long>::map(pyArray) = mat.template cast<long>();
+        return;
+      }
 
       if(GET_PY_ARRAY_TYPE(pyArray) == NPY_FLOAT)
+      {
         MapNumpy<MatType,float>::map(pyArray) = mat.template cast<float>();
+        return;
+      }
 
       if(GET_PY_ARRAY_TYPE(pyArray) == NPY_DOUBLE)
+      {
         MapNumpy<MatType,double>::map(pyArray) = mat.template cast<double>();
+        return;
+      }
     }
   };
 
@@ -186,51 +228,47 @@ namespace eigenpy
     static void allocate(PyArrayObject * pyArray, void * storage)
     {
       typename MapNumpy<MatType,Scalar>::EigenMap numpyMap = MapNumpy<MatType,Scalar>::map(pyArray);
-      new(storage) Type(numpyMap);
+      new (storage) Type(numpyMap);
     }
 
-    static void convert(Type const & mat , PyArrayObject * pyArray)
+    static void convert(Type const & mat, PyArrayObject * pyArray)
     {
-      if(GET_PY_ARRAY_TYPE(pyArray) == NPY_INT)
-        MapNumpy<MatType,int>::map(pyArray) = mat.template cast<int>();
-      
-      if(GET_PY_ARRAY_TYPE(pyArray) == NPY_LONG)
-        MapNumpy<MatType,long>::map(pyArray) = mat.template cast<long>();
-      
-      if(GET_PY_ARRAY_TYPE(pyArray) == NPY_FLOAT)
-        MapNumpy<MatType,float>::map(pyArray) = mat.template cast<float>();
-      
-      if(GET_PY_ARRAY_TYPE(pyArray) == NPY_DOUBLE)
-        MapNumpy<MatType,double>::map(pyArray) = mat.template cast<double>();
+      EigenObjectAllocator<MatType>::convert(mat,pyArray);
     }
   };
 #endif
+  
   /* --- TO PYTHON -------------------------------------------------------------- */
+  
   template<typename MatType>
   struct EigenToPy
   {
-    static PyObject* convert(MatType const& mat)
+    static PyObject* convert(MatType const & mat)
     {
-      typedef typename MatType::Scalar T;
+      typedef typename MatType::Scalar Scalar;
       assert( (mat.rows()<INT_MAX) && (mat.cols()<INT_MAX) 
 	      && "Matrix range larger than int ... should never happen." );
       const int R  = (int)mat.rows(), C = (int)mat.cols();
 
       PyArrayObject* pyArray;
-      if(C == 1 && NumpyType::getType() == ARRAY_TYPE)
+      // Allocate Python memory
+      if(C == 1 && NumpyType::getType() == ARRAY_TYPE) // Handle array with a single dimension
       {
         npy_intp shape[1] = { R };
         pyArray = (PyArrayObject*) PyArray_SimpleNew(1, shape,
-                                                     NumpyEquivalentType<T>::type_code);
+                                                     NumpyEquivalentType<Scalar>::type_code);
       }
       else
       {
         npy_intp shape[2] = { R,C };
         pyArray = (PyArrayObject*) PyArray_SimpleNew(2, shape,
-                                                     NumpyEquivalentType<T>::type_code);
+                                                     NumpyEquivalentType<Scalar>::type_code);
       }
 
+      // Allocate memory
       EigenObjectAllocator<MatType>::convert(mat,pyArray);
+      
+      // Create an instance (either np.array or np.matrix)
       return NumpyType::getInstance().make(pyArray).ptr();
     }
   };
@@ -240,28 +278,28 @@ namespace eigenpy
   template<typename MatType>
   struct EigenFromPy
   {
-    // Determine if obj_ptr can be converted in a Eigenvec
-    static void* convertible(PyArrayObject* obj_ptr)
+    /// \brief Determine if pyObj can be converted into a MatType object
+    static void* convertible(PyArrayObject* pyObj)
     {
-      if (!PyArray_Check(obj_ptr))
+      if (!PyArray_Check(pyObj))
         return 0;
 
       if(MatType::IsVectorAtCompileTime)
       {
         // Special care of scalar matrix of dimension 1x1.
-        if(PyArray_DIMS(obj_ptr)[0] == 1 && PyArray_DIMS(obj_ptr)[1] == 1)
-          return obj_ptr;
+        if(PyArray_DIMS(pyObj)[0] == 1 && PyArray_DIMS(pyObj)[1] == 1)
+          return pyObj;
 
-        if(PyArray_DIMS(obj_ptr)[0] > 1 && PyArray_DIMS(obj_ptr)[1] > 1)
+        if(PyArray_DIMS(pyObj)[0] > 1 && PyArray_DIMS(pyObj)[1] > 1)
         {
 #ifndef NDEBUG
           std::cerr << "The number of dimension of the object does not correspond to a vector" << std::endl;
 #endif
           return 0;
         }
 
-        if(((PyArray_DIMS(obj_ptr)[0] == 1) && (MatType::ColsAtCompileTime == 1))
-           || ((PyArray_DIMS(obj_ptr)[1] == 1) && (MatType::RowsAtCompileTime == 1)))
+        if(((PyArray_DIMS(pyObj)[0] == 1) && (MatType::ColsAtCompileTime == 1))
+           || ((PyArray_DIMS(pyObj)[1] == 1) && (MatType::RowsAtCompileTime == 1)))
         {
 #ifndef NDEBUG
           if(MatType::ColsAtCompileTime == 1)
@@ -273,9 +311,9 @@ namespace eigenpy
         }
       }
 
-      if (PyArray_NDIM(obj_ptr) != 2)
+      if (PyArray_NDIM(pyObj) != 2)
       {
-        if ( (PyArray_NDIM(obj_ptr) !=1) || (! MatType::IsVectorAtCompileTime) )
+        if ( (PyArray_NDIM(pyObj) !=1) || (! MatType::IsVectorAtCompileTime) )
         {
 #ifndef NDEBUG
           std::cerr << "The number of dimension of the object is not correct." << std::endl;
@@ -284,10 +322,10 @@ namespace eigenpy
         }
       }
 
-      if (PyArray_NDIM(obj_ptr) == 2)
+      if (PyArray_NDIM(pyObj) == 2)
       {
-        const int R = (int)PyArray_DIMS(obj_ptr)[0];
-        const int C = (int)PyArray_DIMS(obj_ptr)[1];
+        const int R = (int)PyArray_DIMS(pyObj)[0];
+        const int C = (int)PyArray_DIMS(pyObj)[1];
 
         if( (MatType::RowsAtCompileTime!=R)
            && (MatType::RowsAtCompileTime!=Eigen::Dynamic) )
@@ -298,7 +336,7 @@ namespace eigenpy
       }
 
       // Check if the Scalar type of the obj_ptr is compatible with the Scalar type of MatType
-      if ((PyArray_ObjectType(reinterpret_cast<PyObject *>(obj_ptr), 0)) == NPY_INT)
+      if ((PyArray_ObjectType(reinterpret_cast<PyObject *>(pyObj), 0)) == NPY_INT)
       {
         if(!FromTypeToType<int,typename MatType::Scalar>::value)
         {
@@ -308,7 +346,7 @@ namespace eigenpy
           return 0;
         }
       }
-      else if ((PyArray_ObjectType(reinterpret_cast<PyObject *>(obj_ptr), 0)) == NPY_LONG)
+      else if ((PyArray_ObjectType(reinterpret_cast<PyObject *>(pyObj), 0)) == NPY_LONG)
       {
         if(!FromTypeToType<long,typename MatType::Scalar>::value)
         {
@@ -318,7 +356,7 @@ namespace eigenpy
           return 0;
         }
       }
-      else if ((PyArray_ObjectType(reinterpret_cast<PyObject *>(obj_ptr), 0)) == NPY_FLOAT)
+      else if ((PyArray_ObjectType(reinterpret_cast<PyObject *>(pyObj), 0)) == NPY_FLOAT)
       {
         if(!FromTypeToType<float,typename MatType::Scalar>::value)
         {
@@ -328,7 +366,7 @@ namespace eigenpy
           return 0;
         }
       }
-      else if ((PyArray_ObjectType(reinterpret_cast<PyObject *>(obj_ptr), 0)) == NPY_DOUBLE)
+      else if ((PyArray_ObjectType(reinterpret_cast<PyObject *>(pyObj), 0)) == NPY_DOUBLE)
       {
         if(!FromTypeToType<double,typename MatType::Scalar>::value)
         {
@@ -338,7 +376,7 @@ namespace eigenpy
           return 0;
         }
       }
-      else if ((PyArray_ObjectType(reinterpret_cast<PyObject *>(obj_ptr), 0))
+      else if ((PyArray_ObjectType(reinterpret_cast<PyObject *>(pyObj), 0))
           != NumpyEquivalentType<typename MatType::Scalar>::type_code)
       {
 #ifndef NDEBUG
@@ -349,7 +387,7 @@ namespace eigenpy
       }
 
 #ifdef NPY_1_8_API_VERSION
-      if (!(PyArray_FLAGS(obj_ptr)))
+      if (!(PyArray_FLAGS(pyObj)))
 #else
         if (!(PyArray_FLAGS(obj_ptr) & NPY_ALIGNED))
 #endif
@@ -360,15 +398,13 @@ namespace eigenpy
           return 0;
         }
 
-      return obj_ptr;
+      return pyObj;
     }
 
-    // Convert obj_ptr into an Eigen::Vector
+    /// \brief Allocate memory and copy pyObj in the new storage
     static void construct(PyObject* pyObj,
                           bp::converter::rvalue_from_python_stage1_data* memory)
     {
-      using namespace Eigen;
-      
       PyArrayObject * pyArray = reinterpret_cast<PyArrayObject*>(pyObj);
       assert((PyArray_DIMS(pyArray)[0]<INT_MAX) && (PyArray_DIMS(pyArray)[1]<INT_MAX));
 
@@ -379,6 +415,42 @@ namespace eigenpy
 
       memory->convertible = storage;
     }
+    
+    static void registration()
+    {
+      bp::converter::registry::push_back
+      (reinterpret_cast<void *(*)(_object *)>(&EigenFromPy::convertible),
+       &EigenFromPy::construct,bp::type_id<MatType>());
+    }
+  };
+  
+  template<typename MatType>
+  struct EigenFromPy< Eigen::MatrixBase<MatType> >
+  {
+    typedef EigenFromPy<MatType> EigenFromPyDerived;
+    typedef Eigen::MatrixBase<MatType> Base;
+    
+    /// \brief Determine if pyObj can be converted into a MatType object
+    static void* convertible(PyArrayObject* pyObj)
+    {
+      std::cout << "call: EigenFromPy< Eigen::MatrixBase<MatType> >::convertible" << std::endl;
+      return EigenFromPyDerived::convertible(pyObj);
+    }
+    
+    /// \brief Allocate memory and copy pyObj in the new storage
+    static void construct(PyObject* pyObj,
+                          bp::converter::rvalue_from_python_stage1_data* memory)
+    {
+      std::cout << "call: EigenFromPy< Eigen::MatrixBase<MatType> >::construct" << std::endl;
+      EigenFromPyDerived::construct(pyObj,memory);
+    }
+    
+    static void registration()
+    {
+      bp::converter::registry::push_back
+      (reinterpret_cast<void *(*)(_object *)>(&EigenFromPy::convertible),
+       &EigenFromPy::construct,bp::type_id<Base>());
+    }
   };
 
 #define numpy_import_array() {if (_import_array() < 0) {PyErr_Print(); PyErr_SetString(PyExc_ImportError, "numpy.core.multiarray failed to import"); } }
@@ -394,14 +466,11 @@ namespace eigenpy
   {
     static void registration()
     {
-      bp::converter::registry::push_back
-      (reinterpret_cast<void *(*)(_object *)>(&EigenFromPy<MatType>::convertible),
-       &EigenFromPy<MatType>::construct,bp::type_id<MatType>());
-      
+      EigenFromPy<MatType>::registration();
+
       // Add also conversion to Eigen::MatrixBase<MatType>
-      bp::converter::registry::push_back
-      (reinterpret_cast<void *(*)(_object *)>(&EigenFromPy<MatType>::convertible),
-       &EigenFromPy<MatType>::construct,bp::type_id< Eigen::MatrixBase<MatType> >());
+      typedef Eigen::MatrixBase<MatType> MatTypeBase;
+      EigenFromPy<MatTypeBase>::registration();
     }
   };
 
@@ -419,7 +488,6 @@ namespace eigenpy
   };
 #endif
 
-  
   template<typename MatType>
   void enableEigenPySpecific()
   {