[C++] Add constructor from Matrix6 for Inertia + unittest

src/spatial/inertia.hpp

@@ -123,14 +123,22 @@ namespace se3
     // Constructors
     InertiaTpl() : m(), c(), I() {}
 
-    InertiaTpl(Scalar_t m_, 
-     const Vector3 &c_, 
-     const Matrix3 &I_)
-    : m(m_),
-    c(c_),
-    I(I_)
+    InertiaTpl(const Scalar_t m_, const Vector3 &c_, const Matrix3 &I_)
+    : m(m_), c(c_), I(I_)
+    {}
+    
+    InertiaTpl(const Matrix6 & I6)
     {
-
+      assert((I6 - I6.transpose()).isMushSmallerThan(I6));
+      m = I6(LINEAR, LINEAR);
+      const Matrix3 & mc_cross = I6.template block <3,3> (ANGULAR,LINEAR);
+      c = skewInv(mc_cross);
+      c /= m;
+      
+      Matrix3 I3 (mc_cross * mc_cross);
+      I3 /= m;
+      I3 += I6.template block<3,3>(ANGULAR,ANGULAR);
+      I = Symmetric3(I3);
     }
 
     InertiaTpl(Scalar_t _m, 

unittest/tspatial.cpp

@@ -267,6 +267,10 @@ BOOST_AUTO_TEST_CASE ( test_Inertia )
   double kinetic_ref = v.toVector().transpose() * aI.matrix() * v.toVector();
   double kinetic = aI.vtiv(v);
   BOOST_CHECK_SMALL(kinetic_ref - kinetic, 1e-12);
+
+  // Test constructor (Matrix6)
+  Inertia I1_bis(I1.matrix());
+  is_matrix_absolutely_closed(I1.matrix(), I1_bis.matrix(), 1e-12);
 }
 
 BOOST_AUTO_TEST_CASE ( test_ActOnSet )