Commit 68982317 by jcarpent

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

parent af186283
 ... ... @@ -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 inline Eigen::Matrix skewSquare(const Eigen::MatrixBase & u, const Eigen::MatrixBase & v) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D1,3); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D2,3); typedef Eigen::DiagonalMatrix DiagonalMatrix; const typename D1::Scalar udotv(u.dot(v)); Eigen::Matrix res(v*u.transpose()); res -= DiagonalMatrix(udotv,udotv,udotv); return res; } template inline Eigen::Matrix ... ...
 // // 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 ()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!