/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 <tf2/convert.h>
19 #include <tf2/utils.h>
20 #include <beluga/algorithm/unscented_transform.hpp>
21 #include <beluga/eigen_compatibility.hpp>
22 #include <cmath>
23 #include <sophus/common.hpp>
24 
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>
31 #else
32 #error BELUGA_ROS_VERSION is not defined or invalid
33 #endif
34 
35 #include <sophus/se2.hpp>
36 #include <sophus/se3.hpp>
37 #include <sophus/types.hpp>
38 
39 #include <beluga_ros/messages.hpp>
40 
46 namespace detail {
49 template <typename Scalar>
50 inline Eigen::Vector<Scalar, 6> tangential_space_to_xyz_rpy(const Eigen::Vector<Scalar, 6>& tangent) {
51  // ROS covariances use extrinsic RPY parametrization, we need to convert to it from our tangent space
52  // representation. We use an unscented transform to apply such transform as it's non linear.
53  // See https://www.ros.org/reps/rep-0103.html#covariance-representation .
54  const auto se3 = Sophus::SE3<Scalar>::exp(tangent);
55  // Eigen's eulerAngles uses intrinsic rotations, but XYZ extrinsic rotation is the same as ZYX intrinsic rotation.
56  // See https://pages.github.berkeley.edu/EECS-106/fa21-site/assets/discussions/D1_Rotations_soln.pdf
57  // This gives (extrinsic) yaw, pitch, roll in that order.
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();
64  return ret;
65 }
66 } // namespace detail
67 
69 namespace tf2 {
70 
72 
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();
86  out.position.z = 0;
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.);
91  return out;
92 }
93 
95 
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();
113  return out;
114 }
115 
117 
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.);
131  msg.rotation.x = 0;
132  msg.rotation.y = 0;
133  msg.rotation.z = std::sin(theta / 2.);
134  return msg;
135 }
136 
138 
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();
154  return msg;
155 }
156 
158 
164 template <class Scalar>
165 inline void fromMsg(const beluga_ros::msg::Transform& msg, Sophus::SE2<Scalar>& out) {
166  out.translation() = Sophus::Vector2<Scalar>{
167  msg.translation.x,
168  msg.translation.y,
169  };
170  out.so2() = Sophus::SO2<Scalar>{static_cast<Scalar>(tf2::getYaw(msg.rotation))};
171 }
172 
174 
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),
186  };
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),
192  }};
193 }
194 
196 
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),
207  };
208  out.so2() = Sophus::SO2<Scalar>{static_cast<Scalar>(tf2::getYaw(msg.orientation))};
209 }
210 
212 
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),
224  };
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),
230  }};
231 }
232 
234 
239 template <template <typename, std::size_t> class Array, typename Scalar>
240 inline Array<Scalar, 36>& covarianceEigenToRowMajor(const Sophus::Matrix3<Scalar>& in, Array<Scalar, 36>& out) {
241  out.fill(0);
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);
251  return out;
252 }
253 
255 
260 template <template <typename, std::size_t> class Array, typename Scalar>
261 inline Sophus::Matrix3<Scalar>& covarianceRowMajorToEigen(const Array<Scalar, 36>& in, Sophus::Matrix3<Scalar>& out) {
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];
271  return out;
272 }
273 
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;
282  tf2::toMsg(in, out.pose);
283 
284  // ROS covariances use extrinsic RPY parametrization, we need to convert to it from our tangent space
285  // representation. We use an unscented transform to apply such transform as it's non linear.
286  // See https://www.ros.org/reps/rep-0103.html#covariance-representation .
287 
288  const auto& [base_pose_in_map_xyz_rpy, base_pose_covariance_xyz_rpy] = beluga::unscented_transform(
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();
295 
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;
304  }
305 
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());
309  return out;
310  },
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]));
316 
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]));
320 
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();
325  return out;
326  });
327 
328  // ROS covariance elements type is always double, and they're in RowMajor order.
329  Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>>(out.covariance.data()) =
330  base_pose_covariance_xyz_rpy.template cast<double>();
331  return out;
332 }
333 
334 } // namespace tf2
335 
337 namespace Sophus { // NOLINT(readability-identifier-naming)
338 
340 template <class Scalar>
341 inline beluga_ros::msg::Transform toMsg(const Sophus::SE2<Scalar>& pose) {
342  return tf2::toMsg(pose);
343 }
344 
346 template <class Scalar>
347 inline beluga_ros::msg::Transform toMsg(const Sophus::SE3<Scalar>& in) {
348  return tf2::toMsg(in);
349 }
350 
352 template <class Scalar>
353 inline void fromMsg(const beluga_ros::msg::Transform& message, Sophus::SE2<Scalar>& pose) {
354  tf2::fromMsg(message, pose);
355 }
356 
358 template <class Scalar>
359 inline void fromMsg(const beluga_ros::msg::Transform& message, Sophus::SE3<Scalar>& pose) {
360  tf2::fromMsg(message, pose);
361 }
362 
364 template <class Scalar>
365 inline void fromMsg(const beluga_ros::msg::Pose& message, Sophus::SE2<Scalar>& pose) {
366  tf2::fromMsg(message, pose);
367 }
368 
370 template <class Scalar>
371 inline void fromMsg(const beluga_ros::msg::Pose& message, Sophus::SE3<Scalar>& pose) {
372  tf2::fromMsg(message, pose);
373 }
374 
375 } // namespace Sophus
376 
377 #endif // BELUGA_ROS_TF2_SOPHUS_HPP
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 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:50