fix act

artivis · Mar 19, 2024 · 7572a43 · 7572a43
1 parent d693321
commit 7572a43
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 19 deletions.
diff --git a/include/manif/impl/sgal3/SGal3.h b/include/manif/impl/sgal3/SGal3.h
@@ -32,7 +32,7 @@ struct traits<SGal3<_Scalar>> {
   using Rotation       = Eigen::Matrix<Scalar, 3, 3>;
   using Translation    = Eigen::Matrix<Scalar, 3, 1>;
   using LinearVelocity = Eigen::Matrix<Scalar, 3, 1>;
-  using Vector         = Eigen::Matrix<Scalar, 4, 1>;
+  using Vector         = Eigen::Matrix<Scalar, 3, 1>;
 };
 
 } // namespace internal

diff --git a/include/manif/impl/sgal3/SGal3_base.h b/include/manif/impl/sgal3/SGal3_base.h
@@ -92,16 +92,16 @@ struct SGal3Base : LieGroupBase<_Derived> {
 
   /**
    * @brief Get the action
-   * @param[in] v A 4D point (3D + time).
+   * @param[in] v A 3D point.
    * @param[out] -optional- J_vout_m The Jacobian of the new object wrt this.
    * @param[out] -optional- J_vout_v The Jacobian of the new object wrt input object.
    */
   template <typename _EigenDerived>
-  Eigen::Matrix<Scalar, 4, 1>
+  Eigen::Matrix<Scalar, 3, 1>
   act(
     const Eigen::MatrixBase<_EigenDerived> &v,
-    tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 4, 10>>> J_vout_m = {},
-    tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 4, 4>>> J_vout_v = {}
+    tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 10>>> J_vout_m = {},
+    tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_vout_v = {}
   ) const;
 
   /**
@@ -339,30 +339,28 @@ SGal3Base<_Derived>::compose(
 
 template <typename _Derived>
 template <typename _EigenDerived>
-Eigen::Matrix<typename SGal3Base<_Derived>::Scalar, 4, 1>
+Eigen::Matrix<typename SGal3Base<_Derived>::Scalar, 3, 1>
 SGal3Base<_Derived>::act(
   const Eigen::MatrixBase<_EigenDerived> &v,
-  tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 4, 10>>> J_vout_m,
-  tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 4, 4>>> J_vout_v
+  tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 10>>> J_vout_m,
+  tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_vout_v
 ) const {
-  assert_vector_dim(v, 4);
+  assert_vector_dim(v, 3);
 
   const Rotation R(rotation());
 
   if (J_vout_m) {
-    MANIF_NOT_IMPLEMENTED_YET;
+    J_vout_m->template topLeftCorner<3, 3>() = R;
+    J_vout_m->template block<3, 3>(0, 3).setZero();
+    J_vout_m->template block<3, 3>(0, 6).noalias() = -R * skew(v);
+    J_vout_m->template topRightCorner<3, 1>().setZero();
   }
 
   if (J_vout_v) {
-    J_vout_v->setIdentity();
-    J_vout_v->template topLeftCorner<3, 3>() = R;
+    (*J_vout_v) = R;
   }
 
-  Eigen::Matrix<Scalar, 4, 1> p;
-  p << linearVelocity(), t() + v(3);
-  p.template head<3>() += R * v.template head<3>() + v(3) * translation();
-
-  return p;
+  return translation() + R * v;
 }
 
 

diff --git a/include/manif/impl/sgal3/SGal3_properties.h b/include/manif/impl/sgal3/SGal3_properties.h
@@ -14,14 +14,14 @@ namespace internal {
 //! traits specialization
 template <typename _Derived>
 struct LieGroupProperties<SGal3Base<_Derived>> {
-  static constexpr int Dim = 4; /// @brief Space dimension
+  static constexpr int Dim = 3; /// @brief Space dimension
   static constexpr int DoF = 10; /// @brief Degrees of freedom
 };
 
 //! traits specialization
 template <typename _Derived>
 struct LieGroupProperties<SGal3TangentBase<_Derived>> {
-  static constexpr int Dim = 4; /// @brief Space dimension
+  static constexpr int Dim = 3; /// @brief Space dimension
   static constexpr int DoF = 10; /// @brief Degrees of freedom
 };