Merge pull request #267 from wxmerkt/topic/debug-quaternion-initialisation-segfault

Fix quaternion initialisation segfault under -march=native

Author: jcarpent

include/eigenpy/quaternion.hpp

@@ -110,12 +110,16 @@ namespace eigenpy
     void visit(PyClass& cl) const 
     {
       cl
-      .def(bp::init<Matrix3>((bp::arg("self"),bp::arg("R")),
-                             "Initialize from rotation matrix.\n"
-                             "\tR : a rotation matrix 3x3.")[bp::return_value_policy<bp::return_by_value>()])
-      .def(bp::init<AngleAxis>((bp::arg("self"),bp::arg("aa")),
-                               "Initialize from an angle axis.\n"
-                               "\taa: angle axis object."))
+      .def("__init__",bp::make_constructor(&QuaternionVisitor::FromRotationMatrix,
+                                           bp::default_call_policies(),
+                                           (bp::arg("R"))),
+                                           "Initialize from rotation matrix.\n"
+                                           "\tR : a rotation matrix 3x3.")
+      .def("__init__",bp::make_constructor(&QuaternionVisitor::FromAngleAxis,
+                                           bp::default_call_policies(),
+                                           (bp::arg("aa"))),
+                                           "Initialize from an angle axis.\n"
+                                           "\taa: angle axis object.")
       .def(bp::init<Quaternion>((bp::arg("self"),bp::arg("quat")),
                                 "Copy constructor.\n"
                                 "\tquat: a quaternion.")[bp::return_value_policy<bp::return_by_value>()])
@@ -130,11 +134,12 @@ namespace eigenpy
            "\tvec4 : a 4D vector representing quaternion coefficients in the order xyzw.")
       .def("__init__",bp::make_constructor(&QuaternionVisitor::DefaultConstructor),
            "Default constructor")
-      .def(bp::init<Scalar,Scalar,Scalar,Scalar>
-           ((bp::arg("self"),bp::arg("w"),bp::arg("x"),bp::arg("y"),bp::arg("z")),
+      .def("__init__",bp::make_constructor(&QuaternionVisitor::FromCoefficients,
+                                           bp::default_call_policies(),
+                                           (bp::arg("w"),bp::arg("x"),bp::arg("y"),bp::arg("z"))),
             "Initialize from coefficients.\n\n"
             "... note:: The order of coefficients is *w*, *x*, *y*, *z*. "
-            "The [] operator numbers them differently, 0...4 for *x* *y* *z* *w*!"))
+            "The [] operator numbers them differently, 0...4 for *x* *y* *z* *w*!")
 
       .add_property("x",
                     &QuaternionVisitor::getCoeff<0>,
@@ -244,16 +249,16 @@ namespace eigenpy
       ;
     }
   private:
-    
+
     template<int i>
     static void setCoeff(Quaternion & self, Scalar value) { self.coeffs()[i] = value; }
-    
+
     template<int i>
     static Scalar getCoeff(Quaternion & self) { return self.coeffs()[i]; }
-    
+
     static Quaternion & setFromTwoVectors(Quaternion & self, const Vector3 & a, const Vector3 & b)
     { return self.setFromTwoVectors(a,b); }
-    
+
     template<typename OtherQuat>
     static Quaternion & assign(Quaternion & self, const OtherQuat & quat)
     { return self = quat; }
@@ -263,24 +268,42 @@ namespace eigenpy
       Quaternion* q(new Quaternion); q->setIdentity();
       return q;
     }
-    
-    static Quaternion* FromTwoVectors(const Vector3& u, const Vector3& v)
+
+    static Quaternion* FromCoefficients(Scalar w, Scalar x, Scalar y, Scalar z)
+    {
+      Quaternion* q(new Quaternion(w, x, y, z));
+      return q;
+    }
+
+    static Quaternion* FromAngleAxis(const AngleAxis& aa)
+    {
+      Quaternion* q(new Quaternion(aa));
+      return q;
+    }
+
+    static Quaternion* FromTwoVectors(const Eigen::Ref<Vector3> u, const Eigen::Ref<Vector3> v)
     {
       Quaternion* q(new Quaternion); q->setFromTwoVectors(u,v);
       return q;
     }
-    
+
     static Quaternion* DefaultConstructor()
     {
       return new Quaternion;
     }
-    
+
     static Quaternion* FromOneVector(const Eigen::Ref<Vector4> v)
     {
       Quaternion* q(new Quaternion(v[3],v[0],v[1],v[2]));
       return q;
     }
-  
+
+    static Quaternion* FromRotationMatrix(const Eigen::Ref<Matrix3> R)
+    {
+      Quaternion* q(new Quaternion(R));
+      return q;
+    }
+
     static bool __eq__(const Quaternion & u, const Quaternion & v)
     {
       return u.coeffs() == v.coeffs();

unittest/matrix.cpp

@@ -101,7 +101,7 @@ generateRowMajorMatrix(const Eigen::DenseIndex rows,
   typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> RowMajorMatrix;
   RowMajorMatrix A(rows, cols);
   typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> Vector;
-  Eigen::Map<Vector>(A.data(),A.size()) = Vector::LinSpaced(A.size(),1,A.size());
+  Eigen::Map<Vector>(A.data(),A.size()) = Vector::LinSpaced(A.size(), 1, static_cast<Scalar>(A.size()));
   std::cout << "Matrix values:\n" << A << std::endl;
   return A;
 }
@@ -112,7 +112,7 @@ generateRowMajorVector(const Eigen::DenseIndex size)
 {
   typedef Eigen::Matrix<Scalar,1,Eigen::Dynamic,Eigen::RowMajor> RowMajorVector;
   RowMajorVector A(size);
-  A.setLinSpaced(size,1,size);
+  A.setLinSpaced(size, 1, static_cast<Scalar>(size));
   std::cout << "Vector values: " << A.transpose() << std::endl;
   return A;
 }
@@ -125,7 +125,7 @@ generateColMajorMatrix(const Eigen::DenseIndex rows,
   typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> ColMajorMatrix;
   ColMajorMatrix A(rows, cols);
   typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> Vector;
-  Eigen::Map<Vector>(A.data(),A.size()) = Vector::LinSpaced(A.size(),1,A.size());
+  Eigen::Map<Vector>(A.data(),A.size()) = Vector::LinSpaced(A.size(), 1, static_cast<Scalar>(A.size()));
   std::cout << "Matrix values:\n" << A << std::endl;
   return A;
 }
@@ -136,7 +136,7 @@ generateColMajorVector(const Eigen::DenseIndex size)
 {
   typedef Eigen::Matrix<Scalar,1,Eigen::Dynamic> ColMajorVector;
   ColMajorVector A(size);
-  A.setLinSpaced(size,1,size);
+  A.setLinSpaced(size, 1, static_cast<Scalar>(size));
   std::cout << "Vector values: " << A.transpose() << std::endl;
   return A;
 }

unittest/python/test_geometry.py

@@ -13,15 +13,21 @@ def isapprox(a,b,epsilon=1e-6):
         return abs(a-b)<epsilon
 
 # --- Quaternion ---------------------------------------------------------------
+# Coefficient initialisation
+verbose and print("[Quaternion] Coefficient initialisation")
 q = Quaternion(1,2,3,4)
 q.normalize()
 assert(isapprox(np.linalg.norm(q.coeffs()),q.norm()))
 assert(isapprox(np.linalg.norm(q.coeffs()),1))
 
+# Coefficient-vector initialisation
+verbose and print("[Quaternion] Coefficient-vector initialisation")
 v = np.array([0.5,-0.5,0.5,0.5])
 qv = Quaternion(v)
 assert(isapprox(qv.coeffs(), v))
 
+# Angle axis initialisation
+verbose and print("[Quaternion] AngleAxis initialisation")
 r = AngleAxis(q)
 q2 = Quaternion(r)
 assert(q==q)
@@ -34,6 +40,8 @@ def isapprox(a,b,epsilon=1e-6):
 assert(isapprox(Rq.dot(Rq.T),np.eye(3)))
 assert(isapprox(Rr,Rq))
 
+# Rotation matrix initialisation
+verbose and print("[Quaternion] Rotation Matrix initialisation")
 qR = Quaternion(Rr)
 assert(q.isApprox(qR))
 assert(isapprox(q.coeffs(),qR.coeffs()))
@@ -43,7 +51,7 @@ def isapprox(a,b,epsilon=1e-6):
    qR[5]
    print("Error, this message should not appear.")
 except RuntimeError as e:
-  if verbose: print("As expected, catched exception: ",e)
+  if verbose: print("As expected, caught exception: ", e)
 
 # --- Angle Vector ------------------------------------------------
 r = AngleAxis(.1,np.array([1,0,0],np.double))