IVIGIT added Sym3 from Metapod::lti

CMakeLists.txt

@@ -32,6 +32,7 @@ ADD_REQUIRED_DEPENDENCY("urdfdom >= v0.3.0")
 # ----------------------------------------------------
 SET(${PROJECT_NAME}_HEADERS
   exception.hpp
+  spatial/symmetric3.hpp
   spatial/se3.hpp
   spatial/motion.hpp
   spatial/force.hpp
@@ -86,6 +87,9 @@ ENDFOREACH(header)
 ADD_EXECUTABLE(tspatial EXCLUDE_FROM_ALL unittest/tspatial.cpp)
 PKG_CONFIG_USE_DEPENDENCY(tspatial eigenpy)
 
+ADD_EXECUTABLE(symmetric EXCLUDE_FROM_ALL unittest/symmetric.cpp)
+PKG_CONFIG_USE_DEPENDENCY(symmetric eigenpy)
+
 ADD_EXECUTABLE(constraint EXCLUDE_FROM_ALL unittest/constraint.cpp)
 PKG_CONFIG_USE_DEPENDENCY(constraint eigenpy)
 

src/spatial/symmetric3.hpp

+/* Copyright LAAS-CNRS, 2014
+ *
+ * This file is originally copied from metapod/tools/spatial/lti.hh.
+ * Authors: Olivier Stasse (LAAS, CNRS) and Sébastien Barthélémy (Aldebaran Robotics)
+ * The file was modified in pinocchio by Nicolas Mansard (LAAS, CNRS)
+ *
+ * metapod is free software, distributed under the terms of the GNU Lesser
+ * General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ */
+
+#ifndef __se3__symmetric3_hpp__
+#define __se3__symmetric3_hpp__
+
+#include <ostream>
+
+namespace se3
+{
+
+  template<typename _Scalar, int _Options>
+  class Symmetric3Tpl
+  {
+  public:
+    typedef _Scalar Scalar;
+    enum { Options = _Options };
+    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
+    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
+    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
+    typedef Eigen::Matrix<Scalar,2,2,Options> Matrix2;
+    typedef Eigen::Matrix<Scalar,3,2,Options> Matrix32;
+
+  public:    
+    Symmetric3Tpl(): data() {}
+    Symmetric3Tpl(const Matrix3 &I) 
+    {
+      assert( (I-I.transpose()).isMuchSmallerThan(I) );
+      data(0) = I(0,0);
+      data(1) = I(1,0); data(2) = I(1,1);
+      data(3) = I(2,0); data(4) = I(2,1); data(5) = I(2,2);
+    }
+    Symmetric3Tpl(const Vector6 &I) : data(I) {}
+    Symmetric3Tpl(const double & a0,const double & a1,const double & a2,
+		  const double & a3,const double & a4,const double & a5)
+    { data << a0,a1,a2,a3,a4,a5; }
+
+    static Symmetric3Tpl Zero()     { return Symmetric3Tpl(Vector6::Zero()  );  }
+    static Symmetric3Tpl Random()   { return Symmetric3Tpl(Vector6::Random().eval());  }
+    static Symmetric3Tpl Identity() { return Symmetric3Tpl( 1, 0, 1, 0, 0, 1);  }
+    static Symmetric3Tpl SkewSq( const Vector3 & v )
+    { 
+      const double & x = v[0], & y = v[1], & z = v[2]; 
+      return Symmetric3Tpl( -y*y-z*z,
+			     x*y    ,  -x*x-z*z, 
+			     x*z    ,   y*z    ,  -x*x-y*y );
+    }
+    /* Shoot a positive definite matrix. */
+    static Symmetric3Tpl RandomPositive() 
+    { 
+      double 
+	a = double(std::rand())/RAND_MAX*2.0-1.0,
+	b = double(std::rand())/RAND_MAX*2.0-1.0,
+	c = double(std::rand())/RAND_MAX*2.0-1.0,
+	d = double(std::rand())/RAND_MAX*2.0-1.0,
+	e = double(std::rand())/RAND_MAX*2.0-1.0,
+	f = double(std::rand())/RAND_MAX*2.0-1.0;
+      return Symmetric3Tpl(a*a+b*b+d*d,
+			   a*b+b*c+d*e, b*b+c*c+e*e,
+			   a*d+b*e+d*f, b*d+c*e+e*f,  d*d+e*e+f*f );
+    }
+
+    Matrix3 matrix() const
+    {
+      Matrix3 res;
+      res(0,0) = data(0); res(0,1) = data(1); res(0,2) = data(3);
+      res(1,0) = data(1); res(1,1) = data(2); res(1,2) = data(4);
+      res(2,0) = data(3); res(2,1) = data(4); res(2,2) = data(5);
+      return res;
+    }
+    operator Matrix3 () const { return matrix(); }
+
+    Symmetric3Tpl operator+(const Symmetric3Tpl & s2) const
+    {
+      return Symmetric3Tpl(data+s2.data);
+    }
+
+    Symmetric3Tpl & operator+=(const Symmetric3Tpl & s2)
+    {
+      data += s2.data; return *this;
+    }
+
+    Vector3 operator*(const Vector3 &v) const
+    {
+      return Vector3(
+		     data(0) * v(0) + data(1) * v(1) + data(3) * v(2),
+		     data(1) * v(0) + data(2) * v(1) + data(4) * v(2),
+		     data(3) * v(0) + data(4) * v(1) + data(5) * v(2)
+		     );		     
+    }
+
+    // Matrix3 operator*(const Matrix3 &a) const
+    // {
+    //   Matrix3 r;
+    //   for(unsigned int i=0; i<3; ++i)
+    //     {
+    //       r(0,i) = data(0) * a(0,i) + data(1) * a(1,i) + data(3) * a(2,i);
+    //       r(1,i) = data(1) * a(0,i) + data(2) * a(1,i) + data(4) * a(2,i);
+    //       r(2,i) = data(3) * a(0,i) + data(4) * a(1,i) + data(5) * a(2,i);
+    //     }
+    //   return r;
+    // }
+
+    const Scalar& operator()(const int &i,const int &j) const
+    {
+      return ((i!=2)&&(j!=2)) ? data[i+j] : data[i+j+1];
+    }
+
+    Symmetric3Tpl operator-(const Matrix3 &S) const
+    {
+      assert( (S-S.transpose()).isMuchSmallerThan(S) );
+      return Symmetric3Tpl( data(0)-S(0,0),
+			    data(1)-S(1,0), data(2)-S(1,1),
+			    data(3)-S(2,0), data(4)-S(2,1), data(5)-S(2,2) );
+    }
+
+    Symmetric3Tpl operator+(const Matrix3 &S) const
+    {
+      assert( (S-S.transpose()).isMuchSmallerThan(S) );
+      return Symmetric3Tpl( data(0)+S(0,0),
+			    data(1)+S(1,0), data(2)+S(1,1),
+			    data(3)+S(2,0), data(4)+S(2,1), data(5)+S(2,2) );
+    }
+
+    /* --- Symmetric R*S*R' and R'*S*R products --- */
+  public: //private:
+    
+    /** \brief Computes L for a symmetric matrix A.
+     */
+    Matrix32  decomposeltI() const
+    {
+      Matrix32 L;
+      L << 
+	data(0) - data(5),    data(1),
+	data(1),              data(2) - data(5),
+	2*data(3),            data(4) + data(4);
+      return L;
+    }
+
+
+    /* R*S*R' */
+    Symmetric3Tpl rotate(const Matrix3 & R) const
+    {
+      assert( (R.cols()==3) && (R.rows()==3) );
+      assert( (R.transpose()*R).isApprox(Matrix3::Identity()) );
+
+	Symmetric3Tpl Sres;
+	Matrix2 Y;
+	Matrix32 L;
+
+	// 4 a
+	L = decomposeltI();
+	
+	// Y = R' L   ===> (12 m + 8 a)
+	Y = R.template block<2,3>(1,0) * L;
+
+	// Sres= Y R  ===> (16 m + 8a)
+	Sres.data(1) = Y(0,0)*R(0,0) + Y(0,1)*R(0,1);
+	Sres.data(2) = Y(0,0)*R(1,0) + Y(0,1)*R(1,1);
+	Sres.data(3) = Y(1,0)*R(0,0) + Y(1,1)*R(0,1);
+	Sres.data(4) = Y(1,0)*R(1,0) + Y(1,1)*R(1,1);
+	Sres.data(5) = Y(1,0)*R(2,0) + Y(1,1)*R(2,1);
+
+	// r=R' v ( 6m + 3a)
+	 const Vector3 r( -R(0,0)*data(4) + R(0,1)*data(3),
+			  -R(1,0)*data(4) + R(1,1)*data(3),
+			  -R(2,0)*data(4) + R(2,1)*data(3) );
+
+	// Sres_11 (3a)
+	Sres.data(0) = L(0,0) + L(1,1) - Sres.data(2) - Sres.data(5);
+	
+	// Sres + D + (Ev)x ( 9a)
+	Sres.data(0) += data(5); 
+	Sres.data(1) += r(2);    Sres.data(2)+= data(5); 
+	Sres.data(3) +=-r(1);    Sres.data(4)+=    r(0); Sres.data(5) += data(5);
+
+	return Sres;
+    }
+
+    /* R*S*R' */
+    template<typename D>
+    Symmetric3Tpl rotateTpl(const Eigen::MatrixBase<D> & R) const
+    {
+      assert( (R.cols()==3) && (R.rows()==3) );
+      assert( (R.transpose()*R).isApprox(Matrix3::Identity()) );
+
+	Symmetric3Tpl Sres;
+	Matrix2 Y;
+	Matrix32 L;
+
+	// 4 a
+	L = decomposeltI();
+	
+	// Y = R' L   ===> (12 m + 8 a)
+	Y = R.template block<2,3>(1,0) * L;
+
+	// Sres= Y R  ===> (16 m + 8a)
+	Sres.data(1) = Y(0,0)*R(0,0) + Y(0,1)*R(0,1);
+	Sres.data(2) = Y(0,0)*R(1,0) + Y(0,1)*R(1,1);
+	Sres.data(3) = Y(1,0)*R(0,0) + Y(1,1)*R(0,1);
+	Sres.data(4) = Y(1,0)*R(1,0) + Y(1,1)*R(1,1);
+	Sres.data(5) = Y(1,0)*R(2,0) + Y(1,1)*R(2,1);
+
+	// r=R' v ( 6m + 3a)
+	 const Vector3 r( -R(0,0)*data(4) + R(0,1)*data(3),
+			  -R(1,0)*data(4) + R(1,1)*data(3),
+			  -R(2,0)*data(4) + R(2,1)*data(3) );
+
+	// Sres_11 (3a)
+	Sres.data(0) = L(0,0) + L(1,1) - Sres.data(2) - Sres.data(5);
+	
+	// Sres + D + (Ev)x ( 9a)
+	Sres.data(0) += data(5); 
+	Sres.data(1) += r(2);    Sres.data(2)+= data(5); 
+	Sres.data(3) +=-r(1);    Sres.data(4)+=    r(0); Sres.data(5) += data(5);
+
+	return Sres;
+    }
+
+
+  public: //todo: make private
+
+    Vector6 data; 
+  };
+
+  typedef Symmetric3Tpl<double,0> Symmetric3;
+
+} // namespace se3
+
+#endif // ifndef __se3__symmetric3_hpp__
+

unittest/symmetric.cpp

+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/tools/timer.hpp"
+
+#include <boost/random.hpp>
+#include <assert.h>
+
+#include "pinocchio/spatial/symmetric3.hpp"
+
+#include <Eigen/StdVector>
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION( Eigen::Matrix3d );
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Symmetric3);
+
+void timeSym3(const se3::Symmetric3 & S,
+	      const se3::Symmetric3::Matrix3 & R,
+	      se3::Symmetric3 & res)
+{
+  res = S.rotate(R);
+}
+
+void testSym3()
+{
+  using namespace se3;
+  typedef Symmetric3::Matrix3 Matrix3;
+  typedef Symmetric3::Vector3 Vector3;
+  
+  { 
+    // op(Matrix3)
+    {
+      Matrix3 M = Matrix3::Random(); M = M*M.transpose();
+      Symmetric3 S(M);
+      assert( S.matrix().isApprox(M) );
+    }
+
+    // S += S
+    {
+      Symmetric3
+	S = Symmetric3::Random(),
+	S2 = Symmetric3::Random();
+      Symmetric3 Scopy = S;
+      S+=S2;
+      assert( S.matrix().isApprox( S2.matrix()+Scopy.matrix()) );
+    }
+
+    // S + M
+    {
+      Symmetric3 S = Symmetric3::Random();
+      Matrix3 M = Matrix3::Random(); M = M*M.transpose();
+
+      Symmetric3 S2 = S + M;
+      assert( S2.matrix().isApprox( S.matrix()+M ));
+
+      S2 = S - M;
+      assert( S2.matrix().isApprox( S.matrix()-M ));
+    }
+
+    // S*v
+    {
+      Symmetric3 S = Symmetric3::Random();
+      Vector3 v = Vector3::Random(); 
+      Vector3 Sv = S*v;
+      assert( Sv.isApprox( S.matrix()*v ));
+    }
+
+    // Random
+    for(int i=0;i<100;++i )
+      {
+	Matrix3 M = Matrix3::Random(); M = M*M.transpose();
+	Symmetric3 S = Symmetric3::RandomPositive();
+	Vector3 v = Vector3::Random();
+	assert( (v.transpose()*(S*v))[0] > 0);
+      }
+
+    // Identity
+    { 
+      assert( Symmetric3::Identity().matrix().isApprox( Matrix3::Identity() ) );
+    }
+
+    // Skew2
+    {
+      Vector3 v = Vector3::Random();
+      Symmetric3 vxvx = Symmetric3::SkewSq(v);
+
+      Vector3 p = Vector3::UnitX();
+      assert( (vxvx*p).isApprox( v.cross(v.cross(p)) ));
+      p = Vector3::UnitY();
+      assert( (vxvx*p).isApprox( v.cross(v.cross(p)) ));
+      p = Vector3::UnitZ();
+      assert( (vxvx*p).isApprox( v.cross(v.cross(p)) ));
+    }
+
+    // (i,j)
+    {
+	Matrix3 M = Matrix3::Random(); M = M*M.transpose();
+	M << 1,2,4,2,3,5,4,5,6 ;
+	Symmetric3 S(M); // = Symmetric3::RandomPositive();
+	for(int i=0;i<3;++i)
+	  for(int j=0;j<3;++j)
+	    assert( S(i,j) == M(i,j) );
+    }
+  }
+
+  // SRS
+  {
+    Symmetric3 S = Symmetric3::RandomPositive();
+    Matrix3 R = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
+    
+    Symmetric3 RSRt = S.rotate(R);
+    assert( RSRt.matrix().isApprox( R*S.matrix()*R.transpose() ));
+
+    Symmetric3 RtSR = S.rotate(R.transpose());
+    assert( RtSR.matrix().isApprox( R.transpose()*S.matrix()*R ));
+
+  }
+
+  // Time test 
+  {
+    const int NBT = 100000;
+    Symmetric3 S = Symmetric3::RandomPositive();
+
+    std::vector<Symmetric3> Sres (NBT);
+    std::vector<Matrix3> Rs (NBT);
+    for(int i=0;i<NBT;++i) 
+      Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
+
+    StackTicToc timer(StackTicToc::US); timer.tic();
+    SMOOTH(NBT)
+      {
+	timeSym3(S,Rs[_smooth],Sres[_smooth]);
+      }
+    timer.toc(std::cout,NBT);
+  }
+}
+/* --- METAPOD ---------------------------------------------- */
+#include <metapod/tools/spatial/lti.hh>
+#include <metapod/tools/spatial/rm-general.hh>
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::ltI<double>);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::RotationMatrixTpl<double>);
+
+void timeLTI(const metapod::Spatial::ltI<double>& S,
+	     const metapod::Spatial::RotationMatrixTpl<double>& R, 
+	     metapod::Spatial::ltI<double> & res)
+{
+  res = R.rotSymmetricMatrix(S);
+}
+
+void testLTI()
+{
+  using namespace metapod::Spatial;
+
+  typedef ltI<double> Sym3;
+  typedef Eigen::Matrix3d Matrix3;
+  typedef RotationMatrixTpl<double> R3;
+
+  Matrix3 M = Matrix3::Random();
+  Sym3 S(M),S2;
+
+  R3 R; R.randomInit();
+
+  R.rotTSymmetricMatrix(S);
+  timeLTI(S,R,S2);
+
+  assert( S2.toMatrix().isApprox( R.toMatrix().transpose()*S.toMatrix()*R.toMatrix()) );
+  
+  const int NBT = 100000;
+  std::vector<Sym3> Sres (NBT);
+  std::vector<R3> Rs (NBT);
+  for(int i=0;i<NBT;++i) 
+    Rs[i].randomInit();
+  
+  StackTicToc timer(StackTicToc::US); timer.tic();
+  SMOOTH(NBT)
+    {
+      timeLTI(S,Rs[_smooth],Sres[_smooth]);
+    }
+  timer.toc(std::cout,NBT);
+  //std::cout << Rs[std::rand() % NBT] << std::endl;
+  
+}
+
+void timeSelfAdj( const Eigen::Matrix3d & A,
+		  const Eigen::Matrix3d & Sdense,
+		  Eigen::Matrix3d & ASA )
+{
+  typedef se3::Inertia::Symmetric3 Sym3;
+  Sym3 S(Sdense);
+  ASA.triangularView<Eigen::Upper>()
+    = A * S * A.transpose();
+}
+
+void testSelfAdj()
+{
+  using namespace se3;
+  typedef Inertia::Matrix3 Matrix3;
+  typedef Inertia::Symmetric3 Sym3;
+
+  Matrix3 M = Inertia::Matrix3::Random();
+  Sym3 S(M);
+
+  Matrix3 M2 = Inertia::Matrix3::Random();
+  M.triangularView<Eigen::Upper>() = M2;
+
+  Matrix3 A = Matrix3::Random(), ASA1, ASA2;
+  ASA1.triangularView<Eigen::Upper>() = A * S * A.transpose();
+  timeSelfAdj(A,M,ASA2);
+  assert(ASA1.isApprox(ASA2));
+
+  StackTicToc timer(StackTicToc::US); timer.tic();
+  SMOOTH(1000000)
+    {
+      timeSelfAdj(A,M,ASA2);
+    }
+  timer.toc(std::cout,1000000);
+}
+
+
+int main()
+{
+  //testSelfAdj();
+  testLTI();
+  testSym3();
+  testSym3();
+  testLTI();
+  
+  std::cout << std::endl;
+  return 0;
+}