Add sophus conversion utilities

beluga_amcl/include/beluga_amcl/tf2_sophus.hpp

@@ -0,0 +1,211 @@
+// Copyright 2022 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_AMCL__TF2_SOPHUS_HPP_
+#define BELUGA_AMCL__TF2_SOPHUS_HPP_
+
+#include <tf2/convert.h>
+#include <tf2/utils.h>
+
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/transform.hpp>
+#include <sophus/se2.hpp>
+#include <sophus/se3.hpp>
+
+namespace tf2
+{
+
+/**
+ * @brief Convert a Sophus SE2 type to a Pose message.
+ * This function is a specialization of the toMsg template defined in tf2/convert.h.
+ * @param in The Sophus SE2 to convert.
+ * @param out The pose converted to a Pose message.
+ * @return The pose converted to a Pose message.
+ */
+template<class Scalar>
+inline geometry_msgs::msg::Pose & toMsg(
+  const Sophus::SE2<Scalar> & in,
+  geometry_msgs::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;
+}
+
+/**
+ * @brief Convert a Sophus SE3 type to a Pose message.
+ * This function is a specialization of the toMsg template defined in tf2/convert.h.
+ * @param in The Sophus SE3 to convert.
+ * @param out The pose converted to a Pose message.
+ * @return The pose converted to a Pose message.
+ */
+template<class Scalar>
+inline geometry_msgs::msg::Pose & toMsg(
+  const Sophus::SE3<Scalar> & in,
+  geometry_msgs::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;
+}
+
+/**
+ * @brief Convert a Sophus SE2 type to a Transform message.
+ * This function is a specialization of the toMsg template defined in tf2/convert.h.
+ * @param in The Sophus SE2 to convert.
+ * @return The transform converted to a Transform message.
+ */
+template<class Scalar>
+inline geometry_msgs::msg::Transform toMsg(const Sophus::SE2<Scalar> & in)
+{
+  auto msg = geometry_msgs::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;
+}
+
+/**
+ * @brief Convert a Sophus SE3 type to a Transform message.
+ * This function is a specialization of the toMsg template defined in tf2/convert.h.
+ * @param in The Sophus SE3 to convert.
+ * @return The transform converted to a Transform message.
+ */
+template<class Scalar>
+inline geometry_msgs::msg::Transform toMsg(const Sophus::SE3<Scalar> & in)
+{
+  auto msg = geometry_msgs::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;
+}
+
+/**
+ * @brief Convert a Transform message type to a Sophus-specific SE2 type.
+ * This function is a specialization of the fromMsg template defined in tf2/convert.h
+ * @param msg The Transform message to convert.
+ * @param out The transform converted to a Sophus SE2.
+ */
+template<class Scalar>
+inline void fromMsg(const geometry_msgs::msg::Transform & msg, Sophus::SE2<Scalar> & out)
+{
+  out.translation() = Eigen::Vector2<Scalar>{
+    msg.translation.x,
+    msg.translation.y,
+  };
+  out.so2() = Sophus::SO2<Scalar>{tf2::getYaw(msg.rotation)};
+}
+
+/**
+ * @brief Convert a Transform message type to a Sophus-specific SE3 type.
+ * This function is a specialization of the fromMsg template defined in tf2/convert.h
+ * @param msg The Transform message to convert.
+ * @param out The transform converted to a Sophus SE3.
+ */
+template<class Scalar>
+inline void fromMsg(const geometry_msgs::msg::Transform & msg, Sophus::SE3<Scalar> & out)
+{
+  out.translation() = Eigen::Vector3<Scalar>{
+    msg.translation.x,
+    msg.translation.y,
+    msg.translation.z,
+  };
+  out.so3() = Sophus::SO3<Scalar>{
+    Eigen::Quaternion<Scalar>{
+      msg.rotation.w,
+      msg.rotation.x,
+      msg.rotation.y,
+      msg.rotation.z,
+    }
+  };
+}
+
+/**
+ * @brief Function that converts from an Eigen 3x3 matrix representation of a 2D
+ * covariance matrix to a 6x6 row-major representation in 3D.
+ * @param in An Eigen 3x3 matrix representation of a 2D covariance.
+ * @return A row-major array of 36 covariance values.
+ */
+template<class Scalar>
+inline std::array<Scalar, 36> covarianceToRowMajor(const Eigen::Matrix3<Scalar> & in)
+{
+  auto covariance = std::array<Scalar, 36>{};
+  covariance.fill(0);
+  covariance[0] = in.coeff(0, 0);
+  covariance[1] = in.coeff(0, 1);
+  covariance[5] = in.coeff(0, 2);
+  covariance[6] = in.coeff(1, 0);
+  covariance[7] = in.coeff(1, 1);
+  covariance[11] = in.coeff(1, 2);
+  covariance[30] = in.coeff(2, 0);
+  covariance[31] = in.coeff(2, 1);
+  covariance[35] = in.coeff(2, 2);
+  return covariance;
+}
+
+}  // namespace tf2
+
+namespace Sophus
+{
+// The following conversion functions need to be inside the Sophus namespace to make
+// them findable by ADL when calling tf2::convert().
+
+template<class Scalar>
+inline geometry_msgs::msg::Transform toMsg(const Sophus::SE2<Scalar> & in)
+{
+  return tf2::toMsg(in);
+}
+
+template<class Scalar>
+inline geometry_msgs::msg::Transform toMsg(const Sophus::SE3<Scalar> & in)
+{
+  return tf2::toMsg(in);
+}
+
+template<class Scalar>
+inline void fromMsg(const geometry_msgs::msg::Transform & msg, Sophus::SE2<Scalar> & out)
+{
+  tf2::fromMsg(msg, out);
+}
+
+template<class Scalar>
+inline void fromMsg(const geometry_msgs::msg::Transform & msg, Sophus::SE3<Scalar> & out)
+{
+  tf2::fromMsg(msg, out);
+}
+
+}  // namespace Sophus
+
+#endif  // BELUGA_AMCL__TF2_SOPHUS_HPP_

beluga_amcl/src/amcl_node.cpp

@@ -22,7 +22,7 @@
 #include <limits>
 #include <memory>
 
-#include <beluga_amcl/convert.hpp>
+#include <beluga_amcl/tf2_sophus.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <lifecycle_msgs/msg/state.hpp>
 #include <range/v3/range/conversion.hpp>
@@ -32,6 +32,36 @@
 namespace beluga_amcl
 {
 
+namespace
+{
+
+std::vector<std::pair<double, double>> pre_process_points(
+  const sensor_msgs::msg::LaserScan & laser_scan,
+  const Sophus::SE3d & laser_transform,
+  double range_min,
+  double range_max)
+{
+  auto points = std::vector<std::pair<double, double>>{};
+  points.reserve(laser_scan.ranges.size());
+  for (std::size_t index = 0; index < laser_scan.ranges.size(); ++index) {
+    const float range = laser_scan.ranges[index];
+    if (std::isnan(range) || range <= range_min || range >= range_max) {
+      continue;
+    }
+    // Store points in the robot's reference frame
+    const float angle = laser_scan.angle_min +
+      static_cast<float>(index) * laser_scan.angle_increment;
+    const auto point = laser_transform * Eigen::Vector3d{
+      range * std::cos(angle),
+      range * std::sin(angle),
+      0.0};
+    points.emplace_back(point.x(), point.y());
+  }
+  return points;
+}
+
+}  // namespace
+
 AmclNode::AmclNode(const rclcpp::NodeOptions & options)
 : rclcpp_lifecycle::LifecycleNode{"amcl", "", options}
 {
@@ -477,7 +507,7 @@ void AmclNode::timer_callback()
     ranges::transform(
       particle_filter_->particles(), std::begin(message.particles), [](const auto & particle) {
         auto message = nav2_msgs::msg::Particle{};
-        tf2::convert(beluga::state(particle), message.pose);
+        tf2::toMsg(beluga::state(particle), message.pose);
         message.weight = beluga::weight(particle);
         return message;
       });
@@ -489,12 +519,8 @@ void AmclNode::timer_callback()
     auto message = geometry_msgs::msg::PoseWithCovarianceStamped{};
     message.header.stamp = now();
     message.header.frame_id = get_parameter("global_frame_id").as_string();
-    tf2::convert(pose, message.pose.pose);
-    message.pose.covariance[0] = covariance.coeff(0, 0);
-    message.pose.covariance[1] = covariance.coeff(0, 1);
-    message.pose.covariance[6] = covariance.coeff(1, 0);
-    message.pose.covariance[7] = covariance.coeff(1, 1);
-    message.pose.covariance[35] = covariance.coeff(2, 2);
+    tf2::toMsg(pose, message.pose.pose);
+    message.pose.covariance = tf2::covarianceToRowMajor(covariance);
     pose_pub_->publish(message);
   }
 
@@ -506,10 +532,7 @@ void AmclNode::timer_callback()
       tf2::durationFromSec(get_parameter("transform_tolerance").as_double());
     message.header.frame_id = get_parameter("global_frame_id").as_string();
     message.child_frame_id = get_parameter("odom_frame_id").as_string();
-    message.transform.rotation.x = 0;
-    message.transform.rotation.y = 0;
-    message.transform.rotation.z = 0;
-    message.transform.rotation.w = 1;
+    message.transform = tf2::toMsg(Sophus::SE2d{});
     tf_broadcaster_->sendTransform(message);
   }
 }
@@ -550,39 +573,27 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_
     }
 
     {
-      auto base_to_laser_transform = tf2::Transform{};
+      auto base_to_laser_transform = Sophus::SE3d{};
       tf2::convert(
         tf_buffer_->lookupTransform(
           get_parameter("base_frame_id").as_string(),
           laser_scan->header.frame_id,
           laser_scan->header.stamp,
           std::chrono::seconds(1)).transform,
         base_to_laser_transform);
-      const float range_min =
-        std::max(
+
+      const float range_min = std::max(
         laser_scan->range_min,
         static_cast<float>(get_parameter("laser_min_range").as_double()));
-      const float range_max =
-        std::min(
+      const float range_max = std::min(
         laser_scan->range_max,
         static_cast<float>(get_parameter("laser_max_range").as_double()));
-      auto points = std::vector<std::pair<double, double>>{};
-      points.reserve(laser_scan->ranges.size());
-      for (std::size_t index = 0; index < laser_scan->ranges.size(); ++index) {
-        const float range = laser_scan->ranges[index];
-        if (std::isnan(range) || range < range_min || range > range_max) {
-          continue;
-        }
-        // Store points in the robot's reference frame
-        const float angle = laser_scan->angle_min +
-          static_cast<float>(index) * laser_scan->angle_increment;
-        const auto point = base_to_laser_transform * tf2::Vector3{
-          range * std::cos(angle),
-          range * std::sin(angle),
-          0.0};
-        points.emplace_back(point.x(), point.y());
-      }
-      particle_filter_->update_sensor(std::move(points));
+      particle_filter_->update_sensor(
+        pre_process_points(
+          *laser_scan,
+          base_to_laser_transform,
+          range_min,
+          range_max));
     }
 
     const auto time1 = std::chrono::high_resolution_clock::now();