[operator] Add operator PoseQuaternionToHomogeneousMatrix

Author: NoelieRamuzat

src/dynamic_graph/sot/core/math_small_entities.py

@@ -10,7 +10,7 @@
     Compose_R_and_T, ConvolutionTemporal, HomoToMatrix, HomoToRotation,
     HomoToTwist, Inverse_of_matrix, Inverse_of_matrixHomo,
     Inverse_of_matrixrotation, Inverse_of_matrixtwist, Inverse_of_unitquat,
-    MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion,
+    MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion, PoseQuatToMatrixHomo,
     MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta, MatrixToHomo,
     MatrixToQuaternion, MatrixToRPY, MatrixToUTheta, MatrixTranspose,
     Multiply_double_vector, Multiply_matrix_vector,

src/matrix/operator.cpp

@@ -356,6 +356,15 @@ struct MatrixHomoToPoseQuaternion
 };
 REGISTER_UNARY_OP(MatrixHomoToPoseQuaternion, MatrixHomoToPoseQuaternion);
 
+struct PoseQuaternionToMatrixHomo
+    : public UnaryOpHeader<Vector, MatrixHomogeneous> {
+  void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {
+    Mres.translation() = vect.head<3>();
+    Mres.linear() = VectorQuaternion(vect.tail<4>()).toRotationMatrix();
+  }
+};
+REGISTER_UNARY_OP(PoseQuaternionToMatrixHomo, PoseQuatToMatrixHomo);
+
 struct MatrixHomoToPoseRollPitchYaw
     : public UnaryOpHeader<MatrixHomogeneous, Vector> {
   void operator()(const MatrixHomogeneous &M, dg::Vector &res) {