[Spatial] Add new skewSquare linear operator from two 3d vectors

src/spatial/skew.hpp

@@ -77,6 +77,30 @@ namespace se3
     m(2,0) = - m(0,2);  m(2,1) =  - m(1,2);   m(2,2) =  0;
     return m;
   }
+  
+  ///
+  /// \brief Computes the square cross product linear operator C(u,v) such that for any vector w, \f$ u \times ( v \times w ) = C(u,v) w \f$.
+  ///
+  /// \param[in] u A 3 dimensional vector.
+  /// \param[in] v A 3 dimensional vector.
+  ///
+  /// \return The square cross product C matrix.
+  ///
+  template <typename D1, typename D2>
+  inline Eigen::Matrix<typename D1::Scalar,3,3,D1::Options>
+  skewSquare(const Eigen::MatrixBase<D1> & u, const Eigen::MatrixBase<D2> & v)
+  {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D1,3);
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D2,3);
+    
+    typedef Eigen::DiagonalMatrix<typename D1::Scalar,3> DiagonalMatrix;
+    
+    const typename D1::Scalar udotv(u.dot(v));
+    Eigen::Matrix<typename D1::Scalar,3,3,D1::Options> res(v*u.transpose());
+    res -= DiagonalMatrix(udotv,udotv,udotv);
+    
+    return res;
+  }
 
   template <typename V,typename M>
   inline Eigen::Matrix<typename M::Scalar,3,M::ColsAtCompileTime,M::Options>

unittest/tspatial.cpp

 //
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -371,4 +371,20 @@ BOOST_AUTO_TEST_CASE(test_skew)
   
 }
 
+BOOST_AUTO_TEST_CASE(test_skew_square)
+{
+  using namespace se3;
+  typedef SE3::Vector3 Vector3;
+  typedef SE3::Matrix3 Matrix3;
+  
+  Vector3 u(Vector3::Random());
+  Vector3 v(Vector3::Random());
+  
+  Matrix3 ref = skew(u) * skew(v);
+  
+  Matrix3 res = skewSquare(u,v);
+  
+  BOOST_CHECK(res.isApprox(ref));
+}
+
 BOOST_AUTO_TEST_SUITE_END ()