Skip to content

Commit

Permalink
address review
Browse files Browse the repository at this point in the history
  • Loading branch information
artivis committed Mar 21, 2024
1 parent 82634d5 commit c9776f9
Showing 1 changed file with 13 additions and 13 deletions.
26 changes: 13 additions & 13 deletions include/manif/impl/sgal3/SGal3_base.h
Original file line number Diff line number Diff line change
Expand Up @@ -93,15 +93,15 @@ struct SGal3Base : LieGroupBase<_Derived> {
/**
* @brief Get the action
* @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_p The Jacobian of the new object wrt input object.
* @param[out] -optional- J_pout_m The Jacobian of the new object wrt this.
* @param[out] -optional- J_pout_p The Jacobian of the new object wrt input object.
*/
template <typename _EigenDerived>
Eigen::Matrix<Scalar, 3, 1>
act(
const Eigen::MatrixBase<_EigenDerived> &p,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 10>>> J_vout_m = {},
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_vout_p = {}
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 10>>> J_pout_m = {},
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_pout_p = {}
) const;

/**
Expand Down Expand Up @@ -325,22 +325,22 @@ template <typename _EigenDerived>
Eigen::Matrix<typename SGal3Base<_Derived>::Scalar, 3, 1>
SGal3Base<_Derived>::act(
const Eigen::MatrixBase<_EigenDerived> &p,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 10>>> J_vout_m,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_vout_p
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 10>>> J_pout_m,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_pout_p
) const {
assert_vector_dim(p, 3);

Check warning on line 331 in include/manif/impl/sgal3/SGal3_base.h

View check run for this annotation

Codecov / codecov/patch

include/manif/impl/sgal3/SGal3_base.h#L331

Added line #L331 was not covered by tests

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(p);
J_vout_m->template topRightCorner<3, 1>() = linearVelocity();
if (J_pout_m) {
J_pout_m->template topLeftCorner<3, 3>() = R;
J_pout_m->template block<3, 3>(0, 3).setZero();
J_pout_m->template block<3, 3>(0, 6).noalias() = -R * skew(p);
J_pout_m->template topRightCorner<3, 1>() = linearVelocity();
}

if (J_vout_p) {
(*J_vout_p) = R;
if (J_pout_p) {
(*J_pout_p) = R;
}

return translation() + R * p;
Expand Down

0 comments on commit c9776f9

Please sign in to comment.