tf2_sophus.hpp

Go to the documentation of this file.

// Copyright 2022-2023 Ekumen, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef BELUGA_ROS_TF2_SOPHUS_HPP
#define BELUGA_ROS_TF2_SOPHUS_HPP
 
#include <tf2/convert.h>
#include <tf2/utils.h>
 
#if BELUGA_ROS_VERSION == 2
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#elif BELUGA_ROS_VERSION == 1
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#else
#error BELUGA_ROS_VERSION is not defined or invalid
#endif
 
#include <sophus/se2.hpp>
#include <sophus/se3.hpp>
#include <sophus/types.hpp>
 
#include <beluga_ros/messages.hpp>
 
namespace tf2 {
 
 
template <class Scalar>
inline beluga_ros::msg::Pose& toMsg(const Sophus::SE2<Scalar>& in, beluga_ros::msg::Pose& out) {
  const double theta = in.so2().log();
  out.position.x = in.translation().x();
  out.position.y = in.translation().y();
  out.position.z = 0;
  out.orientation.w = std::cos(theta / 2.);
  out.orientation.x = 0;
  out.orientation.y = 0;
  out.orientation.z = std::sin(theta / 2.);
  return out;
}
 
 
template <class Scalar>
inline beluga_ros::msg::Pose& toMsg(const Sophus::SE3<Scalar>& in, beluga_ros::msg::Pose& out) {
  out.position.x = in.translation().x();
  out.position.y = in.translation().y();
  out.position.z = in.translation().z();
  out.orientation.w = in.so3().unit_quaternion().w();
  out.orientation.x = in.so3().unit_quaternion().x();
  out.orientation.y = in.so3().unit_quaternion().y();
  out.orientation.z = in.so3().unit_quaternion().z();
  return out;
}
 
 
template <class Scalar>
inline beluga_ros::msg::Transform toMsg(const Sophus::SE2<Scalar>& in) {
  auto msg = beluga_ros::msg::Transform{};
  const double theta = in.so2().log();
  msg.translation.x = in.translation().x();
  msg.translation.y = in.translation().y();
  msg.translation.z = 0;
  msg.rotation.w = std::cos(theta / 2.);
  msg.rotation.x = 0;
  msg.rotation.y = 0;
  msg.rotation.z = std::sin(theta / 2.);
  return msg;
}
 
 
template <class Scalar>
inline beluga_ros::msg::Transform toMsg(const Sophus::SE3<Scalar>& in) {
  auto msg = beluga_ros::msg::Transform{};
  msg.translation.x = in.translation().x();
  msg.translation.y = in.translation().y();
  msg.translation.z = in.translation().z();
  msg.rotation.w = in.so3().unit_quaternion().w();
  msg.rotation.x = in.so3().unit_quaternion().x();
  msg.rotation.y = in.so3().unit_quaternion().y();
  msg.rotation.z = in.so3().unit_quaternion().z();
  return msg;
}
 
 
template <class Scalar>
inline void fromMsg(const beluga_ros::msg::Transform& msg, Sophus::SE2<Scalar>& out) {
  out.translation() = Sophus::Vector2<Scalar>{
      msg.translation.x,
      msg.translation.y,
  };
  out.so2() = Sophus::SO2<Scalar>{static_cast<Scalar>(tf2::getYaw(msg.rotation))};
}
 
 
template <class Scalar>
inline void fromMsg(const beluga_ros::msg::Transform& msg, Sophus::SE3<Scalar>& out) {
  out.translation() = Sophus::Vector3<Scalar>{
      static_cast<Scalar>(msg.translation.x),
      static_cast<Scalar>(msg.translation.y),
      static_cast<Scalar>(msg.translation.z),
  };
  out.so3() = Sophus::SO3<Scalar>{Eigen::Quaternion<Scalar>{
      static_cast<Scalar>(msg.rotation.w),
      static_cast<Scalar>(msg.rotation.x),
      static_cast<Scalar>(msg.rotation.y),
      static_cast<Scalar>(msg.rotation.z),
  }};
}
 
 
template <class Scalar>
inline void fromMsg(const beluga_ros::msg::Pose& msg, Sophus::SE2<Scalar>& out) {
  out.translation() = Sophus::Vector2<Scalar>{
      static_cast<Scalar>(msg.position.x),
      static_cast<Scalar>(msg.position.y),
  };
  out.so2() = Sophus::SO2<Scalar>{static_cast<Scalar>(tf2::getYaw(msg.orientation))};
}
 
 
template <class Scalar>
inline void fromMsg(const beluga_ros::msg::Pose& msg, Sophus::SE3<Scalar>& out) {
  out.translation() = Sophus::Vector3<Scalar>{
      static_cast<Scalar>(msg.position.x),
      static_cast<Scalar>(msg.position.y),
      static_cast<Scalar>(msg.position.z),
  };
  out.so3() = Sophus::SO3<Scalar>{Eigen::Quaternion<Scalar>{
      static_cast<Scalar>(msg.orientation.w),
      static_cast<Scalar>(msg.orientation.x),
      static_cast<Scalar>(msg.orientation.y),
      static_cast<Scalar>(msg.orientation.z),
  }};
}
 
 
template <template <typename, std::size_t> class Array, typename Scalar>
inline Array<Scalar, 36>& covarianceEigenToRowMajor(const Sophus::Matrix3<Scalar>& in, Array<Scalar, 36>& out) {
  out.fill(0);
  out[0] = in.coeff(0, 0);
  out[1] = in.coeff(0, 1);
  out[5] = in.coeff(0, 2);
  out[6] = in.coeff(1, 0);
  out[7] = in.coeff(1, 1);
  out[11] = in.coeff(1, 2);
  out[30] = in.coeff(2, 0);
  out[31] = in.coeff(2, 1);
  out[35] = in.coeff(2, 2);
  return out;
}
 
 
template <template <typename, std::size_t> class Array, typename Scalar>
inline Sophus::Matrix3<Scalar>& covarianceRowMajorToEigen(const Array<Scalar, 36>& in, Sophus::Matrix3<Scalar>& out) {
  out.coeffRef(0, 0) = in[0];
  out.coeffRef(0, 1) = in[1];
  out.coeffRef(0, 2) = in[5];
  out.coeffRef(1, 0) = in[6];
  out.coeffRef(1, 1) = in[7];
  out.coeffRef(1, 2) = in[11];
  out.coeffRef(2, 0) = in[30];
  out.coeffRef(2, 1) = in[31];
  out.coeffRef(2, 2) = in[35];
  return out;
}
 
}  // namespace tf2
 
namespace Sophus {  // NOLINT(readability-identifier-naming)
 
template <class Scalar>
inline beluga_ros::msg::Transform toMsg(const Sophus::SE2<Scalar>& pose) {
  return tf2::toMsg(pose);
}
 
template <class Scalar>
inline beluga_ros::msg::Transform toMsg(const Sophus::SE3<Scalar>& in) {
  return tf2::toMsg(in);
}
 
template <class Scalar>
inline void fromMsg(const beluga_ros::msg::Transform& message, Sophus::SE2<Scalar>& pose) {
  tf2::fromMsg(message, pose);
}
 
template <class Scalar>
inline void fromMsg(const beluga_ros::msg::Transform& message, Sophus::SE3<Scalar>& pose) {
  tf2::fromMsg(message, pose);
}
 
template <class Scalar>
inline void fromMsg(const beluga_ros::msg::Pose& message, Sophus::SE2<Scalar>& pose) {
  tf2::fromMsg(message, pose);
}
 
template <class Scalar>
inline void fromMsg(const beluga_ros::msg::Pose& message, Sophus::SE3<Scalar>& pose) {
  tf2::fromMsg(message, pose);
}
 
}  // namespace Sophus
 
#endif  // BELUGA_ROS_TF2_SOPHUS_HPP