diff --git a/include/manif/impl/sgal3/SGal3Tangent_base.h b/include/manif/impl/sgal3/SGal3Tangent_base.h index d33b211a..ad705827 100644 --- a/include/manif/impl/sgal3/SGal3Tangent_base.h +++ b/include/manif/impl/sgal3/SGal3Tangent_base.h @@ -90,7 +90,7 @@ struct SGal3TangentBase : TangentBase<_Derived> { // SGal3Tangent specific API - //! @brief Get the linear velocity part. + //! @brief Get the linear translation part. LinBlock lin(); const ConstLinBlock lin() const; @@ -98,7 +98,7 @@ struct SGal3TangentBase : TangentBase<_Derived> { AngBlock ang(); const ConstAngBlock ang() const; - //! @brief Get the linear acceleration part + //! @brief Get the linear velocity part LinBlock lin2(); const ConstLinBlock lin2() const; @@ -118,18 +118,6 @@ struct SGal3TangentBase : TangentBase<_Derived> { Eigen::Ref> E, const Eigen::Map>& so3 ); - - // static void fillL( - // Eigen::Ref> E, - // const Eigen::Map>& so3 - // ); - - // static void fillS( - // Eigen::Ref> S, - // const Eigen::Map>& so3, - // linearVel, - // t - // ); }; template diff --git a/include/manif/impl/sgal3/SGal3_base.h b/include/manif/impl/sgal3/SGal3_base.h index a5a06f3c..62553996 100644 --- a/include/manif/impl/sgal3/SGal3_base.h +++ b/include/manif/impl/sgal3/SGal3_base.h @@ -92,14 +92,14 @@ struct SGal3Base : LieGroupBase<_Derived> { /** * @brief Get the action - * @param[in] v A 3D point. + * @param[in] p 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 Eigen::Matrix act( - const Eigen::MatrixBase<_EigenDerived> &v, + const Eigen::MatrixBase<_EigenDerived> &p, tl::optional>> J_vout_m = {}, tl::optional>> J_vout_v = {} ) const; @@ -324,18 +324,18 @@ template template Eigen::Matrix::Scalar, 3, 1> SGal3Base<_Derived>::act( - const Eigen::MatrixBase<_EigenDerived> &v, + const Eigen::MatrixBase<_EigenDerived> &p, tl::optional>> J_vout_m, tl::optional>> J_vout_v ) const { - assert_vector_dim(v, 3); + assert_vector_dim(p, 3); const Rotation R(rotation()); if (J_vout_m) { 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 block<3, 3>(0, 6).noalias() = -R * skew(p); J_vout_m->template topRightCorner<3, 1>() = linearVelocity(); } @@ -343,7 +343,7 @@ SGal3Base<_Derived>::act( (*J_vout_v) = R; } - return translation() + R * v; + return translation() + R * p; }