[operator] Add HomogeneousMatrixToSE3Vector & SE3VectorToHomogeneousMatrix

Allows to convert a vector of size 12 (which represents an Homogeneous Matrix) to a 'real' Homogeneous Matrix (dg type).
And the opposite; add comparison test in test_operator.cpp.

Author: NoelieRamuzat

src/dynamic_graph/sot/core/math_small_entities.py

@@ -12,8 +12,8 @@
     Inverse_of_matrixrotation, Inverse_of_matrixtwist, Inverse_of_unitquat,
     MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion, PoseQuatToMatrixHomo,
     MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta, MatrixToHomo,
-    MatrixToQuaternion, MatrixToRPY, MatrixToUTheta, MatrixTranspose,
-    Multiply_double_vector, Multiply_matrix_vector,
+    MatrixToQuaternion, MatrixToRPY, MatrixToUTheta, MatrixHomoToSE3Vector, SE3VectorToMatrixHomo,
+    MatrixTranspose, Multiply_double_vector, Multiply_matrix_vector,
     Multiply_matrixTwist_vector, Multiply_matrixHomo_vector,
     Multiply_of_double, Multiply_of_matrix, Multiply_of_matrixHomo,
     Multiply_of_matrixrotation, Multiply_of_matrixtwist,

src/matrix/operator.cpp

@@ -321,6 +321,28 @@ struct HomogeneousMatrixToVector
 };
 REGISTER_UNARY_OP(HomogeneousMatrixToVector, MatrixHomoToPoseUTheta);
 
+struct HomogeneousMatrixToSE3Vector
+    : public UnaryOpHeader<MatrixHomogeneous, dg::Vector> {
+  void operator()(const MatrixHomogeneous &M, dg::Vector &res) {
+    res.resize(12);
+    res.head<3>() = M.translation();
+    res.segment(3, 3) = M.linear().row(0); 
+    res.segment(6, 3) = M.linear().row(1); 
+    res.segment(9, 3) = M.linear().row(2);   }
+};
+REGISTER_UNARY_OP(HomogeneousMatrixToSE3Vector, MatrixHomoToSE3Vector);
+
+struct SE3VectorToHomogeneousMatrix
+    : public UnaryOpHeader<dg::Vector, MatrixHomogeneous> {
+  void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {
+    Mres.translation() = vect.head<3>();
+    Mres.linear().row(0) = vect.segment(3, 3);
+    Mres.linear().row(1) = vect.segment(6, 3);
+    Mres.linear().row(2) = vect.segment(9, 3);
+  }
+};
+REGISTER_UNARY_OP(SE3VectorToHomogeneousMatrix, SE3VectorToMatrixHomo);
+
 struct SkewSymToVector : public UnaryOpHeader<Matrix, Vector> {
   void operator()(const Matrix &M, Vector &res) {
     res.resize(3);

tests/matrix/test_operator.cpp

@@ -198,5 +198,8 @@ BOOST_AUTO_TEST_CASE(matrix_quaternion) {
 BOOST_AUTO_TEST_CASE(matrixHomo_poseQuaternion) {
   test_impl<MatrixHomoToPoseQuaternion, PoseQuaternionToMatrixHomo>();
 }
+BOOST_AUTO_TEST_CASE(matrixHomo_se3Vector) {
+  test_impl<HomogeneousMatrixToSE3Vector, SE3VectorToHomogeneousMatrix>();
+}
 
 BOOST_AUTO_TEST_SUITE_END()