Merge pull request #152 from NoelieRamuzat/devel

[operator] Add operator PoseQuaternionToHomogeneousMatrix

Author: nim65s

Diff for: 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,

Diff for: 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) {

Diff for: tests/matrix/test_operator.cpp

@@ -154,6 +154,12 @@ template <> VectorQuaternion random<VectorQuaternion>() {
 template <> MatrixRotation random<MatrixRotation>() {
   return MatrixRotation(random<VectorQuaternion>());
 }
+template <> MatrixHomogeneous random<MatrixHomogeneous>() {
+  MatrixHomogeneous matrix_homo;
+  matrix_homo.translation() = Eigen::Vector3d::Random();
+  matrix_homo.linear() = random<MatrixRotation>();
+  return matrix_homo;
+}
 
 template <typename type> bool compare(const type &a, const type &b) {
   return a.isApprox(b);
@@ -189,5 +195,8 @@ BOOST_AUTO_TEST_CASE(quaternion_rpy) {
 BOOST_AUTO_TEST_CASE(matrix_quaternion) {
   test_impl<MatrixToQuaternion, QuaternionToMatrix>();
 }
+BOOST_AUTO_TEST_CASE(matrixHomo_poseQuaternion) {
+  test_impl<MatrixHomoToPoseQuaternion, PoseQuaternionToMatrixHomo>();
+}
 
 BOOST_AUTO_TEST_SUITE_END()