15 #ifndef BELUGA_ROS_TF2_SOPHUS_HPP
16 #define BELUGA_ROS_TF2_SOPHUS_HPP
18 #include <beluga/algorithm/unscented_transform.hpp>
19 #include <beluga/eigen_compatibility.hpp>
21 #include <sophus/common.hpp>
22 #include <tf2/convert.hpp>
23 #include <tf2/utils.hpp>
25 #include <tf2_eigen/tf2_eigen.hpp>
26 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
28 #include <sophus/se2.hpp>
29 #include <sophus/se3.hpp>
30 #include <sophus/types.hpp>
32 #include <geometry_msgs/msg/pose.hpp>
33 #include <geometry_msgs/msg/pose_with_covariance.hpp>
34 #include <geometry_msgs/msg/transform.hpp>
44 template <
typename Scalar>
49 const auto se3 = Sophus::SE3<Scalar>::exp(tangent);
53 const Eigen::Vector<Scalar, 3> euler_angles = se3.so3().matrix().eulerAngles(2, 1, 0);
54 Eigen::Vector<Scalar, 6> ret;
55 ret.template head<3>() = se3.translation();
56 ret[3] = euler_angles.z();
57 ret[4] = euler_angles.y();
58 ret[5] = euler_angles.x();
76 template <
class Scalar>
77 inline geometry_msgs::msg::Pose&
toMsg(
const Sophus::SE2<Scalar>& in, geometry_msgs::msg::Pose& out) {
78 const double theta = in.so2().log();
79 out.position.x = in.translation().x();
80 out.position.y = in.translation().y();
82 out.orientation.w = std::cos(theta / 2.);
83 out.orientation.x = 0;
84 out.orientation.y = 0;
85 out.orientation.z = std::sin(theta / 2.);
99 template <
class Scalar>
100 inline geometry_msgs::msg::Pose&
toMsg(
const Sophus::SE3<Scalar>& in, geometry_msgs::msg::Pose& out) {
101 out.position.x = in.translation().x();
102 out.position.y = in.translation().y();
103 out.position.z = in.translation().z();
104 out.orientation.w = in.so3().unit_quaternion().w();
105 out.orientation.x = in.so3().unit_quaternion().x();
106 out.orientation.y = in.so3().unit_quaternion().y();
107 out.orientation.z = in.so3().unit_quaternion().z();
118 template <
class Scalar>
119 inline geometry_msgs::msg::Transform
toMsg(
const Sophus::SE2<Scalar>& in) {
120 auto msg = geometry_msgs::msg::Transform{};
121 const double theta = in.so2().log();
122 msg.translation.x = in.translation().x();
123 msg.translation.y = in.translation().y();
124 msg.translation.z = 0;
125 msg.rotation.w = std::cos(theta / 2.);
128 msg.rotation.z = std::sin(theta / 2.);
139 template <
class Scalar>
140 inline geometry_msgs::msg::Transform
toMsg(
const Sophus::SE3<Scalar>& in) {
141 auto msg = geometry_msgs::msg::Transform{};
142 msg.translation.x = in.translation().x();
143 msg.translation.y = in.translation().y();
144 msg.translation.z = in.translation().z();
145 msg.rotation.w = in.so3().unit_quaternion().w();
146 msg.rotation.x = in.so3().unit_quaternion().x();
147 msg.rotation.y = in.so3().unit_quaternion().y();
148 msg.rotation.z = in.so3().unit_quaternion().z();
159 template <
class Scalar>
160 inline void fromMsg(
const geometry_msgs::msg::Transform& msg, Sophus::SE2<Scalar>& out) {
161 out.translation() = Sophus::Vector2<Scalar>{
165 out.so2() = Sophus::SO2<Scalar>{
static_cast<Scalar
>(tf2::getYaw(msg.rotation))};
175 template <
class Scalar>
176 inline void fromMsg(
const geometry_msgs::msg::Transform& msg, Sophus::SE3<Scalar>& out) {
177 out.translation() = Sophus::Vector3<Scalar>{
178 static_cast<Scalar
>(msg.translation.x),
179 static_cast<Scalar
>(msg.translation.y),
180 static_cast<Scalar
>(msg.translation.z),
182 out.so3() = Sophus::SO3<Scalar>{Eigen::Quaternion<Scalar>{
183 static_cast<Scalar
>(msg.rotation.w),
184 static_cast<Scalar
>(msg.rotation.x),
185 static_cast<Scalar
>(msg.rotation.y),
186 static_cast<Scalar
>(msg.rotation.z),
197 template <
class Scalar>
198 inline void fromMsg(
const geometry_msgs::msg::Pose& msg, Sophus::SE2<Scalar>& out) {
199 out.translation() = Sophus::Vector2<Scalar>{
200 static_cast<Scalar
>(msg.position.x),
201 static_cast<Scalar
>(msg.position.y),
203 out.so2() = Sophus::SO2<Scalar>{
static_cast<Scalar
>(tf2::getYaw(msg.orientation))};
213 template <
class Scalar>
214 inline void fromMsg(
const geometry_msgs::msg::Pose& msg, Sophus::SE3<Scalar>& out) {
215 out.translation() = Sophus::Vector3<Scalar>{
216 static_cast<Scalar
>(msg.position.x),
217 static_cast<Scalar
>(msg.position.y),
218 static_cast<Scalar
>(msg.position.z),
220 out.so3() = Sophus::SO3<Scalar>{Eigen::Quaternion<Scalar>{
221 static_cast<Scalar
>(msg.orientation.w),
222 static_cast<Scalar
>(msg.orientation.x),
223 static_cast<Scalar
>(msg.orientation.y),
224 static_cast<Scalar
>(msg.orientation.z),
234 template <
template <
typename, std::
size_t>
class Array,
typename Scalar>
237 out[0] = in.coeff(0, 0);
238 out[1] = in.coeff(0, 1);
239 out[5] = in.coeff(0, 2);
240 out[6] = in.coeff(1, 0);
241 out[7] = in.coeff(1, 1);
242 out[11] = in.coeff(1, 2);
243 out[30] = in.coeff(2, 0);
244 out[31] = in.coeff(2, 1);
245 out[35] = in.coeff(2, 2);
255 template <
template <
typename, std::
size_t>
class Array,
typename Scalar>
257 out.coeffRef(0, 0) = in[0];
258 out.coeffRef(0, 1) = in[1];
259 out.coeffRef(0, 2) = in[5];
260 out.coeffRef(1, 0) = in[6];
261 out.coeffRef(1, 1) = in[7];
262 out.coeffRef(1, 2) = in[11];
263 out.coeffRef(2, 0) = in[30];
264 out.coeffRef(2, 1) = in[31];
265 out.coeffRef(2, 2) = in[35];
272 template <
typename Scalar>
273 inline geometry_msgs::msg::PoseWithCovariance
toMsg(
274 const Sophus::SE3<Scalar>& in,
275 const Eigen::Matrix<Scalar, 6, 6>& covariance) {
276 geometry_msgs::msg::PoseWithCovariance out;
284 in.log(),
covariance, detail::tangential_space_to_xyz_rpy<Scalar>, std::nullopt,
285 [](
const std::vector<Eigen::Vector<Scalar, 6>>& samples,
const std::vector<Scalar>&
weights) {
286 Eigen::Vector<Scalar, 6> out = Eigen::Vector<Scalar, 6>::Zero();
287 Eigen::Vector<Scalar, 2> roll_aux = Eigen::Vector<Scalar, 2>::Zero();
288 Eigen::Vector<Scalar, 2> pitch_aux = Eigen::Vector<Scalar, 2>::Zero();
289 Eigen::Vector<Scalar, 2> yaw_aux = Eigen::Vector<Scalar, 2>::Zero();
291 for (const auto& [s, w] : ranges::views::zip(samples, weights)) {
292 out.template head<3>() += s.template head<3>() * w;
293 roll_aux.x() = std::sin(s[3]) * w;
294 roll_aux.y() = std::cos(s[3]) * w;
295 pitch_aux.x() = std::sin(s[4]) * w;
296 pitch_aux.y() = std::cos(s[4]) * w;
297 yaw_aux.x() = std::sin(s[5]) * w;
298 yaw_aux.y() = std::cos(s[5]) * w;
301 out[3] = std::atan2(roll_aux.x(), roll_aux.y());
302 out[4] = std::atan2(pitch_aux.x(), pitch_aux.y());
303 out[5] = std::atan2(yaw_aux.x(), yaw_aux.y());
306 [](
const Eigen::Vector<Scalar, 6>&
sample,
const Eigen::Vector<Scalar, 6>&
mean) {
307 Eigen::Vector<Scalar, 6> out;
308 const Sophus::SO3<Scalar> sample_so3(
309 Sophus::SO3<Scalar>::rotZ(sample[5]) * Sophus::SO3<Scalar>::rotY(sample[4]) *
310 Sophus::SO3<Scalar>::rotX(sample[3]));
312 const Sophus::SO3<Scalar> mean_so3(
313 Sophus::SO3<Scalar>::rotZ(mean[5]) * Sophus::SO3<Scalar>::rotY(mean[4]) *
314 Sophus::SO3<Scalar>::rotX(mean[3]));
316 const Sophus::SO3<Scalar> delta = mean_so3.inverse() * sample_so3;
317 const Eigen::AngleAxis<Scalar> angle_axis{delta.unit_quaternion()};
318 out.template head<3>() =
sample.template head<3>() -
mean.template head<3>();
319 out.template tail<3>() = angle_axis.axis() * angle_axis.angle();
324 Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>>(out.covariance.data()) =
325 base_pose_covariance_xyz_rpy.template cast<double>();
335 template <
class Scalar>
336 inline geometry_msgs::msg::Transform
toMsg(
const Sophus::SE2<Scalar>& pose) {
341 template <
class Scalar>
342 inline geometry_msgs::msg::Transform
toMsg(
const Sophus::SE3<Scalar>& in) {
347 template <
class Scalar>
348 inline void fromMsg(
const geometry_msgs::msg::Transform& message, Sophus::SE2<Scalar>& pose) {
353 template <
class Scalar>
354 inline void fromMsg(
const geometry_msgs::msg::Transform& message, Sophus::SE3<Scalar>& pose) {
359 template <
class Scalar>
360 inline void fromMsg(
const geometry_msgs::msg::Pose& message, Sophus::SE2<Scalar>& pose) {
365 template <
class Scalar>
366 inline void fromMsg(
const geometry_msgs::msg::Pose& message, Sophus::SE3<Scalar>& pose) {
Sophus namespace extension for message conversion function overload resolution (ADL enabled).
Definition: tf2_sophus.hpp:332
void fromMsg(const geometry_msgs::msg::Transform &message, Sophus::SE2< Scalar > &pose)
Extracts an SE2 pose from a geometry_msgs/Transform message.
Definition: tf2_sophus.hpp:348
geometry_msgs::msg::Transform toMsg(const Sophus::SE2< Scalar > &pose)
Converts an SE2 pose to a geometry_msgs/Transform message.
Definition: tf2_sophus.hpp:336
constexpr detail::covariance_fn covariance
auto unscented_transform(const Eigen::MatrixBase< MeanDerived > &mean, const Eigen::MatrixBase< CovarianceDerived > &covariance, TransferFn &&transfer_fn, std::optional< typename MeanDerived::Scalar > kappa=std::nullopt, MeanFn mean_fn=default_weighted_mean, ResidualFn residual_fn=std::minus< TransformedT >{})
constexpr detail::mean_fn mean
tf2 namespace extension for message conversion overload resolution.
Definition: tf2_eigen.hpp:31
geometry_msgs::msg::Point toMsg(const Eigen::Matrix< Scalar, 2, 1 > &in)
Converts an Eigen Vector2 type to a Point message.
Definition: tf2_eigen.hpp:39
void fromMsg(const geometry_msgs::msg::Transform &msg, Sophus::SE2< Scalar > &out)
Converts a Transform message type to a Sophus SE2 type.
Definition: tf2_sophus.hpp:160
Sophus::Matrix3< Scalar > & covarianceRowMajorToEigen(const Array< Scalar, 36 > &in, Sophus::Matrix3< Scalar > &out)
Converts a 6x6 row-major array to an Eigen 3x3 covariance matrix.
Definition: tf2_sophus.hpp:256
Array< Scalar, 36 > & covarianceEigenToRowMajor(const Sophus::Matrix3< Scalar > &in, Array< Scalar, 36 > &out)
Converts a Sophus (ie. Eigen) 3x3 covariance matrix to a 6x6 row-major array.
Definition: tf2_sophus.hpp:235
constexpr ranges::views::view_closure< detail::sample_fn > sample
Eigen::Vector< Scalar, 6 > tangential_space_to_xyz_rpy(const Eigen::Vector< Scalar, 6 > &tangent)
Definition: tf2_sophus.hpp:45