/home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/tf2_sophus.hpp Source File

/home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/tf2_sophus.hpp Source File#

Beluga ROS: /home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/tf2_sophus.hpp Source File
Beluga ROS
tf2_sophus.hpp
Go to the documentation of this file.
1 // Copyright 2022-2023 Ekumen, Inc.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef BELUGA_ROS_TF2_SOPHUS_HPP
16 #define BELUGA_ROS_TF2_SOPHUS_HPP
17 
18 #include <beluga/algorithm/unscented_transform.hpp>
19 #include <beluga/eigen_compatibility.hpp>
20 #include <cmath>
21 #include <sophus/common.hpp>
22 #include <tf2/convert.hpp>
23 #include <tf2/utils.hpp>
24 
25 #include <tf2_eigen/tf2_eigen.hpp>
26 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
27 
28 #include <sophus/se2.hpp>
29 #include <sophus/se3.hpp>
30 #include <sophus/types.hpp>
31 
32 #include <geometry_msgs/msg/pose.hpp>
33 #include <geometry_msgs/msg/pose_with_covariance.hpp>
34 #include <geometry_msgs/msg/transform.hpp>
35 
41 namespace detail {
44 template <typename Scalar>
45 inline Eigen::Vector<Scalar, 6> tangential_space_to_xyz_rpy(const Eigen::Vector<Scalar, 6>& tangent) {
46  // ROS covariances use extrinsic RPY parametrization, we need to convert to it from our tangent space
47  // representation. We use an unscented transform to apply such transform as it's non linear.
48  // See https://www.ros.org/reps/rep-0103.html#covariance-representation .
49  const auto se3 = Sophus::SE3<Scalar>::exp(tangent);
50  // Eigen's eulerAngles uses intrinsic rotations, but XYZ extrinsic rotation is the same as ZYX intrinsic rotation.
51  // See https://pages.github.berkeley.edu/EECS-106/fa21-site/assets/discussions/D1_Rotations_soln.pdf
52  // This gives (extrinsic) yaw, pitch, roll in that order.
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();
59  return ret;
60 }
61 } // namespace detail
62 
64 namespace tf2 {
65 
67 
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();
81  out.position.z = 0;
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.);
86  return out;
87 }
88 
90 
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();
108  return out;
109 }
110 
112 
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.);
126  msg.rotation.x = 0;
127  msg.rotation.y = 0;
128  msg.rotation.z = std::sin(theta / 2.);
129  return msg;
130 }
131 
133 
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();
149  return msg;
150 }
151 
153 
159 template <class Scalar>
160 inline void fromMsg(const geometry_msgs::msg::Transform& msg, Sophus::SE2<Scalar>& out) {
161  out.translation() = Sophus::Vector2<Scalar>{
162  msg.translation.x,
163  msg.translation.y,
164  };
165  out.so2() = Sophus::SO2<Scalar>{static_cast<Scalar>(tf2::getYaw(msg.rotation))};
166 }
167 
169 
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),
181  };
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),
187  }};
188 }
189 
191 
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),
202  };
203  out.so2() = Sophus::SO2<Scalar>{static_cast<Scalar>(tf2::getYaw(msg.orientation))};
204 }
205 
207 
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),
219  };
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),
225  }};
226 }
227 
229 
234 template <template <typename, std::size_t> class Array, typename Scalar>
235 inline Array<Scalar, 36>& covarianceEigenToRowMajor(const Sophus::Matrix3<Scalar>& in, Array<Scalar, 36>& out) {
236  out.fill(0);
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);
246  return out;
247 }
248 
250 
255 template <template <typename, std::size_t> class Array, typename Scalar>
256 inline Sophus::Matrix3<Scalar>& covarianceRowMajorToEigen(const Array<Scalar, 36>& in, Sophus::Matrix3<Scalar>& out) {
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];
266  return out;
267 }
268 
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;
277  tf2::toMsg(in, out.pose);
278 
279  // ROS covariances use extrinsic RPY parametrization, we need to convert to it from our tangent space
280  // representation. We use an unscented transform to apply such transform as it's non linear.
281  // See https://www.ros.org/reps/rep-0103.html#covariance-representation .
282 
283  const auto& [base_pose_in_map_xyz_rpy, base_pose_covariance_xyz_rpy] = beluga::unscented_transform(
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();
290 
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;
299  }
300 
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());
304  return out;
305  },
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]));
311 
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]));
315 
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();
320  return out;
321  });
322 
323  // ROS covariance elements type is always double, and they're in RowMajor order.
324  Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>>(out.covariance.data()) =
325  base_pose_covariance_xyz_rpy.template cast<double>();
326  return out;
327 }
328 
329 } // namespace tf2
330 
332 namespace Sophus { // NOLINT(readability-identifier-naming)
333 
335 template <class Scalar>
336 inline geometry_msgs::msg::Transform toMsg(const Sophus::SE2<Scalar>& pose) {
337  return tf2::toMsg(pose);
338 }
339 
341 template <class Scalar>
342 inline geometry_msgs::msg::Transform toMsg(const Sophus::SE3<Scalar>& in) {
343  return tf2::toMsg(in);
344 }
345 
347 template <class Scalar>
348 inline void fromMsg(const geometry_msgs::msg::Transform& message, Sophus::SE2<Scalar>& pose) {
349  tf2::fromMsg(message, pose);
350 }
351 
353 template <class Scalar>
354 inline void fromMsg(const geometry_msgs::msg::Transform& message, Sophus::SE3<Scalar>& pose) {
355  tf2::fromMsg(message, pose);
356 }
357 
359 template <class Scalar>
360 inline void fromMsg(const geometry_msgs::msg::Pose& message, Sophus::SE2<Scalar>& pose) {
361  tf2::fromMsg(message, pose);
362 }
363 
365 template <class Scalar>
366 inline void fromMsg(const geometry_msgs::msg::Pose& message, Sophus::SE3<Scalar>& pose) {
367  tf2::fromMsg(message, pose);
368 }
369 
370 } // namespace Sophus
371 
372 #endif // BELUGA_ROS_TF2_SOPHUS_HPP
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 auto weights
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