Skip to content

Commit

Permalink
Update tf_sophus.hpp documentation style (#87)
Browse files Browse the repository at this point in the history
This patch updates the documentation style of the `tf_sophus.hpp` header
to match the style used in other parts of the codebase.

Signed-off-by: Nahuel Espinosa <[email protected]>
  • Loading branch information
nahueespinosa authored Jan 26, 2023
1 parent c829ce4 commit c43d564
Showing 1 changed file with 45 additions and 39 deletions.
84 changes: 45 additions & 39 deletions beluga_amcl/include/beluga_amcl/tf2_sophus.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,12 +26,13 @@
namespace tf2
{

/// Converts a Sophus SE2 type to a Pose message.
/**
* @brief Convert a Sophus SE2 type to a Pose message.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* @param in The Sophus SE2 to convert.
* @param out The pose converted to a Pose message.
* @return The pose converted to a Pose message.
*
* \param in The Sophus SE2 element to convert.
* \param out The pose converted to a Pose message.
* \return The pose converted to a Pose message.
*/
template<class Scalar>
inline geometry_msgs::msg::Pose & toMsg(
Expand All @@ -49,12 +50,13 @@ inline geometry_msgs::msg::Pose & toMsg(
return out;
}

/// Converts a Sophus SE3 type to a Pose message.
/**
* @brief Convert a Sophus SE3 type to a Pose message.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* @param in The Sophus SE3 to convert.
* @param out The pose converted to a Pose message.
* @return The pose converted to a Pose message.
*
* \param in The Sophus SE3 element to convert.
* \param out The pose converted to a Pose message.
* \return The pose converted to a Pose message.
*/
template<class Scalar>
inline geometry_msgs::msg::Pose & toMsg(
Expand All @@ -71,11 +73,12 @@ inline geometry_msgs::msg::Pose & toMsg(
return out;
}

/// Converts a Sophus SE2 type to a Transform message.
/**
* @brief Convert a Sophus SE2 type to a Transform message.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* @param in The Sophus SE2 to convert.
* @return The transform converted to a Transform message.
*
* \param in The Sophus SE2 element to convert.
* \return The transform converted to a Transform message.
*/
template<class Scalar>
inline geometry_msgs::msg::Transform toMsg(const Sophus::SE2<Scalar> & in)
Expand All @@ -92,11 +95,12 @@ inline geometry_msgs::msg::Transform toMsg(const Sophus::SE2<Scalar> & in)
return msg;
}

/// Converts a Sophus SE3 type to a Transform message.
/**
* @brief Convert a Sophus SE3 type to a Transform message.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* @param in The Sophus SE3 to convert.
* @return The transform converted to a Transform message.
*
* \param in The Sophus SE3 element to convert.
* \return The transform converted to a Transform message.
*/
template<class Scalar>
inline geometry_msgs::msg::Transform toMsg(const Sophus::SE3<Scalar> & in)
Expand All @@ -112,11 +116,12 @@ inline geometry_msgs::msg::Transform toMsg(const Sophus::SE3<Scalar> & in)
return msg;
}

/// Converts a Transform message type to a Sophus SE2 type.
/**
* @brief Convert a Transform message type to a Sophus-specific SE2 type.
* This function is a specialization of the fromMsg template defined in tf2/convert.h
* @param msg The Transform message to convert.
* @param out The transform converted to a Sophus SE2.
* This function is a specialization of the fromMsg template defined in tf2/convert.h.
*
* \param msg The transform message to convert.
* \param out The transform converted to a Sophus SE2 element.
*/
template<class Scalar>
inline void fromMsg(const geometry_msgs::msg::Transform & msg, Sophus::SE2<Scalar> & out)
Expand All @@ -128,11 +133,12 @@ inline void fromMsg(const geometry_msgs::msg::Transform & msg, Sophus::SE2<Scala
out.so2() = Sophus::SO2<Scalar>{tf2::getYaw(msg.rotation)};
}

/// Converts a Transform message type to a Sophus SE3 type.
/**
* @brief Convert a Transform message type to a Sophus-specific SE3 type.
* This function is a specialization of the fromMsg template defined in tf2/convert.h
* @param msg The Transform message to convert.
* @param out The transform converted to a Sophus SE3.
* This function is a specialization of the fromMsg template defined in tf2/convert.h.
*
* \param msg The transform message to convert.
* \param out The transform converted to a Sophus SE3 element.
*/
template<class Scalar>
inline void fromMsg(const geometry_msgs::msg::Transform & msg, Sophus::SE3<Scalar> & out)
Expand All @@ -152,11 +158,12 @@ inline void fromMsg(const geometry_msgs::msg::Transform & msg, Sophus::SE3<Scala
};
}

/// Converts a Pose message type to a Sophus SE2 type.
/**
* @brief Convert a Pose message type to a Sophus-specific SE2 type.
* This function is a specialization of the fromMsg template defined in tf2/convert.h
* @param msg The Pose message to convert.
* @param out The pose converted to a Sophus SE2.
* This function is a specialization of the fromMsg template defined in tf2/convert.h.
*
* \param msg The pose message to convert.
* \param out The pose converted to a Sophus SE2 element.
*/
template<class Scalar>
inline void fromMsg(const geometry_msgs::msg::Pose & msg, Sophus::SE2<Scalar> & out)
Expand All @@ -168,11 +175,12 @@ inline void fromMsg(const geometry_msgs::msg::Pose & msg, Sophus::SE2<Scalar> &
out.so2() = Sophus::SO2<Scalar>{tf2::getYaw(msg.orientation)};
}

/// Converts a Pose message type to a Sophus SE3 type.
/**
* @brief Convert a Pose message type to a Sophus-specific SE3 type.
* This function is a specialization of the fromMsg template defined in tf2/convert.h
* @param msg The Pose message to convert.
* @param out The pose converted to a Sophus SE3.
* This function is a specialization of the fromMsg template defined in tf2/convert.h.
*
* \param msg The pose message to convert.
* \param out The pose converted to a Sophus SE3 element.
*/
template<class Scalar>
inline void fromMsg(const geometry_msgs::msg::Pose & msg, Sophus::SE3<Scalar> & out)
Expand All @@ -192,11 +200,10 @@ inline void fromMsg(const geometry_msgs::msg::Pose & msg, Sophus::SE3<Scalar> &
};
}

/// Converts an Eigen 3x3 covariance matrix to a 6x6 row-major array.
/**
* @brief Function that converts from an Eigen 3x3 matrix representation of a 2D
* covariance matrix to a 6x6 row-major representation in 3D.
* @param in An Eigen 3x3 matrix representation of a 2D covariance.
* @return A row-major array of 36 covariance values.
* \param in An Eigen 3x3 covariance matrix of a 2D pose (x, y, yaw).
* \return A row-major array of 36 covariance values of a 3D pose.
*/
template<class Scalar>
inline std::array<Scalar, 36> covarianceEigenToRowMajor(const Eigen::Matrix3<Scalar> & in)
Expand All @@ -215,12 +222,11 @@ inline std::array<Scalar, 36> covarianceEigenToRowMajor(const Eigen::Matrix3<Sca
return covariance;
}

/// Converts a 6x6 row-major array to an Eigen 3x3 covariance matrix.
/**
* @brief Function that converts from row-major array representing a 3D
* covariance matrix to an Eigen 3x3 matrix.
* @param in A row-major array of 36 covariance values.
* @param out An Eigen 3x3 matrix representation of a 2D covariance.
* @return An Eigen 3x3 matrix representation of a 2D covariance.
* \param in A row-major array of 36 covariance values of a 3D pose.
* \param out An Eigen 3x3 covariance matrix of a 2D pose (x, y, yaw).
* \return An Eigen 3x3 covariance matrix of a 2D pose (x, y, yaw).
*/
template<class Scalar>
inline Eigen::Matrix3<Scalar> & covarianceRowMajorToEigen(
Expand Down

0 comments on commit c43d564

Please sign in to comment.