15 #ifndef BELUGA_ROS_TF2_SOPHUS_HPP
16 #define BELUGA_ROS_TF2_SOPHUS_HPP
18 #include <tf2/convert.h>
19 #include <tf2/utils.h>
20 #include <beluga/algorithm/unscented_transform.hpp>
21 #include <beluga/eigen_compatibility.hpp>
23 #include <sophus/common.hpp>
25 #if BELUGA_ROS_VERSION == 2
26 #include <tf2_eigen/tf2_eigen.hpp>
27 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
28 #elif BELUGA_ROS_VERSION == 1
29 #include <tf2_eigen/tf2_eigen.h>
30 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
32 #error BELUGA_ROS_VERSION is not defined or invalid
35 #include <sophus/se2.hpp>
36 #include <sophus/se3.hpp>
37 #include <sophus/types.hpp>
39 #include <beluga_ros/messages.hpp>
49 template <
typename Scalar>
54 const auto se3 = Sophus::SE3<Scalar>::exp(tangent);
58 const Eigen::Vector<Scalar, 3> euler_angles = se3.so3().matrix().eulerAngles(2, 1, 0);
59 Eigen::Vector<Scalar, 6> ret;
60 ret.template head<3>() = se3.translation();
61 ret[3] = euler_angles.z();
62 ret[4] = euler_angles.y();
63 ret[5] = euler_angles.x();
81 template <
class Scalar>
82 inline beluga_ros::msg::Pose&
toMsg(
const Sophus::SE2<Scalar>& in, beluga_ros::msg::Pose& out) {
83 const double theta = in.so2().log();
84 out.position.x = in.translation().x();
85 out.position.y = in.translation().y();
87 out.orientation.w = std::cos(theta / 2.);
88 out.orientation.x = 0;
89 out.orientation.y = 0;
90 out.orientation.z = std::sin(theta / 2.);
104 template <
class Scalar>
105 inline beluga_ros::msg::Pose&
toMsg(
const Sophus::SE3<Scalar>& in, beluga_ros::msg::Pose& out) {
106 out.position.x = in.translation().x();
107 out.position.y = in.translation().y();
108 out.position.z = in.translation().z();
109 out.orientation.w = in.so3().unit_quaternion().w();
110 out.orientation.x = in.so3().unit_quaternion().x();
111 out.orientation.y = in.so3().unit_quaternion().y();
112 out.orientation.z = in.so3().unit_quaternion().z();
123 template <
class Scalar>
124 inline beluga_ros::msg::Transform
toMsg(
const Sophus::SE2<Scalar>& in) {
125 auto msg = beluga_ros::msg::Transform{};
126 const double theta = in.so2().log();
127 msg.translation.x = in.translation().x();
128 msg.translation.y = in.translation().y();
129 msg.translation.z = 0;
130 msg.rotation.w = std::cos(theta / 2.);
133 msg.rotation.z = std::sin(theta / 2.);
144 template <
class Scalar>
145 inline beluga_ros::msg::Transform
toMsg(
const Sophus::SE3<Scalar>& in) {
146 auto msg = beluga_ros::msg::Transform{};
147 msg.translation.x = in.translation().x();
148 msg.translation.y = in.translation().y();
149 msg.translation.z = in.translation().z();
150 msg.rotation.w = in.so3().unit_quaternion().w();
151 msg.rotation.x = in.so3().unit_quaternion().x();
152 msg.rotation.y = in.so3().unit_quaternion().y();
153 msg.rotation.z = in.so3().unit_quaternion().z();
164 template <
class Scalar>
165 inline void fromMsg(
const beluga_ros::msg::Transform& msg, Sophus::SE2<Scalar>& out) {
166 out.translation() = Sophus::Vector2<Scalar>{
170 out.so2() = Sophus::SO2<Scalar>{
static_cast<Scalar
>(tf2::getYaw(msg.rotation))};
180 template <
class Scalar>
181 inline void fromMsg(
const beluga_ros::msg::Transform& msg, Sophus::SE3<Scalar>& out) {
182 out.translation() = Sophus::Vector3<Scalar>{
183 static_cast<Scalar
>(msg.translation.x),
184 static_cast<Scalar
>(msg.translation.y),
185 static_cast<Scalar
>(msg.translation.z),
187 out.so3() = Sophus::SO3<Scalar>{Eigen::Quaternion<Scalar>{
188 static_cast<Scalar
>(msg.rotation.w),
189 static_cast<Scalar
>(msg.rotation.x),
190 static_cast<Scalar
>(msg.rotation.y),
191 static_cast<Scalar
>(msg.rotation.z),
202 template <
class Scalar>
203 inline void fromMsg(
const beluga_ros::msg::Pose& msg, Sophus::SE2<Scalar>& out) {
204 out.translation() = Sophus::Vector2<Scalar>{
205 static_cast<Scalar
>(msg.position.x),
206 static_cast<Scalar
>(msg.position.y),
208 out.so2() = Sophus::SO2<Scalar>{
static_cast<Scalar
>(tf2::getYaw(msg.orientation))};
218 template <
class Scalar>
219 inline void fromMsg(
const beluga_ros::msg::Pose& msg, Sophus::SE3<Scalar>& out) {
220 out.translation() = Sophus::Vector3<Scalar>{
221 static_cast<Scalar
>(msg.position.x),
222 static_cast<Scalar
>(msg.position.y),
223 static_cast<Scalar
>(msg.position.z),
225 out.so3() = Sophus::SO3<Scalar>{Eigen::Quaternion<Scalar>{
226 static_cast<Scalar
>(msg.orientation.w),
227 static_cast<Scalar
>(msg.orientation.x),
228 static_cast<Scalar
>(msg.orientation.y),
229 static_cast<Scalar
>(msg.orientation.z),
239 template <
template <
typename, std::
size_t>
class Array,
typename Scalar>
242 out[0] = in.coeff(0, 0);
243 out[1] = in.coeff(0, 1);
244 out[5] = in.coeff(0, 2);
245 out[6] = in.coeff(1, 0);
246 out[7] = in.coeff(1, 1);
247 out[11] = in.coeff(1, 2);
248 out[30] = in.coeff(2, 0);
249 out[31] = in.coeff(2, 1);
250 out[35] = in.coeff(2, 2);
260 template <
template <
typename, std::
size_t>
class Array,
typename Scalar>
262 out.coeffRef(0, 0) = in[0];
263 out.coeffRef(0, 1) = in[1];
264 out.coeffRef(0, 2) = in[5];
265 out.coeffRef(1, 0) = in[6];
266 out.coeffRef(1, 1) = in[7];
267 out.coeffRef(1, 2) = in[11];
268 out.coeffRef(2, 0) = in[30];
269 out.coeffRef(2, 1) = in[31];
270 out.coeffRef(2, 2) = in[35];
277 template <
typename Scalar>
278 inline beluga_ros::msg::PoseWithCovariance
toMsg(
279 const Sophus::SE3<Scalar>& in,
280 const Eigen::Matrix<Scalar, 6, 6>& covariance) {
281 beluga_ros::msg::PoseWithCovariance out;
289 in.log(),
covariance, detail::tangential_space_to_xyz_rpy<Scalar>, std::nullopt,
290 [](
const std::vector<Eigen::Vector<Scalar, 6>>& samples,
const std::vector<Scalar>&
weights) {
291 Eigen::Vector<Scalar, 6> out = Eigen::Vector<Scalar, 6>::Zero();
292 Eigen::Vector<Scalar, 2> roll_aux = Eigen::Vector<Scalar, 2>::Zero();
293 Eigen::Vector<Scalar, 2> pitch_aux = Eigen::Vector<Scalar, 2>::Zero();
294 Eigen::Vector<Scalar, 2> yaw_aux = Eigen::Vector<Scalar, 2>::Zero();
296 for (const auto& [s, w] : ranges::views::zip(samples, weights)) {
297 out.template head<3>() += s.template head<3>() * w;
298 roll_aux.x() = std::sin(s[3]) * w;
299 roll_aux.y() = std::cos(s[3]) * w;
300 pitch_aux.x() = std::sin(s[4]) * w;
301 pitch_aux.y() = std::cos(s[4]) * w;
302 yaw_aux.x() = std::sin(s[5]) * w;
303 yaw_aux.y() = std::cos(s[5]) * w;
306 out[3] = std::atan2(roll_aux.x(), roll_aux.y());
307 out[4] = std::atan2(pitch_aux.x(), pitch_aux.y());
308 out[5] = std::atan2(yaw_aux.x(), yaw_aux.y());
311 [](
const Eigen::Vector<Scalar, 6>&
sample,
const Eigen::Vector<Scalar, 6>&
mean) {
312 Eigen::Vector<Scalar, 6> out;
313 const Sophus::SO3<Scalar> sample_so3(
314 Sophus::SO3<Scalar>::rotZ(sample[5]) * Sophus::SO3<Scalar>::rotY(sample[4]) *
315 Sophus::SO3<Scalar>::rotX(sample[3]));
317 const Sophus::SO3<Scalar> mean_so3(
318 Sophus::SO3<Scalar>::rotZ(mean[5]) * Sophus::SO3<Scalar>::rotY(mean[4]) *
319 Sophus::SO3<Scalar>::rotX(mean[3]));
321 const Sophus::SO3<Scalar> delta = mean_so3.inverse() * sample_so3;
322 const Eigen::AngleAxis<Scalar> angle_axis{delta.unit_quaternion()};
323 out.template head<3>() =
sample.template head<3>() -
mean.template head<3>();
324 out.template tail<3>() = angle_axis.axis() * angle_axis.angle();
329 Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>>(out.covariance.data()) =
330 base_pose_covariance_xyz_rpy.template cast<double>();
340 template <
class Scalar>
341 inline beluga_ros::msg::Transform
toMsg(
const Sophus::SE2<Scalar>& pose) {
346 template <
class Scalar>
347 inline beluga_ros::msg::Transform
toMsg(
const Sophus::SE3<Scalar>& in) {
352 template <
class Scalar>
353 inline void fromMsg(
const beluga_ros::msg::Transform& message, Sophus::SE2<Scalar>& pose) {
358 template <
class Scalar>
359 inline void fromMsg(
const beluga_ros::msg::Transform& message, Sophus::SE3<Scalar>& pose) {
364 template <
class Scalar>
365 inline void fromMsg(
const beluga_ros::msg::Pose& message, Sophus::SE2<Scalar>& pose) {
370 template <
class Scalar>
371 inline void fromMsg(
const beluga_ros::msg::Pose& message, Sophus::SE3<Scalar>& pose) {
Sophus namespace extension for message conversion function overload resolution (ADL enabled).
Definition: tf2_sophus.hpp:337
beluga_ros::msg::Transform toMsg(const Sophus::SE2< Scalar > &pose)
Converts an SE2 pose to a geometry_msgs/Transform message.
Definition: tf2_sophus.hpp:341
void fromMsg(const beluga_ros::msg::Transform &message, Sophus::SE2< Scalar > &pose)
Extracts an SE2 pose from a geometry_msgs/Transform message.
Definition: tf2_sophus.hpp:353
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:38
beluga_ros::msg::Point toMsg(const Eigen::Matrix< Scalar, 2, 1 > &in)
Converts an Eigen Vector2 type to a Point message.
Definition: tf2_eigen.hpp:46
void fromMsg(const beluga_ros::msg::Transform &msg, Sophus::SE2< Scalar > &out)
Converts a Transform message type to a Sophus SE2 type.
Definition: tf2_sophus.hpp:165
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:261
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:240
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:50