From da1237567b2ca1c5755ffb3410a4bfe12feb8a8b Mon Sep 17 00:00:00 2001
From: Arnaud TANGUY <arn.tanguy@gmail.com>
Date: Mon, 11 Sep 2023 09:53:12 +0200
Subject: [PATCH] [KinematicInertial] Fix use of state-observation

---
 .../KinematicInertialPoseObserver.h           | 35 -------------------
 .../KinematicInertialPoseObserver.cpp         | 34 ++----------------
 2 files changed, 3 insertions(+), 66 deletions(-)

diff --git a/include/mc_observers/KinematicInertialPoseObserver.h b/include/mc_observers/KinematicInertialPoseObserver.h
index a3a06c0303..ba60a68240 100644
--- a/include/mc_observers/KinematicInertialPoseObserver.h
+++ b/include/mc_observers/KinematicInertialPoseObserver.h
@@ -101,41 +101,6 @@ struct MC_OBSERVER_DLLAPI KinematicInertialPoseObserver : public Observer
    */
   void estimatePosition(const mc_control::MCController & ctl);
 
-protected:
-  /**
-   * @brief Merge roll and pitch orientation from a rotation matrix R1 with yaw (rotation around gravity) from another
-   * rotation matrix R2
-   *
-   * This function was adpated from https://github.com/mehdi-benallegue/state-observation and modified to follow
-   * SpaceVecAlg conventions for rotation. It computes:
-   *
-   *
-   * \f[
-   *  R=\left(
-   *  \begin{array}{ccc}
-   *   \frac{m\times e_{z}}{\left\Vert m\times e_{z}\right\Vert } & \frac{e_{z}\times m\times e_{z}}{\left\Vert m\times
-   * e_{z}\right\Vert } & e_{z}\end{array}\right)\left(\begin{array}{ccc} \frac{m_{l}\times v_{1}}{\left\Vert
-   * m_{l}\times v_{1}\right\Vert } & \frac{v_{1}\times m_{l}\times v_{1}}{\left\Vert m_{l}\times v_{1}\right\Vert } &
-   * v_{1}\end{array}
-   *   \right)^{T}\\
-   *   v_{1}=R_{1}e_{z}\qquad m_{l}=R_{2}m\\
-   *   m = \left\{
-   *   \begin{array}{c}
-   *   e_x \mbox{ if } ||R_2e_x \times v_1||^2 < \epsilon^2\\
-   *   e_y \mbox{ otherwise }
-   *   \end{array}
-   *   \right.
-   *  \f]
-   *
-   * @param R1 First rotation matrix (for roll and pitch)
-   * @param R2 Second rotation matrix (for yaw)
-   *
-   * @return a rotation matrix composed of roll and pitch from R1, yaw from R2
-   */
-  inline Eigen::Matrix3d mergeRoll1Pitch1WithYaw2(const Eigen::Matrix3d & R1,
-                                                  const Eigen::Matrix3d & R2,
-                                                  double epsilonAngle = 1e-16);
-
 protected:
   std::string robot_; /**< Robot to observe (default main robot) */
   std::string realRobot_; /**< Corresponding real robot (default main real robot) */
diff --git a/src/mc_observers/KinematicInertialPoseObserver.cpp b/src/mc_observers/KinematicInertialPoseObserver.cpp
index c72dd2e79c..0269a18b34 100644
--- a/src/mc_observers/KinematicInertialPoseObserver.cpp
+++ b/src/mc_observers/KinematicInertialPoseObserver.cpp
@@ -7,6 +7,7 @@
 
 #include <mc_observers/KinematicInertialPoseObserver.h>
 #include <mc_observers/ObserverMacros.h>
+#include <state-observation/tools/rigid-body-kinematics.hpp>
 
 #include <mc_control/MCController.h>
 #include <mc_rbdyn/rpy_utils.h>
@@ -110,7 +111,8 @@ void KinematicInertialPoseObserver::estimateOrientation(const mc_rbdyn::Robot &
   Eigen::Matrix3d E_0_mIMU = imuSensor.X_b_s().rotation().transpose() * imuSensor.orientation().toRotationMatrix();
   const Eigen::Matrix3d & E_0_cIMU = X_0_cIMU.rotation();
   // Estimate IMU orientation: merges roll+pitch from measurement with yaw from control
-  Eigen::Matrix3d E_0_eIMU = mergeRoll1Pitch1WithYaw2(E_0_mIMU, E_0_cIMU);
+  Eigen::Matrix3d E_0_eIMU =
+      stateObservation::kine::mergeRoll1Pitch1WithYaw2(E_0_mIMU.transpose(), E_0_cIMU.transpose());
   pose_.rotation() = X_rIMU_rBase.rotation() * E_0_eIMU.transpose();
 }
 
@@ -218,36 +220,6 @@ void KinematicInertialPoseObserver::addToGUI(const mc_control::MCController & ct
   showHidePose();
 }
 
-inline Eigen::Matrix3d KinematicInertialPoseObserver::mergeRoll1Pitch1WithYaw2(const Eigen::Matrix3d & R1,
-                                                                               const Eigen::Matrix3d & R2,
-                                                                               double epsilonAngle)
-{
-  using Matrix3 = Eigen::Matrix3d;
-  using Vector3 = Eigen::Vector3d;
-
-  const Vector3 & ez = Vector3::UnitZ();
-  Matrix3 R_temp1, R_temp2;
-  Vector3 v1 = R1 * ez;
-  Vector3 mlxv1 = (R2 * Vector3::UnitX()).cross(v1);
-  double n2 = mlxv1.squaredNorm();
-
-  if(n2 > epsilonAngle * epsilonAngle)
-  {
-    // we take m=ex
-    R_temp1 << -Vector3::UnitY(), Vector3::UnitX(), ez;
-    mlxv1 /= sqrt(n2);
-    R_temp2 << mlxv1.transpose(), v1.cross(mlxv1).transpose(), v1.transpose();
-    return R_temp1 * R_temp2;
-  }
-  else
-  {
-    // we take m=ey
-    mlxv1 = (R2 * Vector3::UnitY()).cross(v1).normalized();
-    R_temp2 << mlxv1.transpose(), v1.cross(mlxv1).transpose(), v1.transpose();
-    return R_temp2.transpose();
-  }
-}
-
 } // namespace mc_observers
 
 EXPORT_OBSERVER_MODULE("KinematicInertialPose", mc_observers::KinematicInertialPoseObserver)