feat(tier4_autoware_utils): add geometry utils (tier4#1257)

* feat(tier4_autoware_utils): add geometry utils

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Add test and remove averagepose

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix typo

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Use EXPECT_NEAR

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp

@@ -38,6 +38,67 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
+// TODO(wep21): Remove these apis
+//              after they are implemented in ros2 geometry2.
+namespace tf2
+{
+inline void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped<tf2::Transform> & out)
+{
+  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
+  out.frame_id_ = msg.header.frame_id;
+  tf2::Transform tmp;
+  fromMsg(msg.pose, tmp);
+  out.setData(tmp);
+}
+#ifdef ROS_DISTRO_GALACTIC
+// Remove after this commit is released
+// https://github.com/ros2/geometry2/commit/e9da371d81e388a589540357c050e262442f1b4a
+inline geometry_msgs::msg::Point & toMsg(const tf2::Vector3 & in, geometry_msgs::msg::Point & out)
+{
+  out.x = in.getX();
+  out.y = in.getY();
+  out.z = in.getZ();
+  return out;
+}
+
+// Remove after this commit is released
+// https://github.com/ros2/geometry2/commit/e9da371d81e388a589540357c050e262442f1b4a
+inline void fromMsg(const geometry_msgs::msg::Point & in, tf2::Vector3 & out)
+{
+  out = tf2::Vector3(in.x, in.y, in.z);
+}
+
+template <>
+inline void doTransform(
+  const geometry_msgs::msg::Point & t_in, geometry_msgs::msg::Point & t_out,
+  const geometry_msgs::msg::TransformStamped & transform)
+{
+  tf2::Transform t;
+  fromMsg(transform.transform, t);
+  tf2::Vector3 v_in;
+  fromMsg(t_in, v_in);
+  tf2::Vector3 v_out = t * v_in;
+  toMsg(v_out, t_out);
+}
+
+template <>
+inline void doTransform(
+  const geometry_msgs::msg::Pose & t_in, geometry_msgs::msg::Pose & t_out,
+  const geometry_msgs::msg::TransformStamped & transform)
+{
+  tf2::Vector3 v;
+  fromMsg(t_in.position, v);
+  tf2::Quaternion r;
+  fromMsg(t_in.orientation, r);
+
+  tf2::Transform t;
+  fromMsg(transform.transform, t);
+  tf2::Transform v_out = t * tf2::Transform(r, v);
+  toMsg(v_out, t_out);
+}
+#endif
+}  // namespace tf2
+
 namespace tier4_autoware_utils
 {
 template <class T>
@@ -339,6 +400,32 @@ inline Point2d transformPoint(
   return Point2d{transformed.x(), transformed.y()};
 }
 
+inline Eigen::Vector3d transformPoint(
+  const Eigen::Vector3d point, const geometry_msgs::msg::Pose pose)
+{
+  geometry_msgs::msg::Transform transform;
+  transform.translation.x = pose.position.x;
+  transform.translation.y = pose.position.y;
+  transform.translation.z = pose.position.z;
+  transform.rotation = pose.orientation;
+
+  Point3d p = transformPoint(Point3d(point.x(), point.y(), point.z()), transform);
+  return Eigen::Vector3d(p.x(), p.y(), p.z());
+}
+
+inline geometry_msgs::msg::Point transformPoint(
+  const geometry_msgs::msg::Point point, const geometry_msgs::msg::Pose pose)
+{
+  const Eigen::Vector3d vec = Eigen::Vector3d(point.x, point.y, point.z);
+  auto transformed_vec = transformPoint(vec, pose);
+
+  geometry_msgs::msg::Point transformed_point;
+  transformed_point.x = transformed_vec.x();
+  transformed_point.y = transformed_vec.y();
+  transformed_point.z = transformed_vec.z();
+  return transformed_point;
+}
+
 template <class T>
 T transformVector(const T & points, const geometry_msgs::msg::Transform & transform)
 {
@@ -349,6 +436,97 @@ T transformVector(const T & points, const geometry_msgs::msg::Transform & transf
   return transformed;
 }
 
+inline geometry_msgs::msg::Pose transformPose(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform)
+{
+  geometry_msgs::msg::Pose transformed_pose;
+  tf2::doTransform(pose, transformed_pose, transform);
+
+  return transformed_pose;
+}
+
+inline geometry_msgs::msg::Pose transformPose(
+  const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Transform & transform)
+{
+  geometry_msgs::msg::TransformStamped transform_stamped;
+  transform_stamped.transform = transform;
+
+  return transformPose(pose, transform_stamped);
+}
+
+inline geometry_msgs::msg::Pose transformPose(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & pose_transform)
+{
+  tf2::Transform transform;
+  tf2::convert(pose_transform, transform);
+
+  geometry_msgs::msg::TransformStamped transform_msg;
+  transform_msg.transform = tf2::toMsg(transform);
+
+  return transformPose(pose, transform_msg);
+}
+
+// Transform pose in world coordinates to local coordinates
+inline geometry_msgs::msg::Pose inverseTransformPose(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform)
+{
+  tf2::Transform tf;
+  tf2::fromMsg(transform, tf);
+  geometry_msgs::msg::TransformStamped transform_stamped;
+  transform_stamped.transform = tf2::toMsg(tf.inverse());
+
+  return transformPose(pose, transform_stamped);
+}
+
+// Transform pose in world coordinates to local coordinates
+inline geometry_msgs::msg::Pose inverseTransformPose(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Transform & transform)
+{
+  tf2::Transform tf;
+  tf2::fromMsg(transform, tf);
+  geometry_msgs::msg::TransformStamped transform_stamped;
+  transform_stamped.transform = tf2::toMsg(tf.inverse());
+
+  return transformPose(pose, transform_stamped);
+}
+
+// Transform pose in world coordinates to local coordinates
+inline geometry_msgs::msg::Pose inverseTransformPose(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & transform_pose)
+{
+  tf2::Transform transform;
+  tf2::convert(transform_pose, transform);
+
+  return inverseTransformPose(pose, tf2::toMsg(transform));
+}
+
+// Transform point in world coordinates to local coordinates
+inline Eigen::Vector3d inverseTransformPoint(
+  const Eigen::Vector3d point, const geometry_msgs::msg::Pose pose)
+{
+  const Eigen::Quaterniond q(
+    pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
+  const Eigen::Matrix3d R = q.normalized().toRotationMatrix();
+
+  const Eigen::Vector3d local_origin(pose.position.x, pose.position.y, pose.position.z);
+  const Eigen::Vector3d local_point = R.transpose() * point - R.transpose() * local_origin;
+
+  return local_point;
+}
+
+// Transform point in world coordinates to local coordinates
+inline geometry_msgs::msg::Point inverseTransformPoint(
+  const geometry_msgs::msg::Point point, const geometry_msgs::msg::Pose pose)
+{
+  const Eigen::Vector3d local_vec =
+    inverseTransformPoint(Eigen::Vector3d(point.x, point.y, point.z), pose);
+  geometry_msgs::msg::Point local_point;
+  local_point.x = local_vec.x();
+  local_point.y = local_vec.y();
+  local_point.z = local_vec.z();
+  return local_point;
+}
+
 inline double calcCurvature(
   const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2,
   const geometry_msgs::msg::Point & p3)

common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp

@@ -735,6 +735,175 @@ TEST(geometry, transformPoint)
     EXPECT_DOUBLE_EQ(p_transformed.y(), 3.5334936490538906);
     EXPECT_DOUBLE_EQ(p_transformed.z(), 5.6160254037844393);
   }
+
+  {
+    const Eigen::Vector3d p(1.0, 2.0, 3.0);
+
+    geometry_msgs::msg::Pose pose_transform;
+    pose_transform.position.x = 1.0;
+    pose_transform.position.y = 2.0;
+    pose_transform.position.z = 3.0;
+    pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30));
+
+    const Eigen::Vector3d p_transformed = transformPoint(p, pose_transform);
+
+    EXPECT_DOUBLE_EQ(p_transformed.x(), 3.1919872981077804);
+    EXPECT_DOUBLE_EQ(p_transformed.y(), 3.5334936490538906);
+    EXPECT_DOUBLE_EQ(p_transformed.z(), 5.6160254037844393);
+  }
+
+  {
+    geometry_msgs::msg::Point p;
+    p.x = 1.0;
+    p.y = 2.0;
+    p.z = 3.0;
+
+    geometry_msgs::msg::Pose pose_transform;
+    pose_transform.position.x = 1.0;
+    pose_transform.position.y = 2.0;
+    pose_transform.position.z = 3.0;
+    pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30));
+
+    const geometry_msgs::msg::Point p_transformed = transformPoint(p, pose_transform);
+
+    EXPECT_DOUBLE_EQ(p_transformed.x, 3.1919872981077804);
+    EXPECT_DOUBLE_EQ(p_transformed.y, 3.5334936490538906);
+    EXPECT_DOUBLE_EQ(p_transformed.z, 5.6160254037844393);
+  }
+}
+
+TEST(geometry, transformPose)
+{
+  using tier4_autoware_utils::createQuaternionFromRPY;
+  using tier4_autoware_utils::deg2rad;
+  using tier4_autoware_utils::transformPose;
+
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = 2.0;
+  pose.position.y = 4.0;
+  pose.position.z = 6.0;
+  pose.orientation = createQuaternionFromRPY(deg2rad(10), deg2rad(20), deg2rad(30));
+
+  // with transform
+  {
+    geometry_msgs::msg::Transform transform;
+    transform.translation.x = 1.0;
+    transform.translation.y = 2.0;
+    transform.translation.z = 3.0;
+    transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30));
+
+    const geometry_msgs::msg::Pose pose_transformed = transformPose(pose, transform);
+
+    EXPECT_NEAR(pose_transformed.position.x, 5.3839745962155598, epsilon);
+    EXPECT_NEAR(pose_transformed.position.y, 5.0669872981077804, epsilon);
+    EXPECT_NEAR(pose_transformed.position.z, 8.2320508075688785, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.x, 0.24304508436548405, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.y, 0.4296803495383052, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.z, 0.40981009820187703, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.w, 0.76704600096616271, epsilon);
+  }
+
+  // with pose_transform
+  {
+    geometry_msgs::msg::Pose pose_transform;
+    pose_transform.position.x = 1.0;
+    pose_transform.position.y = 2.0;
+    pose_transform.position.z = 3.0;
+    pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30));
+
+    const geometry_msgs::msg::Pose pose_transformed = transformPose(pose, pose_transform);
+
+    EXPECT_NEAR(pose_transformed.position.x, 5.3839745962155598, epsilon);
+    EXPECT_NEAR(pose_transformed.position.y, 5.0669872981077804, epsilon);
+    EXPECT_NEAR(pose_transformed.position.z, 8.2320508075688785, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.x, 0.24304508436548405, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.y, 0.4296803495383052, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.z, 0.40981009820187703, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.w, 0.76704600096616271, epsilon);
+  }
+}
+
+TEST(geometry, inverseTransformPose)
+{
+  using tier4_autoware_utils::createQuaternionFromRPY;
+  using tier4_autoware_utils::deg2rad;
+  using tier4_autoware_utils::inverseTransformPose;
+
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = 2.0;
+  pose.position.y = 4.0;
+  pose.position.z = 6.0;
+  pose.orientation = createQuaternionFromRPY(deg2rad(10), deg2rad(20), deg2rad(30));
+
+  // with transform
+  {
+    geometry_msgs::msg::Transform transform;
+    transform.translation.x = 1.0;
+    transform.translation.y = 2.0;
+    transform.translation.z = 3.0;
+    transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30));
+
+    const geometry_msgs::msg::Pose pose_transformed = inverseTransformPose(pose, transform);
+
+    EXPECT_NEAR(pose_transformed.position.x, 0.11602540378443926, epsilon);
+    EXPECT_NEAR(pose_transformed.position.y, 2.8325317547305482, epsilon);
+    EXPECT_NEAR(pose_transformed.position.z, 2.4419872981077804, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.x, -0.17298739392508941, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.y, -0.08189960831908924, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.z, 0.029809019626209146, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.w, 0.98106026219040698, epsilon);
+  }
+
+  // with pose_transform
+  {
+    geometry_msgs::msg::Pose pose_transform;
+    pose_transform.position.x = 1.0;
+    pose_transform.position.y = 2.0;
+    pose_transform.position.z = 3.0;
+    pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30));
+
+    const geometry_msgs::msg::Pose pose_transformed = inverseTransformPose(pose, pose_transform);
+
+    EXPECT_NEAR(pose_transformed.position.x, 0.11602540378443926, epsilon);
+    EXPECT_NEAR(pose_transformed.position.y, 2.8325317547305482, epsilon);
+    EXPECT_NEAR(pose_transformed.position.z, 2.4419872981077804, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.x, -0.17298739392508941, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.y, -0.08189960831908924, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.z, 0.029809019626209146, epsilon);
+    EXPECT_NEAR(pose_transformed.orientation.w, 0.98106026219040698, epsilon);
+  }
+}
+
+TEST(geometry, inverseTransformPoint)
+{
+  using tier4_autoware_utils::createQuaternionFromRPY;
+  using tier4_autoware_utils::deg2rad;
+  using tier4_autoware_utils::inverseTransformPoint;
+  using tier4_autoware_utils::inverseTransformPose;
+
+  geometry_msgs::msg::Pose pose_transform;
+  pose_transform.position.x = 1.0;
+  pose_transform.position.y = 2.0;
+  pose_transform.position.z = 3.0;
+  pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30));
+
+  // calc expected values
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = 2.0;
+  pose.position.y = 4.0;
+  pose.position.z = 6.0;
+  pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0));
+  const geometry_msgs::msg::Pose pose_transformed = inverseTransformPose(pose, pose_transform);
+  const geometry_msgs::msg::Point expected_p = pose_transformed.position;
+
+  geometry_msgs::msg::Point p;
+  p.x = 2.0;
+  p.y = 4.0;
+  p.z = 6.0;
+  const geometry_msgs::msg::Point p_transformed = inverseTransformPoint(p, pose_transform);
+  EXPECT_NEAR(p_transformed.x, expected_p.x, epsilon);
+  EXPECT_NEAR(p_transformed.y, expected_p.y, epsilon);
+  EXPECT_NEAR(p_transformed.z, expected_p.z, epsilon);
 }
 
 TEST(geometry, transformVector)