diff --git a/autoware_utils_geometry/CMakeLists.txt b/autoware_utils_geometry/CMakeLists.txt
index 866c964..4e96799 100644
--- a/autoware_utils_geometry/CMakeLists.txt
+++ b/autoware_utils_geometry/CMakeLists.txt
@@ -19,6 +19,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
if(BUILD_TESTING)
file(GLOB_RECURSE test_files test/*.cpp)
+
ament_auto_add_gtest(test_${PROJECT_NAME} ${test_files})
endif()
diff --git a/autoware_utils_geometry/package.xml b/autoware_utils_geometry/package.xml
index 019e45a..48eda94 100644
--- a/autoware_utils_geometry/package.xml
+++ b/autoware_utils_geometry/package.xml
@@ -20,9 +20,9 @@
tf2
tf2_eigen
tf2_geometry_msgs
-
ament_lint_auto
autoware_lint_common
+ autoware_utils_system
ament_cmake
diff --git a/autoware_utils_geometry/test/cases/alt_geometry.cpp b/autoware_utils_geometry/test/cases/alt_geometry.cpp
new file mode 100644
index 0000000..c2c78bc
--- /dev/null
+++ b/autoware_utils_geometry/test/cases/alt_geometry.cpp
@@ -0,0 +1,1145 @@
+// Copyright 2020-2024 Tier IV, 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.
+
+#include "autoware_utils_geometry/alt_geometry.hpp"
+
+#include "autoware_utils_geometry/random_convex_polygon.hpp"
+#include "autoware_utils_system/stop_watch.hpp"
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+constexpr double epsilon = 1e-6;
+
+TEST(alt_geometry, area)
+{
+ using autoware_utils_geometry::area;
+ using autoware_utils_geometry::alt::ConvexPolygon2d;
+ using autoware_utils_geometry::alt::Point2d;
+
+ {
+ const Point2d p1 = {0.0, 0.0};
+ const Point2d p2 = {0.0, 7.0};
+ const Point2d p3 = {4.0, 2.0};
+ const Point2d p4 = {2.0, 0.0};
+ const auto result = area(ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+ EXPECT_NEAR(result, 16.0, epsilon);
+ }
+}
+
+TEST(alt_geometry, convexHull)
+{
+ using autoware_utils_geometry::convex_hull;
+ using autoware_utils_geometry::alt::PointList2d;
+ using autoware_utils_geometry::alt::Points2d;
+
+ {
+ Points2d points;
+ points.push_back({2.0, 1.3});
+ points.push_back({2.4, 1.7});
+ points.push_back({2.8, 1.8});
+ points.push_back({3.4, 1.2});
+ points.push_back({3.7, 1.6});
+ points.push_back({3.4, 2.0});
+ points.push_back({4.1, 3.0});
+ points.push_back({5.3, 2.6});
+ points.push_back({5.4, 1.2});
+ points.push_back({4.9, 0.8});
+ points.push_back({2.9, 0.7});
+ const auto result = convex_hull(points);
+
+ ASSERT_TRUE(result);
+ PointList2d ground_truth = {{2.0, 1.3}, {2.4, 1.7}, {4.1, 3.0}, {5.3, 2.6},
+ {5.4, 1.2}, {4.9, 0.8}, {2.9, 0.7}, {2.0, 1.3}};
+ ASSERT_EQ(result->vertices().size(), ground_truth.size());
+ auto alt_it = result->vertices().begin();
+ auto ground_truth_it = ground_truth.begin();
+ for (; alt_it != result->vertices().end(); ++alt_it, ++ground_truth_it) {
+ EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon);
+ EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon);
+ }
+ }
+}
+
+TEST(alt_geometry, correct)
+{
+ using autoware_utils_geometry::correct;
+ using autoware_utils_geometry::alt::ConvexPolygon2d;
+ using autoware_utils_geometry::alt::PointList2d;
+
+ { // Correctly oriented
+ PointList2d vertices;
+ vertices.push_back({1.0, 1.0});
+ vertices.push_back({1.0, -1.0});
+ vertices.push_back({-1.0, -1.0});
+ vertices.push_back({-1.0, 1.0});
+ auto poly = ConvexPolygon2d::create(vertices).value(); // correct()-ed in create()
+
+ PointList2d ground_truth = {{1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}};
+ ASSERT_EQ(poly.vertices().size(), ground_truth.size());
+ auto alt_it = poly.vertices().begin();
+ auto ground_truth_it = ground_truth.begin();
+ for (; alt_it != poly.vertices().end(); ++alt_it, ++ground_truth_it) {
+ EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon);
+ EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon);
+ }
+ }
+
+ { // Wrongly oriented
+ PointList2d vertices;
+ vertices.push_back({1.0, 1.0});
+ vertices.push_back({-1.0, 1.0});
+ vertices.push_back({-1.0, -1.0});
+ vertices.push_back({1.0, -1.0});
+ auto poly = ConvexPolygon2d::create(vertices).value(); // correct()-ed in create()
+
+ PointList2d ground_truth = {{1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}};
+ ASSERT_EQ(poly.vertices().size(), ground_truth.size());
+ auto alt_it = poly.vertices().begin();
+ auto ground_truth_it = ground_truth.begin();
+ for (; alt_it != poly.vertices().end(); ++alt_it, ++ground_truth_it) {
+ EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon);
+ EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon);
+ }
+ }
+}
+
+TEST(alt_geometry, coveredBy)
+{
+ using autoware_utils_geometry::covered_by;
+ using autoware_utils_geometry::alt::ConvexPolygon2d;
+ using autoware_utils_geometry::alt::Point2d;
+
+ { // The point is within the polygon
+ const Point2d point = {0.0, 0.0};
+ const Point2d p1 = {1.0, 1.0};
+ const Point2d p2 = {1.0, -1.0};
+ const Point2d p3 = {-1.0, -1.0};
+ const Point2d p4 = {-1.0, 1.0};
+ const auto result = covered_by(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+ EXPECT_TRUE(result);
+ }
+
+ { // The point is outside the polygon
+ const Point2d point = {0.0, 0.0};
+ const Point2d p1 = {2.0, 2.0};
+ const Point2d p2 = {2.0, 1.0};
+ const Point2d p3 = {1.0, 1.0};
+ const Point2d p4 = {1.0, 2.0};
+ const auto result = covered_by(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+ EXPECT_FALSE(result);
+ }
+
+ { // The point is on the edge of the polygon
+ const Point2d point = {0.0, 0.0};
+ const Point2d p1 = {2.0, 1.0};
+ const Point2d p2 = {2.0, -1.0};
+ const Point2d p3 = {0.0, -1.0};
+ const Point2d p4 = {0.0, 1.0};
+ const auto result = covered_by(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+ EXPECT_TRUE(result);
+ }
+}
+
+TEST(alt_geometry, disjoint)
+{
+ using autoware_utils_geometry::disjoint;
+ using autoware_utils_geometry::alt::ConvexPolygon2d;
+ using autoware_utils_geometry::alt::Point2d;
+
+ { // Disjoint
+ const Point2d p1 = {0.0, 2.0};
+ const Point2d p2 = {-2.0, 0.0};
+ const Point2d p3 = {-4.0, 2.0};
+ const Point2d p4 = {-2.0, 4.0};
+ const Point2d p5 = {2.0, 2.0};
+ const Point2d p6 = {4.0, 4.0};
+ const Point2d p7 = {6.0, 2.0};
+ const Point2d p8 = {4.0, 0.0};
+ const auto result = disjoint(
+ ConvexPolygon2d::create({p1, p2, p3, p4}).value(),
+ ConvexPolygon2d::create({p5, p6, p7, p8}).value());
+
+ EXPECT_TRUE(result);
+ }
+
+ { // Not disjoint (two polygons share a vertex)
+ const Point2d p1 = {0.0, 2.0};
+ const Point2d p2 = {-2.0, 0.0};
+ const Point2d p3 = {-4.0, 2.0};
+ const Point2d p4 = {-2.0, 4.0};
+ const Point2d p5 = {0.0, 2.0};
+ const Point2d p6 = {2.0, 4.0};
+ const Point2d p7 = {4.0, 2.0};
+ const Point2d p8 = {2.0, 0.0};
+ const auto result = disjoint(
+ ConvexPolygon2d::create({p1, p2, p3, p4}).value(),
+ ConvexPolygon2d::create({p5, p6, p7, p8}).value());
+
+ EXPECT_FALSE(result);
+ }
+}
+
+TEST(alt_geometry, distance)
+{
+ using autoware_utils_geometry::distance;
+ using autoware_utils_geometry::alt::ConvexPolygon2d;
+ using autoware_utils_geometry::alt::Point2d;
+
+ { // Normal setting
+ const Point2d p = {0.0, 1.0};
+ const Point2d p1 = {-1.0, 0.0};
+ const Point2d p2 = {1.0, 0.0};
+ const auto result = distance(p, p1, p2);
+
+ EXPECT_NEAR(result, 1.0, epsilon);
+ }
+
+ {
+ // The point is out of range of the segment to the start point side
+ const Point2d p = {-2.0, 1.0};
+ const Point2d p1 = {-1.0, 0.0};
+ const Point2d p2 = {1.0, 0.0};
+ const auto result = distance(p, p1, p2);
+
+ EXPECT_NEAR(result, std::sqrt(2), epsilon);
+ }
+
+ {
+ // The point is out of range of the segment to the end point side
+ const Point2d p = {2.0, 1.0};
+ const Point2d p1 = {-1.0, 0.0};
+ const Point2d p2 = {1.0, 0.0};
+ const auto result = distance(p, p1, p2);
+
+ EXPECT_NEAR(result, std::sqrt(2), epsilon);
+ }
+
+ {
+ // The point is on the segment
+ const Point2d p = {0.0, 0.0};
+ const Point2d p1 = {-1.0, 0.0};
+ const Point2d p2 = {1.0, 0.0};
+ const auto result = distance(p, p1, p2);
+
+ EXPECT_NEAR(result, 0.0, epsilon);
+ }
+
+ {
+ // The point is the start point of the segment
+ const Point2d p = {-1.0, 0.0};
+ const Point2d p1 = {-1.0, 0.0};
+ const Point2d p2 = {1.0, 0.0};
+ const auto result = distance(p, p1, p2);
+
+ EXPECT_NEAR(result, 0.0, epsilon);
+ }
+
+ {
+ // The point is the end point of the segment
+ const Point2d p = {1.0, 0.0};
+ const Point2d p1 = {-1.0, 0.0};
+ const Point2d p2 = {1.0, 0.0};
+ const auto result = distance(p, p1, p2);
+
+ EXPECT_NEAR(result, 0.0, epsilon);
+ }
+
+ { // The segment is a point
+ const Point2d p = {0.0, 0.0};
+ const Point2d p1 = {1.0, 0.0};
+ const Point2d p2 = {1.0, 0.0};
+ const auto result = distance(p, p1, p2);
+
+ EXPECT_NEAR(result, 1.0, epsilon);
+ }
+
+ { // The point is outside the polygon
+ const Point2d p = {0.0, 0.0};
+ const Point2d p1 = {3.0, 1.0};
+ const Point2d p2 = {3.0, -1.0};
+ const Point2d p3 = {1.0, -1.0};
+ const Point2d p4 = {1.0, 1.0};
+ const auto result = distance(p, ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+ EXPECT_NEAR(result, 1.0, epsilon);
+ }
+
+ { // The point is within the polygon
+ const Point2d p = {0.0, 0.0};
+ const Point2d p1 = {2.0, 1.0};
+ const Point2d p2 = {2.0, -1.0};
+ const Point2d p3 = {-1.0, -1.0};
+ const Point2d p4 = {-1.0, 1.0};
+ const auto result = distance(p, ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+ EXPECT_NEAR(result, 0.0, epsilon);
+ }
+}
+
+TEST(geometry, envelope)
+{
+ using autoware_utils_geometry::envelope;
+ using autoware_utils_geometry::alt::PointList2d;
+ using autoware_utils_geometry::alt::Polygon2d;
+
+ {
+ const auto poly = Polygon2d::create(
+ PointList2d{
+ {2.0, 1.3},
+ {2.4, 1.7},
+ {2.8, 1.8},
+ {3.4, 1.2},
+ {3.7, 1.6},
+ {3.4, 2.0},
+ {4.1, 3.0},
+ {5.3, 2.6},
+ {5.4, 1.2},
+ {4.9, 0.8},
+ {2.9, 0.7},
+ {2.0, 1.3}},
+ {PointList2d{{4.0, 2.0}, {4.2, 1.4}, {4.8, 1.9}, {4.4, 2.2}, {4.0, 2.0}}})
+ .value();
+ const auto result = envelope(poly);
+
+ ASSERT_TRUE(result);
+
+ PointList2d ground_truth = {{2.0, 0.7}, {2.0, 3.0}, {5.4, 3.0}, {5.4, 0.7}, {2.0, 0.7}};
+ ASSERT_EQ(result->vertices().size(), ground_truth.size());
+ auto alt_it = result->vertices().begin();
+ auto ground_truth_it = ground_truth.begin();
+ for (; alt_it != result->vertices().end(); ++alt_it, ++ground_truth_it) {
+ EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon);
+ EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon);
+ }
+ }
+}
+
+TEST(alt_geometry, intersects)
+{
+ using autoware_utils_geometry::intersects;
+ using autoware_utils_geometry::alt::ConvexPolygon2d;
+ using autoware_utils_geometry::alt::Point2d;
+
+ { // Normally crossing
+ const Point2d p1 = {0.0, -1.0};
+ const Point2d p2 = {0.0, 1.0};
+ const Point2d p3 = {-1.0, 0.0};
+ const Point2d p4 = {1.0, 0.0};
+ const auto result = intersects(p1, p2, p3, p4);
+
+ EXPECT_TRUE(result);
+ }
+
+ { // No crossing
+ const Point2d p1 = {0.0, -1.0};
+ const Point2d p2 = {0.0, 1.0};
+ const Point2d p3 = {1.0, 0.0};
+ const Point2d p4 = {3.0, 0.0};
+ const auto result = intersects(p1, p2, p3, p4);
+
+ EXPECT_FALSE(result);
+ }
+
+ { // One segment is the point on the other's segment
+ const Point2d p1 = {0.0, -1.0};
+ const Point2d p2 = {0.0, 1.0};
+ const Point2d p3 = {0.0, 0.0};
+ const Point2d p4 = {0.0, 0.0};
+ const auto result = intersects(p1, p2, p3, p4);
+
+ EXPECT_FALSE(result);
+ }
+
+ { // One segment is the point not on the other's segment
+ const Point2d p1 = {0.0, -1.0};
+ const Point2d p2 = {0.0, 1.0};
+ const Point2d p3 = {1.0, 0.0};
+ const Point2d p4 = {1.0, 0.0};
+ const auto result = intersects(p1, p2, p3, p4);
+
+ EXPECT_FALSE(result);
+ }
+
+ { // Both segments are the points which are the same position
+ const Point2d p1 = {0.0, 0.0};
+ const Point2d p2 = {0.0, 0.0};
+ const Point2d p3 = {0.0, 0.0};
+ const Point2d p4 = {0.0, 0.0};
+ const auto result = intersects(p1, p2, p3, p4);
+
+ EXPECT_FALSE(result);
+ }
+
+ { // Both segments are the points which are different position
+ const Point2d p1 = {0.0, 1.0};
+ const Point2d p2 = {0.0, 1.0};
+ const Point2d p3 = {1.0, 0.0};
+ const Point2d p4 = {1.0, 0.0};
+ const auto result = intersects(p1, p2, p3, p4);
+
+ EXPECT_FALSE(result);
+ }
+
+ { // Segments are the same
+ const Point2d p1 = {0.0, -1.0};
+ const Point2d p2 = {0.0, 1.0};
+ const Point2d p3 = {0.0, -1.0};
+ const Point2d p4 = {0.0, 1.0};
+ const auto result = intersects(p1, p2, p3, p4);
+
+ EXPECT_FALSE(result);
+ }
+
+ { // One's edge is on the other's segment
+ const Point2d p1 = {0.0, -1.0};
+ const Point2d p2 = {0.0, 1.0};
+ const Point2d p3 = {0.0, 0.0};
+ const Point2d p4 = {1.0, 0.0};
+ const auto result = intersects(p1, p2, p3, p4);
+
+ EXPECT_TRUE(result);
+ }
+
+ { // One's edge is the same as the other's edge.
+ const Point2d p1 = {0.0, -1.0};
+ const Point2d p2 = {0.0, 1.0};
+ const Point2d p3 = {0.0, -1.0};
+ const Point2d p4 = {2.0, -1.0};
+ const auto result = intersects(p1, p2, p3, p4);
+
+ EXPECT_TRUE(result);
+ }
+
+ { // One polygon intersects the other
+ const Point2d p1 = {1.0, 1.0};
+ const Point2d p2 = {1.0, -1.0};
+ const Point2d p3 = {-1.0, -1.0};
+ const Point2d p4 = {-1.0, 1.0};
+ const Point2d p5 = {2.0, 2.0};
+ const Point2d p6 = {2.0, 0.0};
+ const Point2d p7 = {0.0, 0.0};
+ const Point2d p8 = {0.0, 2.0};
+ const auto result = intersects(
+ ConvexPolygon2d::create({p1, p2, p3, p4}).value(),
+ ConvexPolygon2d::create({p5, p6, p7, p8}).value());
+
+ EXPECT_TRUE(result);
+ }
+
+ { // Two polygons do not intersect each other
+ const Point2d p1 = {1.0, 1.0};
+ const Point2d p2 = {1.0, -1.0};
+ const Point2d p3 = {-1.0, -1.0};
+ const Point2d p4 = {-1.0, 1.0};
+ const Point2d p5 = {3.0, 3.0};
+ const Point2d p6 = {3.0, 2.0};
+ const Point2d p7 = {2.0, 2.0};
+ const Point2d p8 = {2.0, 3.0};
+ const auto result = intersects(
+ ConvexPolygon2d::create({p1, p2, p3, p4}).value(),
+ ConvexPolygon2d::create({p5, p6, p7, p8}).value());
+
+ EXPECT_FALSE(result);
+ }
+
+ { // Two polygons share a vertex
+ const Point2d p1 = {1.0, 1.0};
+ const Point2d p2 = {1.0, -1.0};
+ const Point2d p3 = {-1.0, -1.0};
+ const Point2d p4 = {-1.0, 1.0};
+ const Point2d p5 = {2.0, 2.0};
+ const Point2d p6 = {2.0, 1.0};
+ const Point2d p7 = {1.0, 1.0};
+ const Point2d p8 = {1.0, 2.0};
+ const auto result = intersects(
+ ConvexPolygon2d::create({p1, p2, p3, p4}).value(),
+ ConvexPolygon2d::create({p5, p6, p7, p8}).value());
+
+ EXPECT_FALSE(result);
+ }
+}
+
+TEST(alt_geometry, isAbove)
+{
+ using autoware_utils_geometry::is_above;
+ using autoware_utils_geometry::alt::Point2d;
+
+ { // The point is above the line
+ const Point2d point = {0.0, 1.0};
+ const Point2d p1 = {-1.0, 0.0};
+ const Point2d p2 = {1.0, 0.0};
+ const auto result = is_above(point, p1, p2);
+
+ EXPECT_TRUE(result);
+ }
+
+ { // The point is below the line
+ const Point2d point = {0.0, -1.0};
+ const Point2d p1 = {-1.0, 0.0};
+ const Point2d p2 = {1.0, 0.0};
+ const auto result = is_above(point, p1, p2);
+
+ EXPECT_FALSE(result);
+ }
+
+ { // The point is on the line
+ const Point2d point = {0.0, 0.0};
+ const Point2d p1 = {-1.0, 0.0};
+ const Point2d p2 = {1.0, 0.0};
+ const auto result = is_above(point, p1, p2);
+
+ EXPECT_FALSE(result);
+ }
+}
+
+TEST(alt_geometry, isClockwise)
+{
+ using autoware_utils_geometry::is_clockwise;
+ using autoware_utils_geometry::alt::PointList2d;
+
+ { // Clockwise
+ PointList2d vertices;
+ vertices.push_back({0.0, 0.0});
+ vertices.push_back({0.0, 7.0});
+ vertices.push_back({4.0, 2.0});
+ vertices.push_back({2.0, 0.0});
+ const auto result = is_clockwise(vertices);
+
+ EXPECT_TRUE(result);
+ }
+
+ { // Counter-clockwise
+ PointList2d vertices;
+ vertices.push_back({0.0, 0.0});
+ vertices.push_back({2.0, 0.0});
+ vertices.push_back({4.0, 2.0});
+ vertices.push_back({0.0, 7.0});
+ const auto result = is_clockwise(vertices);
+
+ EXPECT_FALSE(result);
+ }
+}
+
+TEST(geometry, simplify)
+{
+ using autoware_utils_geometry::simplify;
+ using autoware_utils_geometry::alt::PointList2d;
+
+ {
+ const PointList2d points = {{1.1, 1.1}, {2.5, 2.1}, {3.1, 3.1}, {4.9, 1.1}, {3.1, 1.9}};
+ const double max_distance = 0.5;
+ const auto result = simplify(points, max_distance);
+
+ PointList2d ground_truth = {{1.1, 1.1}, {3.1, 3.1}, {4.9, 1.1}, {3.1, 1.9}};
+ ASSERT_EQ(result.size(), ground_truth.size());
+ auto alt_it = result.begin();
+ auto ground_truth_it = ground_truth.begin();
+ for (; alt_it != result.end(); ++alt_it, ++ground_truth_it) {
+ EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon);
+ EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon);
+ }
+ }
+}
+
+TEST(alt_geometry, touches)
+{
+ using autoware_utils_geometry::touches;
+ using autoware_utils_geometry::alt::ConvexPolygon2d;
+ using autoware_utils_geometry::alt::Point2d;
+
+ {
+ // The point is on the segment
+ const Point2d p = {0.0, 0.0};
+ const Point2d p1 = {-1.0, 0.0};
+ const Point2d p2 = {1.0, 0.0};
+ const auto result = touches(p, p1, p2);
+
+ EXPECT_TRUE(result);
+ }
+
+ {
+ // The point is the start point of the segment
+ const Point2d p = {-1.0, 0.0};
+ const Point2d p1 = {-1.0, 0.0};
+ const Point2d p2 = {1.0, 0.0};
+ const auto result = touches(p, p1, p2);
+
+ EXPECT_TRUE(result);
+ }
+
+ {
+ // The point is the end point of the segment
+ const Point2d p = {1.0, 0.0};
+ const Point2d p1 = {-1.0, 0.0};
+ const Point2d p2 = {1.0, 0.0};
+ const auto result = touches(p, p1, p2);
+
+ EXPECT_TRUE(result);
+ }
+
+ { // The point is not on the segment
+ const Point2d p = {0.0, 1.0};
+ const Point2d p1 = {-1.0, 0.0};
+ const Point2d p2 = {1.0, 0.0};
+ const auto result = touches(p, p1, p2);
+
+ EXPECT_FALSE(result);
+ }
+
+ { // The point is on the edge of the polygon
+ const Point2d point = {0.0, 0.0};
+ const Point2d p1 = {2.0, 1.0};
+ const Point2d p2 = {2.0, -1.0};
+ const Point2d p3 = {0.0, -1.0};
+ const Point2d p4 = {0.0, 1.0};
+ const auto result = touches(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+ EXPECT_TRUE(result);
+ }
+
+ { // The point is outside the polygon
+ const Point2d point = {0.0, 0.0};
+ const Point2d p1 = {2.0, 2.0};
+ const Point2d p2 = {2.0, 1.0};
+ const Point2d p3 = {1.0, 1.0};
+ const Point2d p4 = {1.0, 2.0};
+ const auto result = touches(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+ EXPECT_FALSE(result);
+ }
+}
+
+TEST(alt_geometry, within)
+{
+ using autoware_utils_geometry::within;
+ using autoware_utils_geometry::alt::ConvexPolygon2d;
+ using autoware_utils_geometry::alt::Point2d;
+
+ { // The point is within the polygon
+ const Point2d point = {0.0, 0.0};
+ const Point2d p1 = {1.0, 1.0};
+ const Point2d p2 = {1.0, -1.0};
+ const Point2d p3 = {-1.0, -1.0};
+ const Point2d p4 = {-1.0, 1.0};
+ const auto result = within(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+ EXPECT_TRUE(result);
+ }
+
+ { // The point is outside the polygon
+ const Point2d point = {0.0, 0.0};
+ const Point2d p1 = {2.0, 2.0};
+ const Point2d p2 = {2.0, 1.0};
+ const Point2d p3 = {1.0, 1.0};
+ const Point2d p4 = {1.0, 2.0};
+ const auto result = within(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+ EXPECT_FALSE(result);
+ }
+
+ { // The point is on the edge of the polygon
+ const Point2d point = {0.0, 0.0};
+ const Point2d p1 = {2.0, 1.0};
+ const Point2d p2 = {2.0, -1.0};
+ const Point2d p3 = {0.0, -1.0};
+ const Point2d p4 = {0.0, 1.0};
+ const auto result = within(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+ EXPECT_FALSE(result);
+ }
+
+ { // One polygon is within the other
+ const Point2d p1 = {1.0, 1.0};
+ const Point2d p2 = {1.0, -1.0};
+ const Point2d p3 = {-1.0, -1.0};
+ const Point2d p4 = {-1.0, 1.0};
+ const Point2d p5 = {2.0, 2.0};
+ const Point2d p6 = {2.0, -2.0};
+ const Point2d p7 = {-2.0, -2.0};
+ const Point2d p8 = {-2.0, 2.0};
+ const auto result = within(
+ ConvexPolygon2d::create({p1, p2, p3, p4}).value(),
+ ConvexPolygon2d::create({p5, p6, p7, p8}).value());
+
+ EXPECT_TRUE(result);
+ }
+
+ { // One polygon is outside the other
+ const Point2d p1 = {1.0, 1.0};
+ const Point2d p2 = {1.0, -1.0};
+ const Point2d p3 = {-1.0, -1.0};
+ const Point2d p4 = {-1.0, 1.0};
+ const Point2d p5 = {3.0, 3.0};
+ const Point2d p6 = {3.0, 2.0};
+ const Point2d p7 = {2.0, 2.0};
+ const Point2d p8 = {2.0, 3.0};
+ const auto result = within(
+ ConvexPolygon2d::create({p1, p2, p3, p4}).value(),
+ ConvexPolygon2d::create({p5, p6, p7, p8}).value());
+
+ EXPECT_FALSE(result);
+ }
+
+ { // Both polygons are the same
+ const Point2d p1 = {1.0, 1.0};
+ const Point2d p2 = {1.0, -1.0};
+ const Point2d p3 = {-1.0, -1.0};
+ const Point2d p4 = {-1.0, 1.0};
+ const Point2d p5 = {1.0, 1.0};
+ const Point2d p6 = {1.0, -1.0};
+ const Point2d p7 = {-1.0, -1.0};
+ const Point2d p8 = {-1.0, 1.0};
+ const auto result = within(
+ ConvexPolygon2d::create({p1, p2, p3, p4}).value(),
+ ConvexPolygon2d::create({p5, p6, p7, p8}).value());
+
+ EXPECT_TRUE(result);
+ }
+}
+
+TEST(alt_geometry, areaRand)
+{
+ std::vector polygons;
+ constexpr auto polygons_nb = 100;
+ constexpr auto max_vertices = 10;
+ constexpr auto max_values = 1000;
+
+ autoware_utils_system::StopWatch sw;
+ for (auto vertices = 3UL; vertices < max_vertices; ++vertices) {
+ double ground_truth_area_ns = 0.0;
+ double alt_area_ns = 0.0;
+
+ polygons.clear();
+ for (auto i = 0; i < polygons_nb; ++i) {
+ polygons.push_back(autoware_utils_geometry::random_convex_polygon(vertices, max_values));
+ }
+ for (auto i = 0UL; i < polygons.size(); ++i) {
+ sw.tic();
+ const auto ground_truth = boost::geometry::area(polygons[i]);
+ ground_truth_area_ns += sw.toc();
+
+ const auto alt_poly =
+ autoware_utils_geometry::alt::ConvexPolygon2d::create(polygons[i]).value();
+ sw.tic();
+ const auto alt = autoware_utils_geometry::area(alt_poly);
+ alt_area_ns += sw.toc();
+
+ if (std::abs(alt - ground_truth) > epsilon) {
+ std::cout << "Alt failed for the polygon: ";
+ std::cout << boost::geometry::wkt(polygons[i]) << std::endl;
+ }
+ EXPECT_NEAR(ground_truth, alt, epsilon);
+ }
+ std::printf("polygons_nb = %d, vertices = %ld\n", polygons_nb, vertices);
+ std::printf(
+ "\tArea:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n", ground_truth_area_ns / 1e6,
+ alt_area_ns / 1e6);
+ }
+}
+
+TEST(alt_geometry, convexHullRand)
+{
+ std::vector polygons;
+ constexpr auto polygons_nb = 100;
+ constexpr auto max_vertices = 10;
+ constexpr auto max_values = 1000;
+
+ autoware_utils_system::StopWatch sw;
+ for (auto vertices = 3UL; vertices < max_vertices; ++vertices) {
+ double ground_truth_hull_ns = 0.0;
+ double alt_hull_ns = 0.0;
+
+ polygons.clear();
+ for (auto i = 0; i < polygons_nb; ++i) {
+ polygons.push_back(autoware_utils_geometry::random_convex_polygon(vertices, max_values));
+ }
+ for (auto i = 0UL; i < polygons.size(); ++i) {
+ autoware_utils_geometry::MultiPoint2d outer;
+ for (const auto & point : polygons[i].outer()) {
+ outer.push_back(point);
+ }
+ autoware_utils_geometry::Polygon2d ground_truth;
+ sw.tic();
+ boost::geometry::convex_hull(outer, ground_truth);
+ ground_truth_hull_ns += sw.toc();
+
+ const auto vertices =
+ autoware_utils_geometry::alt::ConvexPolygon2d::create(polygons[i]).value().vertices();
+ sw.tic();
+ const auto alt = autoware_utils_geometry::convex_hull({vertices.begin(), vertices.end()});
+ alt_hull_ns += sw.toc();
+
+ ASSERT_TRUE(alt);
+ ASSERT_EQ(ground_truth.outer().size(), alt->vertices().size());
+ auto ground_truth_it = ground_truth.outer().begin();
+ auto alt_it = alt->vertices().begin();
+ for (; ground_truth_it != ground_truth.outer().end(); ++ground_truth_it, ++alt_it) {
+ EXPECT_NEAR(ground_truth_it->x(), alt_it->x(), epsilon);
+ EXPECT_NEAR(ground_truth_it->y(), alt_it->y(), epsilon);
+ }
+ }
+ std::printf("polygons_nb = %d, vertices = %ld\n", polygons_nb, vertices);
+ std::printf(
+ "\tConvex Hull:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ ground_truth_hull_ns / 1e6, alt_hull_ns / 1e6);
+ }
+}
+
+TEST(alt_geometry, coveredByRand)
+{
+ std::vector polygons;
+ constexpr auto polygons_nb = 100;
+ constexpr auto max_vertices = 10;
+ constexpr auto max_values = 1000;
+
+ autoware_utils_system::StopWatch sw;
+ for (auto vertices = 3UL; vertices < max_vertices; ++vertices) {
+ double ground_truth_covered_ns = 0.0;
+ double ground_truth_not_covered_ns = 0.0;
+ double alt_covered_ns = 0.0;
+ double alt_not_covered_ns = 0.0;
+ int covered_count = 0;
+
+ polygons.clear();
+ for (auto i = 0; i < polygons_nb; ++i) {
+ polygons.push_back(autoware_utils_geometry::random_convex_polygon(vertices, max_values));
+ }
+ for (auto i = 0UL; i < polygons.size(); ++i) {
+ for (const auto & point : polygons[i].outer()) {
+ for (auto j = 0UL; j < polygons.size(); ++j) {
+ sw.tic();
+ const auto ground_truth = boost::geometry::covered_by(point, polygons[j]);
+ if (ground_truth) {
+ ++covered_count;
+ ground_truth_covered_ns += sw.toc();
+ } else {
+ ground_truth_not_covered_ns += sw.toc();
+ }
+
+ const auto alt_point = autoware_utils_geometry::alt::Point2d(point);
+ const auto alt_poly =
+ autoware_utils_geometry::alt::ConvexPolygon2d::create(polygons[j]).value();
+ sw.tic();
+ const auto alt = autoware_utils_geometry::covered_by(alt_point, alt_poly);
+ if (alt) {
+ alt_covered_ns += sw.toc();
+ } else {
+ alt_not_covered_ns += sw.toc();
+ }
+
+ if (ground_truth != alt) {
+ std::cout << "Alt failed for the point and polygon: ";
+ std::cout << boost::geometry::wkt(point) << boost::geometry::wkt(polygons[j])
+ << std::endl;
+ }
+ EXPECT_EQ(ground_truth, alt);
+ }
+ }
+ }
+ std::printf(
+ "polygons_nb = %d, vertices = %ld, %d / %ld covered pairs\n", polygons_nb, vertices,
+ covered_count, polygons_nb * vertices * polygons_nb);
+ std::printf(
+ "\tCovered:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ ground_truth_covered_ns / 1e6, alt_covered_ns / 1e6);
+ std::printf(
+ "\tNot covered:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ ground_truth_not_covered_ns / 1e6, alt_not_covered_ns / 1e6);
+ std::printf(
+ "\tTotal:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ (ground_truth_not_covered_ns + ground_truth_covered_ns) / 1e6,
+ (alt_not_covered_ns + alt_covered_ns) / 1e6);
+ }
+}
+
+TEST(alt_geometry, disjointRand)
+{
+ std::vector polygons;
+ constexpr auto polygons_nb = 100;
+ constexpr auto max_vertices = 10;
+ constexpr auto max_values = 1000;
+
+ autoware_utils_system::StopWatch sw;
+ for (auto vertices = 3UL; vertices < max_vertices; ++vertices) {
+ double ground_truth_disjoint_ns = 0.0;
+ double ground_truth_not_disjoint_ns = 0.0;
+ double alt_disjoint_ns = 0.0;
+ double alt_not_disjoint_ns = 0.0;
+ int disjoint_count = 0;
+
+ polygons.clear();
+ for (auto i = 0; i < polygons_nb; ++i) {
+ polygons.push_back(autoware_utils_geometry::random_convex_polygon(vertices, max_values));
+ }
+ for (auto i = 0UL; i < polygons.size(); ++i) {
+ for (auto j = 0UL; j < polygons.size(); ++j) {
+ sw.tic();
+ const auto ground_truth = boost::geometry::disjoint(polygons[i], polygons[j]);
+ if (ground_truth) {
+ ++disjoint_count;
+ ground_truth_disjoint_ns += sw.toc();
+ } else {
+ ground_truth_not_disjoint_ns += sw.toc();
+ }
+
+ const auto alt_poly1 =
+ autoware_utils_geometry::alt::ConvexPolygon2d::create(polygons[i]).value();
+ const auto alt_poly2 =
+ autoware_utils_geometry::alt::ConvexPolygon2d::create(polygons[j]).value();
+ sw.tic();
+ const auto alt = autoware_utils_geometry::disjoint(alt_poly1, alt_poly2);
+ if (alt) {
+ alt_disjoint_ns += sw.toc();
+ } else {
+ alt_not_disjoint_ns += sw.toc();
+ }
+
+ if (ground_truth != alt) {
+ std::cout << "Failed for the 2 polygons: ";
+ std::cout << boost::geometry::wkt(polygons[i]) << boost::geometry::wkt(polygons[j])
+ << std::endl;
+ }
+ EXPECT_EQ(ground_truth, alt);
+ }
+ }
+ std::printf(
+ "polygons_nb = %d, vertices = %ld, %d / %d disjoint pairs\n", polygons_nb, vertices,
+ disjoint_count, polygons_nb * polygons_nb);
+ std::printf(
+ "\tDisjoint:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ ground_truth_disjoint_ns / 1e6, alt_disjoint_ns / 1e6);
+ std::printf(
+ "\tNot disjoint:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ ground_truth_not_disjoint_ns / 1e6, alt_not_disjoint_ns / 1e6);
+ std::printf(
+ "\tTotal:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ (ground_truth_not_disjoint_ns + ground_truth_disjoint_ns) / 1e6,
+ (alt_not_disjoint_ns + alt_disjoint_ns) / 1e6);
+ }
+}
+
+TEST(alt_geometry, intersectsRand)
+{
+ std::vector polygons;
+ constexpr auto polygons_nb = 100;
+ constexpr auto max_vertices = 10;
+ constexpr auto max_values = 1000;
+
+ autoware_utils_system::StopWatch sw;
+ for (auto vertices = 3UL; vertices < max_vertices; ++vertices) {
+ double ground_truth_intersect_ns = 0.0;
+ double ground_truth_no_intersect_ns = 0.0;
+ double alt_intersect_ns = 0.0;
+ double alt_no_intersect_ns = 0.0;
+ int intersect_count = 0;
+
+ polygons.clear();
+ for (auto i = 0; i < polygons_nb; ++i) {
+ polygons.push_back(autoware_utils_geometry::random_convex_polygon(vertices, max_values));
+ }
+ for (auto i = 0UL; i < polygons.size(); ++i) {
+ for (auto j = 0UL; j < polygons.size(); ++j) {
+ sw.tic();
+ const auto ground_truth = boost::geometry::intersects(polygons[i], polygons[j]);
+ if (ground_truth) {
+ ++intersect_count;
+ ground_truth_intersect_ns += sw.toc();
+ } else {
+ ground_truth_no_intersect_ns += sw.toc();
+ }
+
+ const auto alt_poly1 =
+ autoware_utils_geometry::alt::ConvexPolygon2d::create(polygons[i]).value();
+ const auto alt_poly2 =
+ autoware_utils_geometry::alt::ConvexPolygon2d::create(polygons[j]).value();
+ sw.tic();
+ const auto alt = autoware_utils_geometry::intersects(alt_poly1, alt_poly2);
+ if (alt) {
+ alt_intersect_ns += sw.toc();
+ } else {
+ alt_no_intersect_ns += sw.toc();
+ }
+
+ if (ground_truth != alt) {
+ std::cout << "Failed for the 2 polygons: ";
+ std::cout << boost::geometry::wkt(polygons[i]) << boost::geometry::wkt(polygons[j])
+ << std::endl;
+ }
+ EXPECT_EQ(ground_truth, alt);
+ }
+ }
+ std::printf(
+ "polygons_nb = %d, vertices = %ld, %d / %d pairs with intersects\n", polygons_nb, vertices,
+ intersect_count, polygons_nb * polygons_nb);
+ std::printf(
+ "\tIntersect:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ ground_truth_intersect_ns / 1e6, alt_intersect_ns / 1e6);
+ std::printf(
+ "\tNo intersect:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ ground_truth_no_intersect_ns / 1e6, alt_no_intersect_ns / 1e6);
+ std::printf(
+ "\tTotal:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ (ground_truth_no_intersect_ns + ground_truth_intersect_ns) / 1e6,
+ (alt_no_intersect_ns + alt_intersect_ns) / 1e6);
+ }
+}
+
+TEST(alt_geometry, touchesRand)
+{
+ std::vector polygons;
+ constexpr auto polygons_nb = 100;
+ constexpr auto max_vertices = 10;
+ constexpr auto max_values = 1000;
+
+ autoware_utils_system::StopWatch sw;
+ for (auto vertices = 3UL; vertices < max_vertices; ++vertices) {
+ double ground_truth_touching_ns = 0.0;
+ double ground_truth_not_touching_ns = 0.0;
+ double alt_touching_ns = 0.0;
+ double alt_not_touching_ns = 0.0;
+ int touching_count = 0;
+
+ polygons.clear();
+ for (auto i = 0; i < polygons_nb; ++i) {
+ polygons.push_back(autoware_utils_geometry::random_convex_polygon(vertices, max_values));
+ }
+ for (auto i = 0UL; i < polygons.size(); ++i) {
+ for (const auto & point : polygons[i].outer()) {
+ for (auto j = 0UL; j < polygons.size(); ++j) {
+ sw.tic();
+ const auto ground_truth = boost::geometry::touches(point, polygons[j]);
+ if (ground_truth) {
+ ++touching_count;
+ ground_truth_touching_ns += sw.toc();
+ } else {
+ ground_truth_not_touching_ns += sw.toc();
+ }
+
+ const auto alt_point = autoware_utils_geometry::alt::Point2d(point);
+ const auto alt_poly =
+ autoware_utils_geometry::alt::ConvexPolygon2d::create(polygons[j]).value();
+ sw.tic();
+ const auto alt = autoware_utils_geometry::touches(alt_point, alt_poly);
+ if (alt) {
+ alt_touching_ns += sw.toc();
+ } else {
+ alt_not_touching_ns += sw.toc();
+ }
+
+ if (ground_truth != alt) {
+ std::cout << "Alt failed for the point and polygon: ";
+ std::cout << boost::geometry::wkt(point) << boost::geometry::wkt(polygons[j])
+ << std::endl;
+ }
+ EXPECT_EQ(ground_truth, alt);
+ }
+ }
+ }
+ std::printf(
+ "polygons_nb = %d, vertices = %ld, %d / %ld touching pairs\n", polygons_nb, vertices,
+ touching_count, polygons_nb * vertices * polygons_nb);
+ std::printf(
+ "\tTouching:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ ground_truth_touching_ns / 1e6, alt_touching_ns / 1e6);
+ std::printf(
+ "\tNot touching:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ ground_truth_not_touching_ns / 1e6, alt_not_touching_ns / 1e6);
+ std::printf(
+ "\tTotal:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ (ground_truth_not_touching_ns + ground_truth_touching_ns) / 1e6,
+ (alt_not_touching_ns + alt_touching_ns) / 1e6);
+ }
+}
+
+TEST(alt_geometry, withinPolygonRand)
+{
+ std::vector polygons;
+ constexpr auto polygons_nb = 100;
+ constexpr auto max_vertices = 10;
+ constexpr auto max_values = 1000;
+
+ autoware_utils_system::StopWatch sw;
+ for (auto vertices = 3UL; vertices < max_vertices; ++vertices) {
+ double ground_truth_within_ns = 0.0;
+ double ground_truth_not_within_ns = 0.0;
+ double alt_within_ns = 0.0;
+ double alt_not_within_ns = 0.0;
+ int within_count = 0;
+
+ polygons.clear();
+ for (auto i = 0; i < polygons_nb; ++i) {
+ polygons.push_back(autoware_utils_geometry::random_convex_polygon(vertices, max_values));
+ }
+ for (auto i = 0UL; i < polygons.size(); ++i) {
+ for (auto j = 0UL; j < polygons.size(); ++j) {
+ sw.tic();
+ const auto ground_truth = boost::geometry::within(polygons[i], polygons[j]);
+ if (ground_truth) {
+ ++within_count;
+ ground_truth_within_ns += sw.toc();
+ } else {
+ ground_truth_not_within_ns += sw.toc();
+ }
+
+ const auto alt_poly1 =
+ autoware_utils_geometry::alt::ConvexPolygon2d::create(polygons[i]).value();
+ const auto alt_poly2 =
+ autoware_utils_geometry::alt::ConvexPolygon2d::create(polygons[j]).value();
+ sw.tic();
+ const auto alt = autoware_utils_geometry::within(alt_poly1, alt_poly2);
+ if (alt) {
+ alt_within_ns += sw.toc();
+ } else {
+ alt_not_within_ns += sw.toc();
+ }
+
+ if (ground_truth != alt) {
+ std::cout << "Alt failed for the 2 polygons: ";
+ std::cout << boost::geometry::wkt(polygons[i]) << boost::geometry::wkt(polygons[j])
+ << std::endl;
+ }
+ EXPECT_EQ(ground_truth, alt);
+ }
+ }
+ std::printf(
+ "polygons_nb = %d, vertices = %ld, %d / %d pairs either of which is within the other\n",
+ polygons_nb, vertices, within_count, polygons_nb * polygons_nb);
+ std::printf(
+ "\tWithin:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ ground_truth_within_ns / 1e6, alt_within_ns / 1e6);
+ std::printf(
+ "\tNot within:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ ground_truth_not_within_ns / 1e6, alt_not_within_ns / 1e6);
+ std::printf(
+ "\tTotal:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+ (ground_truth_not_within_ns + ground_truth_within_ns) / 1e6,
+ (alt_not_within_ns + alt_within_ns) / 1e6);
+ }
+}
diff --git a/autoware_utils_geometry/test/cases/boost_geometry.cpp b/autoware_utils_geometry/test/cases/boost_geometry.cpp
new file mode 100644
index 0000000..fd91a06
--- /dev/null
+++ b/autoware_utils_geometry/test/cases/boost_geometry.cpp
@@ -0,0 +1,83 @@
+// Copyright 2020 Tier IV, 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.
+
+#include "autoware_utils_geometry/boost_geometry.hpp"
+
+#include
+
+#include
+
+namespace bg = boost::geometry;
+
+using autoware_utils_geometry::Point2d;
+using autoware_utils_geometry::Point3d;
+
+TEST(boost_geometry, boost_geometry_distance)
+{
+ {
+ const Point2d p1(1.0, 2.0);
+ const Point2d p2(2.0, 4.0);
+ EXPECT_DOUBLE_EQ(bg::distance(p1, p2), std::sqrt(5));
+ }
+
+ {
+ const Point3d p1(1.0, 2.0, 3.0);
+ const Point3d p2(2.0, 4.0, 6.0);
+ EXPECT_DOUBLE_EQ(bg::distance(p1, p2), std::sqrt(14));
+ }
+}
+
+TEST(boost_geometry, to_3d)
+{
+ const Point2d p_2d(1.0, 2.0);
+ const Point3d p_3d(1.0, 2.0, 3.0);
+ EXPECT_TRUE(p_2d.to_3d(3.0) == p_3d);
+}
+
+TEST(boost_geometry, to_2d)
+{
+ const Point2d p_2d(1.0, 2.0);
+ const Point3d p_3d(1.0, 2.0, 3.0);
+ EXPECT_TRUE(p_3d.to_2d() == p_2d);
+}
+
+TEST(boost_geometry, to_msg)
+{
+ using autoware_utils_geometry::to_msg;
+
+ {
+ const Point3d p(1.0, 2.0, 3.0);
+ const geometry_msgs::msg::Point p_msg = to_msg(p);
+
+ EXPECT_DOUBLE_EQ(p_msg.x, 1.0);
+ EXPECT_DOUBLE_EQ(p_msg.y, 2.0);
+ EXPECT_DOUBLE_EQ(p_msg.z, 3.0);
+ }
+}
+
+TEST(boost_geometry, from_msg)
+{
+ using autoware_utils_geometry::from_msg;
+
+ geometry_msgs::msg::Point p_msg;
+ p_msg.x = 1.0;
+ p_msg.y = 2.0;
+ p_msg.z = 3.0;
+
+ const Point3d p = from_msg(p_msg);
+
+ EXPECT_DOUBLE_EQ(p.x(), 1.0);
+ EXPECT_DOUBLE_EQ(p.y(), 2.0);
+ EXPECT_DOUBLE_EQ(p.z(), 3.0);
+}
diff --git a/autoware_utils_geometry/test/cases/boost_polygon_utils.cpp b/autoware_utils_geometry/test/cases/boost_polygon_utils.cpp
new file mode 100644
index 0000000..e9b6279
--- /dev/null
+++ b/autoware_utils_geometry/test/cases/boost_polygon_utils.cpp
@@ -0,0 +1,333 @@
+// Copyright 2020 Tier IV, 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.
+
+#include "autoware_utils_geometry/boost_polygon_utils.hpp"
+
+#include "autoware_utils_geometry/geometry.hpp"
+
+#include
+
+#include
+
+using autoware_utils_geometry::Polygon2d;
+
+namespace
+{
+geometry_msgs::msg::Point32 create_point32(const double x, const double y)
+{
+ geometry_msgs::msg::Point32 p;
+ p.x = x;
+ p.y = y;
+ p.z = 0.0;
+
+ return p;
+}
+
+geometry_msgs::msg::Pose create_pose(const double x, const double y, const double yaw)
+{
+ geometry_msgs::msg::Pose p;
+ p.position.x = x;
+ p.position.y = y;
+ p.position.z = 0.0;
+ p.orientation = autoware_utils_geometry::create_quaternion_from_yaw(yaw);
+
+ return p;
+}
+} // namespace
+
+TEST(boost_geometry, boost_is_clockwise)
+{
+ using autoware_utils_geometry::is_clockwise;
+ using autoware_utils_geometry::Polygon2d;
+
+ // empty
+ Polygon2d empty_polygon;
+ EXPECT_THROW(is_clockwise(empty_polygon), std::out_of_range);
+
+ // normal case
+ Polygon2d clock_wise_polygon{{{0.0, 0.0}, {0.0, 1.0}, {1.0, 1.0}, {1.0, 0.0}, {0.0, 0.0}}};
+ EXPECT_TRUE(is_clockwise(clock_wise_polygon));
+
+ Polygon2d anti_clock_wise_polygon{{{0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 1.0}, {0.0, 0.0}}};
+ EXPECT_FALSE(is_clockwise(anti_clock_wise_polygon));
+
+ // duplicated
+ Polygon2d duplicated_clock_wise_polygon{
+ {{0.0, 0.0}, {0.0, 1.0}, {0.0, 1.0}, {1.0, 1.0}, {1.0, 0.0}, {0.0, 0.0}}};
+ EXPECT_TRUE(is_clockwise(duplicated_clock_wise_polygon));
+
+ Polygon2d duplicated_anti_clock_wise_polygon{
+ {{0.0, 0.0}, {1.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 1.0}, {0.0, 0.0}}};
+ EXPECT_FALSE(is_clockwise(duplicated_anti_clock_wise_polygon));
+}
+
+TEST(boost_geometry, boost_inverse_clockwise)
+{
+ using autoware_utils_geometry::inverse_clockwise;
+ using autoware_utils_geometry::is_clockwise;
+
+ // empty
+ Polygon2d empty_polygon;
+ const auto reversed_empty_polygon = inverse_clockwise(empty_polygon);
+ EXPECT_EQ(static_cast(reversed_empty_polygon.outer().size()), 0);
+
+ // normal case
+ Polygon2d clock_wise_polygon{{{0.0, 0.0}, {0.0, 1.0}, {1.0, 1.0}, {1.0, 0.0}, {0.0, 0.0}}};
+ EXPECT_FALSE(is_clockwise(inverse_clockwise(clock_wise_polygon)));
+
+ Polygon2d anti_clock_wise_polygon{{{0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 1.0}, {0.0, 0.0}}};
+ EXPECT_TRUE(is_clockwise(inverse_clockwise(anti_clock_wise_polygon)));
+
+ // duplicated
+ Polygon2d duplicated_clock_wise_polygon{
+ {{0.0, 0.0}, {0.0, 1.0}, {0.0, 1.0}, {1.0, 1.0}, {1.0, 0.0}, {0.0, 0.0}}};
+ EXPECT_FALSE(is_clockwise(inverse_clockwise(duplicated_clock_wise_polygon)));
+
+ Polygon2d duplicated_anti_clock_wise_polygon{
+ {{0.0, 0.0}, {1.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 1.0}, {0.0, 0.0}}};
+ EXPECT_TRUE(is_clockwise(inverse_clockwise(duplicated_anti_clock_wise_polygon)));
+}
+
+TEST(boost_geometry, boost_rotate_polygon)
+{
+ constexpr double epsilon = 1e-6;
+ using autoware_utils_geometry::rotate_polygon;
+
+ // empty
+ geometry_msgs::msg::Polygon empty_polygon;
+ const auto rotated_empty_polygon = rotate_polygon(empty_polygon, 0.0);
+ EXPECT_EQ(static_cast(rotated_empty_polygon.points.size()), 0);
+
+ // normal case
+ geometry_msgs::msg::Polygon clock_wise_polygon;
+ clock_wise_polygon.points.push_back(create_point32(0.0, 0.0));
+ clock_wise_polygon.points.push_back(create_point32(0.0, 1.0));
+ clock_wise_polygon.points.push_back(create_point32(1.0, 1.0));
+ clock_wise_polygon.points.push_back(create_point32(1.0, 0.0));
+ clock_wise_polygon.points.push_back(create_point32(0.0, 0.0));
+ const auto rotated_clock_wise_polygon = rotate_polygon(clock_wise_polygon, M_PI_4);
+
+ EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(0).x, 0.0);
+ EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(0).y, 0.0);
+ EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(1).x, -0.70710676908493042);
+ EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(1).y, 0.70710676908493042);
+ EXPECT_NEAR(rotated_clock_wise_polygon.points.at(2).x, 0.0, epsilon);
+ EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(2).y, 1.4142135381698608);
+ EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(3).x, 0.70710676908493042);
+ EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(3).y, 0.70710676908493042);
+ EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(4).x, 0.0);
+ EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(4).y, 0.0);
+}
+
+TEST(boost_geometry, boost_to_polygon2d)
+{
+ using autoware_utils_geometry::to_polygon2d;
+
+ { // bounding box
+ const double x = 1.0;
+ const double y = 1.0;
+
+ const auto pose = create_pose(1.0, 1.0, M_PI_4);
+ autoware_perception_msgs::msg::Shape shape;
+ shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
+ shape.dimensions.x = x;
+ shape.dimensions.y = y;
+
+ const auto poly = to_polygon2d(pose, shape);
+ EXPECT_DOUBLE_EQ(poly.outer().at(0).x(), 1.0);
+ EXPECT_DOUBLE_EQ(poly.outer().at(0).y(), 1.7071067811865475);
+ EXPECT_DOUBLE_EQ(poly.outer().at(1).x(), 1.7071067811865475);
+ EXPECT_DOUBLE_EQ(poly.outer().at(1).y(), 1.0);
+ EXPECT_DOUBLE_EQ(poly.outer().at(2).x(), 1.0);
+ EXPECT_DOUBLE_EQ(poly.outer().at(2).y(), 0.29289321881345254);
+ EXPECT_DOUBLE_EQ(poly.outer().at(3).x(), 0.29289321881345254);
+ EXPECT_DOUBLE_EQ(poly.outer().at(3).y(), 1.0);
+ EXPECT_DOUBLE_EQ(poly.outer().at(4).x(), 1.0);
+ EXPECT_DOUBLE_EQ(poly.outer().at(4).y(), 1.7071067811865475);
+ }
+
+ { // cylinder
+ const double diameter = 1.0;
+
+ const auto pose = create_pose(1.0, 1.0, M_PI_4);
+ autoware_perception_msgs::msg::Shape shape;
+ shape.type = autoware_perception_msgs::msg::Shape::CYLINDER;
+ shape.dimensions.x = diameter;
+
+ const auto poly = to_polygon2d(pose, shape);
+ EXPECT_DOUBLE_EQ(poly.outer().at(0).x(), 1.4330127018922194);
+ EXPECT_DOUBLE_EQ(poly.outer().at(0).y(), 1.25);
+ EXPECT_DOUBLE_EQ(poly.outer().at(1).x(), 1.4330127018922194);
+ EXPECT_DOUBLE_EQ(poly.outer().at(1).y(), 0.75);
+ EXPECT_DOUBLE_EQ(poly.outer().at(2).x(), 1.0);
+ EXPECT_DOUBLE_EQ(poly.outer().at(2).y(), 0.5);
+ EXPECT_DOUBLE_EQ(poly.outer().at(3).x(), 0.56698729810778059);
+ EXPECT_DOUBLE_EQ(poly.outer().at(3).y(), 0.75);
+ EXPECT_DOUBLE_EQ(poly.outer().at(4).x(), 0.56698729810778081);
+ EXPECT_DOUBLE_EQ(poly.outer().at(4).y(), 1.25);
+ EXPECT_DOUBLE_EQ(poly.outer().at(5).x(), 1.0);
+ EXPECT_DOUBLE_EQ(poly.outer().at(5).y(), 1.5);
+ }
+
+ { // polygon
+ const double x = 0.5;
+ const double y = 0.5;
+
+ const auto pose = create_pose(1.0, 1.0, M_PI_4);
+ autoware_perception_msgs::msg::Shape shape;
+ shape.type = autoware_perception_msgs::msg::Shape::POLYGON;
+ shape.footprint.points.push_back(create_point32(-x, -y));
+ shape.footprint.points.push_back(create_point32(-x, y));
+ shape.footprint.points.push_back(create_point32(x, y));
+ shape.footprint.points.push_back(create_point32(x, -y));
+
+ const auto poly = to_polygon2d(pose, shape);
+ EXPECT_DOUBLE_EQ(poly.outer().at(0).x(), 1.0);
+ EXPECT_DOUBLE_EQ(poly.outer().at(0).y(), 0.29289323091506958);
+ EXPECT_DOUBLE_EQ(poly.outer().at(1).x(), 0.29289323091506958);
+ EXPECT_DOUBLE_EQ(poly.outer().at(1).y(), 1.0);
+ EXPECT_DOUBLE_EQ(poly.outer().at(2).x(), 1.0);
+ EXPECT_DOUBLE_EQ(poly.outer().at(2).y(), 1.7071067690849304);
+ EXPECT_DOUBLE_EQ(poly.outer().at(3).x(), 1.7071067690849304);
+ EXPECT_DOUBLE_EQ(poly.outer().at(3).y(), 1.0);
+ EXPECT_DOUBLE_EQ(poly.outer().at(4).x(), 1.0);
+ EXPECT_DOUBLE_EQ(poly.outer().at(4).y(), 0.29289323091506958);
+ }
+}
+
+TEST(boost_geometry, boost_to_footprint)
+{
+ using autoware_utils_geometry::to_footprint;
+
+ // from base link
+ {
+ const double x = 1.0;
+ const double y = 1.0;
+
+ const auto base_link_pose = create_pose(1.0, 1.0, M_PI_4);
+ const double base_to_front = 4.0;
+ const double base_to_back = 1.0;
+ const double width = 2.0;
+
+ const auto poly = to_footprint(base_link_pose, base_to_front, base_to_back, width);
+ EXPECT_DOUBLE_EQ(poly.outer().at(0).x(), 3.0 / std::sqrt(2) + x);
+ EXPECT_DOUBLE_EQ(poly.outer().at(0).y(), 5.0 / std::sqrt(2) + y);
+ EXPECT_DOUBLE_EQ(poly.outer().at(1).x(), 5.0 / std::sqrt(2) + x);
+ EXPECT_DOUBLE_EQ(poly.outer().at(1).y(), 3.0 / std::sqrt(2) + y);
+ EXPECT_DOUBLE_EQ(poly.outer().at(2).x(), x);
+ EXPECT_DOUBLE_EQ(poly.outer().at(2).y(), y - std::sqrt(2));
+ EXPECT_DOUBLE_EQ(poly.outer().at(3).x(), x - std::sqrt(2));
+ EXPECT_DOUBLE_EQ(poly.outer().at(3).y(), y);
+ EXPECT_DOUBLE_EQ(poly.outer().at(4).x(), 3.0 / std::sqrt(2) + x);
+ EXPECT_DOUBLE_EQ(poly.outer().at(4).y(), 5.0 / std::sqrt(2) + y);
+ }
+}
+
+TEST(boost_geometry, boost_get_area)
+{
+ using autoware_utils_geometry::get_area;
+
+ { // bounding box
+ const double x = 1.0;
+ const double y = 2.0;
+
+ autoware_perception_msgs::msg::Shape shape;
+ shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
+ shape.dimensions.x = x;
+ shape.dimensions.y = y;
+
+ const double area = get_area(shape);
+ EXPECT_DOUBLE_EQ(area, x * y);
+ }
+
+ { // cylinder
+ const double diameter = 1.0;
+
+ autoware_perception_msgs::msg::Shape shape;
+ shape.type = autoware_perception_msgs::msg::Shape::CYLINDER;
+ shape.dimensions.x = diameter;
+
+ const double area = get_area(shape);
+ EXPECT_DOUBLE_EQ(area, std::pow(diameter / 2.0, 2.0) * M_PI);
+ }
+
+ { // polygon
+ const double x = 1.0;
+ const double y = 2.0;
+
+ // clock wise
+ autoware_perception_msgs::msg::Shape clock_wise_shape;
+ clock_wise_shape.type = autoware_perception_msgs::msg::Shape::POLYGON;
+ clock_wise_shape.footprint.points.push_back(create_point32(0.0, 0.0));
+ clock_wise_shape.footprint.points.push_back(create_point32(0.0, y));
+ clock_wise_shape.footprint.points.push_back(create_point32(x, y));
+ clock_wise_shape.footprint.points.push_back(create_point32(x, 0.0));
+
+ const double clock_wise_area = get_area(clock_wise_shape);
+ EXPECT_DOUBLE_EQ(clock_wise_area, -x * y);
+
+ // anti clock wise
+ autoware_perception_msgs::msg::Shape anti_clock_wise_shape;
+ anti_clock_wise_shape.type = autoware_perception_msgs::msg::Shape::POLYGON;
+ anti_clock_wise_shape.footprint.points.push_back(create_point32(0.0, 0.0));
+ anti_clock_wise_shape.footprint.points.push_back(create_point32(x, 0.0));
+ anti_clock_wise_shape.footprint.points.push_back(create_point32(x, y));
+ anti_clock_wise_shape.footprint.points.push_back(create_point32(0.0, y));
+
+ const double anti_clock_wise_area = get_area(anti_clock_wise_shape);
+ EXPECT_DOUBLE_EQ(anti_clock_wise_area, x * y);
+ }
+}
+
+TEST(boost_geometry, boost_expand_polygon)
+{
+ using autoware_utils_geometry::expand_polygon;
+
+ { // box with a certain offset
+ Polygon2d box_poly{{{-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}}};
+ const auto expanded_poly = expand_polygon(box_poly, 1.0);
+
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(0).x(), -2.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(0).y(), -2.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(1).x(), -2.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(1).y(), 2.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(2).x(), 2.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(2).y(), 2.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(3).x(), 2.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(3).y(), -2.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(4).x(), -2.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(4).y(), -2.0);
+ }
+
+ { // box with no offset
+ Polygon2d box_poly{{{-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}}};
+ const auto expanded_poly = expand_polygon(box_poly, 0.0);
+
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(0).x(), -1.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(0).y(), -1.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(1).x(), -1.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(1).y(), 1.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(2).x(), 1.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(2).y(), 1.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(3).x(), 1.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(3).y(), -1.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(4).x(), -1.0);
+ EXPECT_DOUBLE_EQ(expanded_poly.outer().at(4).y(), -1.0);
+ }
+
+ { // empty polygon
+ Polygon2d empty_poly;
+ EXPECT_THROW(expand_polygon(empty_poly, 1.0), std::out_of_range);
+ }
+}
diff --git a/autoware_utils_geometry/test/cases/geometry.cpp b/autoware_utils_geometry/test/cases/geometry.cpp
new file mode 100644
index 0000000..e8a8a0b
--- /dev/null
+++ b/autoware_utils_geometry/test/cases/geometry.cpp
@@ -0,0 +1,2427 @@
+// Copyright 2020-2024 Tier IV, 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.
+
+#include "autoware_utils_geometry/geometry.hpp"
+
+#include "autoware_utils_geometry/boost_geometry.hpp"
+#include "autoware_utils_geometry/ear_clipping.hpp"
+#include "autoware_utils_geometry/random_concave_polygon.hpp"
+#include "autoware_utils_geometry/random_convex_polygon.hpp"
+#include "autoware_utils_geometry/sat_2d.hpp"
+#include "autoware_utils_math/unit_conversion.hpp"
+#include "autoware_utils_system/stop_watch.hpp"
+
+#include
+
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+constexpr double epsilon = 1e-6;
+
+TEST(geometry, get_point)
+{
+ using autoware_utils_geometry::get_point;
+
+ const double x_ans = 1.0;
+ const double y_ans = 2.0;
+ const double z_ans = 3.0;
+
+ {
+ struct AnyPoint
+ {
+ double x;
+ double y;
+ double z;
+ };
+ const AnyPoint p{x_ans, y_ans, z_ans};
+ const geometry_msgs::msg::Point p_out = get_point(p);
+ EXPECT_DOUBLE_EQ(p_out.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.z, z_ans);
+ }
+
+ {
+ geometry_msgs::msg::Point p;
+ p.x = x_ans;
+ p.y = y_ans;
+ p.z = z_ans;
+ const geometry_msgs::msg::Point p_out = get_point(p);
+ EXPECT_DOUBLE_EQ(p_out.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.z, z_ans);
+ }
+
+ {
+ geometry_msgs::msg::Pose p;
+ p.position.x = x_ans;
+ p.position.y = y_ans;
+ p.position.z = z_ans;
+ const geometry_msgs::msg::Point p_out = get_point(p);
+ EXPECT_DOUBLE_EQ(p_out.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.z, z_ans);
+ }
+
+ {
+ geometry_msgs::msg::PoseStamped p;
+ p.pose.position.x = x_ans;
+ p.pose.position.y = y_ans;
+ p.pose.position.z = z_ans;
+ const geometry_msgs::msg::Point p_out = get_point(p);
+ EXPECT_DOUBLE_EQ(p_out.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.z, z_ans);
+ }
+
+ {
+ geometry_msgs::msg::PoseWithCovarianceStamped p;
+ p.pose.pose.position.x = x_ans;
+ p.pose.pose.position.y = y_ans;
+ p.pose.pose.position.z = z_ans;
+ const geometry_msgs::msg::Point p_out = get_point(p);
+ EXPECT_DOUBLE_EQ(p_out.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.z, z_ans);
+ }
+
+ {
+ autoware_planning_msgs::msg::PathPoint p;
+ p.pose.position.x = x_ans;
+ p.pose.position.y = y_ans;
+ p.pose.position.z = z_ans;
+ const geometry_msgs::msg::Point p_out = get_point(p);
+ EXPECT_DOUBLE_EQ(p_out.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.z, z_ans);
+ }
+
+ {
+ autoware_planning_msgs::msg::TrajectoryPoint p;
+ p.pose.position.x = x_ans;
+ p.pose.position.y = y_ans;
+ p.pose.position.z = z_ans;
+ const geometry_msgs::msg::Point p_out = get_point(p);
+ EXPECT_DOUBLE_EQ(p_out.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.z, z_ans);
+ }
+}
+
+TEST(geometry, get_pose)
+{
+ using autoware_utils_geometry::get_pose;
+
+ const double x_ans = 1.0;
+ const double y_ans = 2.0;
+ const double z_ans = 3.0;
+ const double q_x_ans = 0.1;
+ const double q_y_ans = 0.2;
+ const double q_z_ans = 0.3;
+ const double q_w_ans = 0.4;
+
+ {
+ geometry_msgs::msg::Pose p;
+ p.position.x = x_ans;
+ p.position.y = y_ans;
+ p.position.z = z_ans;
+ p.orientation.x = q_x_ans;
+ p.orientation.y = q_y_ans;
+ p.orientation.z = q_z_ans;
+ p.orientation.w = q_w_ans;
+ const geometry_msgs::msg::Pose p_out = get_pose(p);
+ EXPECT_DOUBLE_EQ(p_out.position.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.position.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.position.z, z_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans);
+ }
+
+ {
+ geometry_msgs::msg::PoseStamped p;
+ p.pose.position.x = x_ans;
+ p.pose.position.y = y_ans;
+ p.pose.position.z = z_ans;
+ p.pose.orientation.x = q_x_ans;
+ p.pose.orientation.y = q_y_ans;
+ p.pose.orientation.z = q_z_ans;
+ p.pose.orientation.w = q_w_ans;
+ const geometry_msgs::msg::Pose p_out = get_pose(p);
+ EXPECT_DOUBLE_EQ(p_out.position.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.position.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.position.z, z_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans);
+ }
+
+ {
+ autoware_planning_msgs::msg::PathPoint p;
+ p.pose.position.x = x_ans;
+ p.pose.position.y = y_ans;
+ p.pose.position.z = z_ans;
+ p.pose.orientation.x = q_x_ans;
+ p.pose.orientation.y = q_y_ans;
+ p.pose.orientation.z = q_z_ans;
+ p.pose.orientation.w = q_w_ans;
+ const geometry_msgs::msg::Pose p_out = get_pose(p);
+ EXPECT_DOUBLE_EQ(p_out.position.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.position.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.position.z, z_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans);
+ }
+
+ {
+ autoware_planning_msgs::msg::TrajectoryPoint p;
+ p.pose.position.x = x_ans;
+ p.pose.position.y = y_ans;
+ p.pose.position.z = z_ans;
+ p.pose.orientation.x = q_x_ans;
+ p.pose.orientation.y = q_y_ans;
+ p.pose.orientation.z = q_z_ans;
+ p.pose.orientation.w = q_w_ans;
+ const geometry_msgs::msg::Pose p_out = get_pose(p);
+ EXPECT_DOUBLE_EQ(p_out.position.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.position.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.position.z, z_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans);
+ }
+}
+
+TEST(geometry, get_longitudinal_velocity)
+{
+ using autoware_utils_geometry::get_longitudinal_velocity;
+
+ const double velocity = 1.0;
+
+ {
+ autoware_planning_msgs::msg::PathPoint p;
+ p.longitudinal_velocity_mps = velocity;
+ EXPECT_DOUBLE_EQ(get_longitudinal_velocity(p), velocity);
+ }
+
+ {
+ autoware_planning_msgs::msg::TrajectoryPoint p;
+ p.longitudinal_velocity_mps = velocity;
+ EXPECT_DOUBLE_EQ(get_longitudinal_velocity(p), velocity);
+ }
+}
+
+TEST(geometry, set_pose)
+{
+ using autoware_utils_geometry::set_pose;
+
+ const double x_ans = 1.0;
+ const double y_ans = 2.0;
+ const double z_ans = 3.0;
+ const double q_x_ans = 0.1;
+ const double q_y_ans = 0.2;
+ const double q_z_ans = 0.3;
+ const double q_w_ans = 0.4;
+
+ geometry_msgs::msg::Pose p;
+ p.position.x = x_ans;
+ p.position.y = y_ans;
+ p.position.z = z_ans;
+ p.orientation.x = q_x_ans;
+ p.orientation.y = q_y_ans;
+ p.orientation.z = q_z_ans;
+ p.orientation.w = q_w_ans;
+
+ {
+ geometry_msgs::msg::Pose p_out{};
+ set_pose(p, p_out);
+ EXPECT_DOUBLE_EQ(p_out.position.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.position.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.position.z, z_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans);
+ }
+
+ {
+ geometry_msgs::msg::PoseStamped p_out{};
+ set_pose(p, p_out);
+ EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.position.z, z_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.orientation.x, q_x_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.orientation.y, q_y_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.orientation.z, q_z_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.orientation.w, q_w_ans);
+ }
+
+ {
+ autoware_planning_msgs::msg::PathPoint p_out{};
+ set_pose(p, p_out);
+ EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.position.z, z_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.orientation.x, q_x_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.orientation.y, q_y_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.orientation.z, q_z_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.orientation.w, q_w_ans);
+ }
+
+ {
+ autoware_planning_msgs::msg::TrajectoryPoint p_out{};
+ set_pose(p, p_out);
+ EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.position.z, z_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.orientation.x, q_x_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.orientation.y, q_y_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.orientation.z, q_z_ans);
+ EXPECT_DOUBLE_EQ(p_out.pose.orientation.w, q_w_ans);
+ }
+}
+
+TEST(geometry, set_orientation)
+{
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_geometry::set_orientation;
+ using autoware_utils_math::deg2rad;
+
+ geometry_msgs::msg::Pose p;
+ const auto orientation = create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+ set_orientation(orientation, p);
+
+ EXPECT_DOUBLE_EQ(p.orientation.x, orientation.x);
+ EXPECT_DOUBLE_EQ(p.orientation.y, orientation.y);
+ EXPECT_DOUBLE_EQ(p.orientation.z, orientation.z);
+ EXPECT_DOUBLE_EQ(p.orientation.w, orientation.w);
+}
+
+TEST(geometry, set_longitudinal_velocity)
+{
+ using autoware_utils_geometry::set_longitudinal_velocity;
+
+ const double velocity = 1.0;
+
+ {
+ autoware_planning_msgs::msg::PathPoint p{};
+ set_longitudinal_velocity(velocity, p);
+ EXPECT_DOUBLE_EQ(p.longitudinal_velocity_mps, velocity);
+ }
+
+ {
+ autoware_planning_msgs::msg::TrajectoryPoint p{};
+ set_longitudinal_velocity(velocity, p);
+ EXPECT_DOUBLE_EQ(p.longitudinal_velocity_mps, velocity);
+ }
+}
+
+TEST(geometry, create_point)
+{
+ using autoware_utils_geometry::create_point;
+
+ const geometry_msgs::msg::Point p_out = create_point(1.0, 2.0, 3.0);
+ EXPECT_DOUBLE_EQ(p_out.x, 1.0);
+ EXPECT_DOUBLE_EQ(p_out.y, 2.0);
+ EXPECT_DOUBLE_EQ(p_out.z, 3.0);
+}
+
+TEST(geometry, create_quaternion)
+{
+ using autoware_utils_geometry::create_quaternion;
+
+ // (0.18257419, 0.36514837, 0.54772256, 0.73029674) is normalized quaternion of (1, 2, 3, 4)
+ const geometry_msgs::msg::Quaternion q_out =
+ create_quaternion(0.18257419, 0.36514837, 0.54772256, 0.73029674);
+ EXPECT_DOUBLE_EQ(q_out.x, 0.18257419);
+ EXPECT_DOUBLE_EQ(q_out.y, 0.36514837);
+ EXPECT_DOUBLE_EQ(q_out.z, 0.54772256);
+ EXPECT_DOUBLE_EQ(q_out.w, 0.73029674);
+}
+
+TEST(geometry, create_translation)
+{
+ using autoware_utils_geometry::create_translation;
+
+ const geometry_msgs::msg::Vector3 v_out = create_translation(1.0, 2.0, 3.0);
+ EXPECT_DOUBLE_EQ(v_out.x, 1.0);
+ EXPECT_DOUBLE_EQ(v_out.y, 2.0);
+ EXPECT_DOUBLE_EQ(v_out.z, 3.0);
+}
+
+TEST(geometry, create_quaternion_from_rpy)
+{
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_math::deg2rad;
+
+ {
+ const auto q_out = create_quaternion_from_rpy(0, 0, 0);
+ EXPECT_DOUBLE_EQ(q_out.x, 0.0);
+ EXPECT_DOUBLE_EQ(q_out.y, 0.0);
+ EXPECT_DOUBLE_EQ(q_out.z, 0.0);
+ EXPECT_DOUBLE_EQ(q_out.w, 1.0);
+ }
+
+ {
+ const auto q_out = create_quaternion_from_rpy(0, 0, deg2rad(90));
+ EXPECT_DOUBLE_EQ(q_out.x, 0.0);
+ EXPECT_DOUBLE_EQ(q_out.y, 0.0);
+ EXPECT_DOUBLE_EQ(q_out.z, 0.70710678118654757);
+ EXPECT_DOUBLE_EQ(q_out.w, 0.70710678118654757);
+ }
+
+ {
+ const auto q_out = create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+ EXPECT_DOUBLE_EQ(q_out.x, 0.17677669529663687);
+ EXPECT_DOUBLE_EQ(q_out.y, 0.30618621784789724);
+ EXPECT_DOUBLE_EQ(q_out.z, 0.17677669529663692);
+ EXPECT_DOUBLE_EQ(q_out.w, 0.91855865354369193);
+ }
+}
+
+TEST(geometry, create_quaternion_from_yaw)
+{
+ using autoware_utils_geometry::create_quaternion_from_yaw;
+ using autoware_utils_math::deg2rad;
+
+ {
+ const auto q_out = create_quaternion_from_yaw(0);
+ EXPECT_DOUBLE_EQ(q_out.x, 0.0);
+ EXPECT_DOUBLE_EQ(q_out.y, 0.0);
+ EXPECT_DOUBLE_EQ(q_out.z, 0.0);
+ EXPECT_DOUBLE_EQ(q_out.w, 1.0);
+ }
+
+ {
+ const auto q_out = create_quaternion_from_yaw(deg2rad(90));
+ EXPECT_DOUBLE_EQ(q_out.x, 0.0);
+ EXPECT_DOUBLE_EQ(q_out.y, 0.0);
+ EXPECT_DOUBLE_EQ(q_out.z, 0.70710678118654757);
+ EXPECT_DOUBLE_EQ(q_out.w, 0.70710678118654757);
+ }
+
+ {
+ const auto q_out = create_quaternion_from_yaw(deg2rad(30));
+ EXPECT_DOUBLE_EQ(q_out.x, 0.0);
+ EXPECT_DOUBLE_EQ(q_out.y, 0.0);
+ EXPECT_DOUBLE_EQ(q_out.z, 0.25881904510252074);
+ EXPECT_DOUBLE_EQ(q_out.w, 0.96592582628906831);
+ }
+}
+
+TEST(geometry, calc_elevation_angle)
+{
+ using autoware_utils_geometry::calc_elevation_angle;
+ using autoware_utils_geometry::create_point;
+ using autoware_utils_math::deg2rad;
+
+ {
+ const auto p1 = create_point(1.0, 1.0, 1.0);
+ const auto p2 = create_point(1.0, 1.0, -10.0);
+ EXPECT_NEAR(calc_elevation_angle(p1, p2), deg2rad(-90.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(0.0, 0.0, 0.0);
+ const auto p2 = create_point(1.0, 0.0, -std::sqrt(3.0));
+ EXPECT_NEAR(calc_elevation_angle(p1, p2), deg2rad(-60.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(0.0, 0.0, -1.0);
+ const auto p2 = create_point(0.0, 1.0, -2.0);
+ EXPECT_NEAR(calc_elevation_angle(p1, p2), deg2rad(-45.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(0.0, 0.0, 1.0);
+ const auto p2 = create_point(1.0, 1.0, 1.0);
+ EXPECT_NEAR(calc_elevation_angle(p1, p2), deg2rad(0.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(-100, -100, 0.0);
+ const auto p2 = create_point(0.0, 0.0, 0.0);
+ EXPECT_NEAR(calc_elevation_angle(p1, p2), deg2rad(0.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(0.0, 0.0, 1.0);
+ const auto p2 = create_point(0.0, 1.0, 2.0);
+ EXPECT_NEAR(calc_elevation_angle(p1, p2), deg2rad(45.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(0.0, 0.0, 0.0);
+ const auto p2 = create_point(1.0, 0.0, std::sqrt(3.0));
+ EXPECT_NEAR(calc_elevation_angle(p1, p2), deg2rad(60.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(1.0, 1.0, 1.0);
+ const auto p2 = create_point(1.0, 1.0, 10.0);
+ EXPECT_NEAR(calc_elevation_angle(p1, p2), deg2rad(90.0), epsilon);
+ }
+}
+
+TEST(geometry, calc_azimuth_angle)
+{
+ using autoware_utils_geometry::calc_azimuth_angle;
+ using autoware_utils_geometry::create_point;
+ using autoware_utils_math::deg2rad;
+
+ {
+ const auto p1 = create_point(0.0, 0.0, 9.0);
+ const auto p2 = create_point(-100, -epsilon, 0.0);
+ EXPECT_NEAR(calc_azimuth_angle(p1, p2), deg2rad(-180.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(0.0, 0.0, 2.0);
+ const auto p2 = create_point(-1.0, -1.0, 0.0);
+ EXPECT_NEAR(calc_azimuth_angle(p1, p2), deg2rad(-135.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(0.0, 10.0, 0.0);
+ const auto p2 = create_point(0.0, 0.0, 6.0);
+ EXPECT_NEAR(calc_azimuth_angle(p1, p2), deg2rad(-90.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(0.0, 0.0, 0.0);
+ const auto p2 = create_point(1.0, -1.0, 4.0);
+ EXPECT_NEAR(calc_azimuth_angle(p1, p2), deg2rad(-45.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(0.0, 1.0, 3.3);
+ const auto p2 = create_point(10.0, 1.0, -10.0);
+ EXPECT_NEAR(calc_azimuth_angle(p1, p2), deg2rad(0.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(0.0, 0.0, 2.0);
+ const auto p2 = create_point(1.0, 1.0, 0.0);
+ EXPECT_NEAR(calc_azimuth_angle(p1, p2), deg2rad(45.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(0.0, 0.0, 10.0);
+ const auto p2 = create_point(0.0, 10.0, 0.0);
+ EXPECT_NEAR(calc_azimuth_angle(p1, p2), deg2rad(90.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(0.0, 0.0, 2.0);
+ const auto p2 = create_point(-1.0, 1.0, 0.0);
+ EXPECT_NEAR(calc_azimuth_angle(p1, p2), deg2rad(135.0), epsilon);
+ }
+ {
+ const auto p1 = create_point(0.0, 0.0, 9.0);
+ const auto p2 = create_point(-100, epsilon, 0.0);
+ EXPECT_NEAR(calc_azimuth_angle(p1, p2), deg2rad(180.0), epsilon);
+ }
+}
+
+TEST(geometry, calc_distance2d)
+{
+ using autoware_utils_geometry::calc_distance2d;
+
+ geometry_msgs::msg::Point point;
+ point.x = 1.0;
+ point.y = 2.0;
+ point.z = 3.0;
+
+ geometry_msgs::msg::Pose pose;
+ pose.position.x = 5.0;
+ pose.position.y = 5.0;
+ pose.position.z = 4.0;
+
+ EXPECT_DOUBLE_EQ(calc_distance2d(point, pose), 5.0);
+}
+
+TEST(geometry, calc_squared_distance2d)
+{
+ using autoware_utils_geometry::calc_squared_distance2d;
+
+ geometry_msgs::msg::Point point;
+ point.x = 1.0;
+ point.y = 2.0;
+ point.z = 3.0;
+
+ geometry_msgs::msg::Pose pose;
+ pose.position.x = 5.0;
+ pose.position.y = 5.0;
+ pose.position.z = 4.0;
+
+ EXPECT_DOUBLE_EQ(calc_squared_distance2d(point, pose), 25.0);
+}
+
+TEST(geometry, calc_distance3d)
+{
+ using autoware_utils_geometry::calc_distance3d;
+
+ geometry_msgs::msg::Point point;
+ point.x = 1.0;
+ point.y = 2.0;
+ point.z = 3.0;
+
+ geometry_msgs::msg::Pose pose;
+ pose.position.x = 3.0;
+ pose.position.y = 4.0;
+ pose.position.z = 4.0;
+
+ EXPECT_DOUBLE_EQ(calc_distance3d(point, pose), 3.0);
+}
+
+TEST(geometry, get_rpy)
+{
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_geometry::get_rpy;
+ using autoware_utils_math::deg2rad;
+
+ {
+ const double ans_roll = deg2rad(5);
+ const double ans_pitch = deg2rad(10);
+ const double ans_yaw = deg2rad(15);
+ const auto quat = create_quaternion_from_rpy(ans_roll, ans_pitch, ans_yaw);
+ const auto rpy = get_rpy(quat);
+ EXPECT_NEAR(rpy.x, ans_roll, epsilon);
+ EXPECT_NEAR(rpy.y, ans_pitch, epsilon);
+ EXPECT_NEAR(rpy.z, ans_yaw, epsilon);
+ }
+ {
+ const double ans_roll = deg2rad(0);
+ const double ans_pitch = deg2rad(5);
+ const double ans_yaw = deg2rad(-10);
+ const auto quat = create_quaternion_from_rpy(ans_roll, ans_pitch, ans_yaw);
+ const auto rpy = get_rpy(quat);
+ EXPECT_NEAR(rpy.x, ans_roll, epsilon);
+ EXPECT_NEAR(rpy.y, ans_pitch, epsilon);
+ EXPECT_NEAR(rpy.z, ans_yaw, epsilon);
+ }
+ {
+ const double ans_roll = deg2rad(30);
+ const double ans_pitch = deg2rad(-20);
+ const double ans_yaw = deg2rad(0);
+ const auto quat = create_quaternion_from_rpy(ans_roll, ans_pitch, ans_yaw);
+ const auto rpy = get_rpy(quat);
+ EXPECT_NEAR(rpy.x, ans_roll, epsilon);
+ EXPECT_NEAR(rpy.y, ans_pitch, epsilon);
+ EXPECT_NEAR(rpy.z, ans_yaw, epsilon);
+ }
+}
+
+TEST(geometry, get_rpy_wrapper)
+{
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_geometry::get_rpy;
+ using autoware_utils_math::deg2rad;
+
+ {
+ const double ans_roll = deg2rad(45);
+ const double ans_pitch = deg2rad(25);
+ const double ans_yaw = deg2rad(-5);
+ const auto quat = create_quaternion_from_rpy(ans_roll, ans_pitch, ans_yaw);
+
+ // geometry_msgs::Pose
+ {
+ geometry_msgs::msg::Pose pose{};
+ pose.orientation = quat;
+ const auto rpy = get_rpy(pose);
+ EXPECT_NEAR(rpy.x, ans_roll, epsilon);
+ EXPECT_NEAR(rpy.y, ans_pitch, epsilon);
+ EXPECT_NEAR(rpy.z, ans_yaw, epsilon);
+ }
+ // geometry_msgs::PoseStamped
+ {
+ geometry_msgs::msg::PoseStamped pose{};
+ pose.pose.orientation = quat;
+ const auto rpy = get_rpy(pose);
+ EXPECT_NEAR(rpy.x, ans_roll, epsilon);
+ EXPECT_NEAR(rpy.y, ans_pitch, epsilon);
+ EXPECT_NEAR(rpy.z, ans_yaw, epsilon);
+ }
+ // geometry_msgs::PoseWithCovarianceStamped
+ {
+ geometry_msgs::msg::PoseWithCovarianceStamped pose{};
+ pose.pose.pose.orientation = quat;
+ const auto rpy = get_rpy(pose);
+ EXPECT_NEAR(rpy.x, ans_roll, epsilon);
+ EXPECT_NEAR(rpy.y, ans_pitch, epsilon);
+ EXPECT_NEAR(rpy.z, ans_yaw, epsilon);
+ }
+ }
+}
+
+TEST(geometry, transform2pose)
+{
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_geometry::transform2pose;
+ using autoware_utils_math::deg2rad;
+
+ {
+ geometry_msgs::msg::Transform transform;
+ transform.translation.x = 1.0;
+ transform.translation.y = 2.0;
+ transform.translation.z = 3.0;
+ transform.rotation = create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+
+ const geometry_msgs::msg::Pose pose = transform2pose(transform);
+
+ EXPECT_DOUBLE_EQ(transform.translation.x, pose.position.x);
+ EXPECT_DOUBLE_EQ(transform.translation.y, pose.position.y);
+ EXPECT_DOUBLE_EQ(transform.translation.z, pose.position.z);
+ EXPECT_DOUBLE_EQ(transform.rotation.x, pose.orientation.x);
+ EXPECT_DOUBLE_EQ(transform.rotation.y, pose.orientation.y);
+ EXPECT_DOUBLE_EQ(transform.rotation.z, pose.orientation.z);
+ EXPECT_DOUBLE_EQ(transform.rotation.w, pose.orientation.w);
+ }
+
+ {
+ geometry_msgs::msg::TransformStamped transform_stamped;
+ transform_stamped.header.frame_id = "test";
+ transform_stamped.header.stamp = rclcpp::Time(2.0);
+ transform_stamped.transform.translation.x = 1.0;
+ transform_stamped.transform.translation.y = 2.0;
+ transform_stamped.transform.translation.z = 3.0;
+ transform_stamped.transform.rotation =
+ create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+
+ const geometry_msgs::msg::PoseStamped pose_stamped = transform2pose(transform_stamped);
+
+ EXPECT_EQ(transform_stamped.header.frame_id, pose_stamped.header.frame_id);
+ EXPECT_DOUBLE_EQ(
+ rclcpp::Time(transform_stamped.header.stamp).seconds(),
+ rclcpp::Time(pose_stamped.header.stamp).seconds());
+
+ EXPECT_DOUBLE_EQ(transform_stamped.transform.translation.x, pose_stamped.pose.position.x);
+ EXPECT_DOUBLE_EQ(transform_stamped.transform.translation.y, pose_stamped.pose.position.y);
+ EXPECT_DOUBLE_EQ(transform_stamped.transform.translation.z, pose_stamped.pose.position.z);
+ EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.x, pose_stamped.pose.orientation.x);
+ EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.y, pose_stamped.pose.orientation.y);
+ EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.z, pose_stamped.pose.orientation.z);
+ EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.w, pose_stamped.pose.orientation.w);
+ }
+}
+
+TEST(geometry, pose2transform)
+{
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_geometry::pose2transform;
+ using autoware_utils_math::deg2rad;
+
+ {
+ geometry_msgs::msg::Pose pose;
+ pose.position.x = 1.0;
+ pose.position.y = 2.0;
+ pose.position.z = 3.0;
+ pose.orientation = create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+
+ const geometry_msgs::msg::Transform transform = pose2transform(pose);
+
+ EXPECT_DOUBLE_EQ(pose.position.x, transform.translation.x);
+ EXPECT_DOUBLE_EQ(pose.position.y, transform.translation.y);
+ EXPECT_DOUBLE_EQ(pose.position.z, transform.translation.z);
+ EXPECT_DOUBLE_EQ(pose.orientation.x, transform.rotation.x);
+ EXPECT_DOUBLE_EQ(pose.orientation.y, transform.rotation.y);
+ EXPECT_DOUBLE_EQ(pose.orientation.z, transform.rotation.z);
+ EXPECT_DOUBLE_EQ(pose.orientation.w, transform.rotation.w);
+ }
+
+ {
+ geometry_msgs::msg::PoseStamped pose_stamped;
+ pose_stamped.header.frame_id = "test";
+ pose_stamped.header.stamp = rclcpp::Time(2.0);
+ pose_stamped.pose.position.x = 1.0;
+ pose_stamped.pose.position.y = 2.0;
+ pose_stamped.pose.position.z = 3.0;
+ pose_stamped.pose.orientation =
+ create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+ const std::string child_frame_id = "child";
+
+ const geometry_msgs::msg::TransformStamped transform_stamped =
+ pose2transform(pose_stamped, child_frame_id);
+
+ EXPECT_EQ(pose_stamped.header.frame_id, transform_stamped.header.frame_id);
+ EXPECT_EQ(child_frame_id, transform_stamped.child_frame_id);
+ EXPECT_DOUBLE_EQ(
+ rclcpp::Time(pose_stamped.header.stamp).seconds(),
+ rclcpp::Time(transform_stamped.header.stamp).seconds());
+
+ EXPECT_DOUBLE_EQ(pose_stamped.pose.position.x, transform_stamped.transform.translation.x);
+ EXPECT_DOUBLE_EQ(pose_stamped.pose.position.y, transform_stamped.transform.translation.y);
+ EXPECT_DOUBLE_EQ(pose_stamped.pose.position.z, transform_stamped.transform.translation.z);
+ EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.x, transform_stamped.transform.rotation.x);
+ EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.y, transform_stamped.transform.rotation.y);
+ EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.z, transform_stamped.transform.rotation.z);
+ EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.w, transform_stamped.transform.rotation.w);
+ }
+}
+
+TEST(geometry, point_2_tf_vector)
+{
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_geometry::point_2_tf_vector;
+ using autoware_utils_math::deg2rad;
+
+ // Point
+ {
+ geometry_msgs::msg::Point src;
+ src.x = 1.0;
+ src.y = 2.0;
+ src.z = 3.0;
+
+ geometry_msgs::msg::Point dst;
+ dst.x = 10.0;
+ dst.y = 5.0;
+ dst.z = -5.0;
+
+ const auto vec = point_2_tf_vector(src, dst);
+
+ EXPECT_DOUBLE_EQ(vec.x(), 9.0);
+ EXPECT_DOUBLE_EQ(vec.y(), 3.0);
+ EXPECT_DOUBLE_EQ(vec.z(), -8.0);
+ }
+
+ // Pose
+ {
+ geometry_msgs::msg::Pose src;
+ src.position.x = 1.0;
+ src.position.y = 2.0;
+ src.position.z = 3.0;
+ src.orientation = create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+
+ geometry_msgs::msg::Pose dst;
+ dst.position.x = 10.0;
+ dst.position.y = 5.0;
+ dst.position.z = -5.0;
+ dst.orientation = create_quaternion_from_rpy(deg2rad(10), deg2rad(10), deg2rad(10));
+
+ const auto vec = point_2_tf_vector(src, dst);
+
+ EXPECT_DOUBLE_EQ(vec.x(), 9.0);
+ EXPECT_DOUBLE_EQ(vec.y(), 3.0);
+ EXPECT_DOUBLE_EQ(vec.z(), -8.0);
+ }
+
+ // Point and Pose
+ {
+ geometry_msgs::msg::Point src;
+ src.x = 1.0;
+ src.y = 2.0;
+ src.z = 3.0;
+
+ geometry_msgs::msg::Pose dst;
+ dst.position.x = 10.0;
+ dst.position.y = 5.0;
+ dst.position.z = -5.0;
+ dst.orientation = create_quaternion_from_rpy(deg2rad(10), deg2rad(10), deg2rad(10));
+
+ const auto vec = point_2_tf_vector(src, dst);
+
+ EXPECT_DOUBLE_EQ(vec.x(), 9.0);
+ EXPECT_DOUBLE_EQ(vec.y(), 3.0);
+ EXPECT_DOUBLE_EQ(vec.z(), -8.0);
+ }
+}
+
+TEST(geometry, transform_point)
+{
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_geometry::Point2d;
+ using autoware_utils_geometry::Point3d;
+ using autoware_utils_geometry::transform_point;
+ using autoware_utils_math::deg2rad;
+
+ {
+ const Point2d p(1.0, 2.0);
+
+ geometry_msgs::msg::Transform transform;
+ transform.translation.x = 1.0;
+ transform.translation.y = 2.0;
+ transform.rotation = create_quaternion_from_rpy(0, 0, deg2rad(30));
+
+ const Point2d p_transformed = transform_point(p, transform);
+
+ EXPECT_DOUBLE_EQ(p_transformed.x(), 0.86602540378443882);
+ EXPECT_DOUBLE_EQ(p_transformed.y(), 4.2320508075688767);
+ }
+
+ {
+ const Point3d p(1.0, 2.0, 3.0);
+
+ geometry_msgs::msg::Transform transform;
+ transform.translation.x = 1.0;
+ transform.translation.y = 2.0;
+ transform.translation.z = 3.0;
+ transform.rotation = create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+
+ const Point3d p_transformed = transform_point(p, 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);
+ }
+
+ {
+ 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 = create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+
+ const Eigen::Vector3d p_transformed = transform_point(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 = create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+
+ const geometry_msgs::msg::Point p_transformed = transform_point(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::Point32 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 = create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+
+ const geometry_msgs::msg::Point32 p_transformed = transform_point(p, pose_transform);
+
+ EXPECT_DOUBLE_EQ(p_transformed.x, 3.1919872760772705);
+ EXPECT_DOUBLE_EQ(p_transformed.y, 3.5334937572479248);
+ EXPECT_DOUBLE_EQ(p_transformed.z, 5.616025447845459);
+ }
+}
+
+TEST(geometry, transform_pose)
+{
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_geometry::transform_pose;
+ using autoware_utils_math::deg2rad;
+
+ geometry_msgs::msg::Pose pose;
+ pose.position.x = 2.0;
+ pose.position.y = 4.0;
+ pose.position.z = 6.0;
+ pose.orientation = create_quaternion_from_rpy(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 = create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+
+ const geometry_msgs::msg::Pose pose_transformed = transform_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);
+ }
+
+ // 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 = create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+
+ const geometry_msgs::msg::Pose pose_transformed = transform_pose(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, inverse_transform_pose)
+{
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_geometry::inverse_transform_pose;
+ using autoware_utils_math::deg2rad;
+
+ geometry_msgs::msg::Pose pose;
+ pose.position.x = 2.0;
+ pose.position.y = 4.0;
+ pose.position.z = 6.0;
+ pose.orientation = create_quaternion_from_rpy(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 = create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+
+ const geometry_msgs::msg::Pose pose_transformed = inverse_transform_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);
+ }
+
+ // 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 = create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+
+ const geometry_msgs::msg::Pose pose_transformed = inverse_transform_pose(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, inverse_transform_point)
+{
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_geometry::inverse_transform_point;
+ using autoware_utils_geometry::inverse_transform_pose;
+ using autoware_utils_math::deg2rad;
+
+ 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 = create_quaternion_from_rpy(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 = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(0));
+ const geometry_msgs::msg::Pose pose_transformed = inverse_transform_pose(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 = inverse_transform_point(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, transform_vector)
+{
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_geometry::MultiPoint3d;
+ using autoware_utils_geometry::transform_vector;
+ using autoware_utils_math::deg2rad;
+
+ {
+ const MultiPoint3d ps{{1.0, 2.0, 3.0}, {2.0, 3.0, 4.0}};
+
+ geometry_msgs::msg::Transform transform;
+ transform.translation.x = 1.0;
+ transform.translation.y = 2.0;
+ transform.translation.z = 3.0;
+ transform.rotation = create_quaternion_from_rpy(deg2rad(30), deg2rad(30), deg2rad(30));
+
+ const MultiPoint3d ps_transformed = transform_vector(ps, transform);
+
+ EXPECT_DOUBLE_EQ(ps_transformed.at(0).x(), 3.1919872981077804);
+ EXPECT_DOUBLE_EQ(ps_transformed.at(0).y(), 3.5334936490538906);
+ EXPECT_DOUBLE_EQ(ps_transformed.at(0).z(), 5.6160254037844393);
+
+ EXPECT_DOUBLE_EQ(ps_transformed.at(1).x(), 4.350480947161671);
+ EXPECT_DOUBLE_EQ(ps_transformed.at(1).y(), 4.625);
+ EXPECT_DOUBLE_EQ(ps_transformed.at(1).z(), 6.299038105676658);
+ }
+}
+
+TEST(geometry, calc_curvature)
+{
+ using autoware_utils_geometry::calc_curvature;
+ // Straight Line
+ {
+ geometry_msgs::msg::Point p1 = autoware_utils_geometry::create_point(0.0, 0.0, 0.0);
+ geometry_msgs::msg::Point p2 = autoware_utils_geometry::create_point(1.0, 0.0, 0.0);
+ geometry_msgs::msg::Point p3 = autoware_utils_geometry::create_point(2.0, 0.0, 0.0);
+ EXPECT_DOUBLE_EQ(calc_curvature(p1, p2, p3), 0.0);
+ }
+
+ // Clockwise Curved Road with a 1[m] radius
+ {
+ geometry_msgs::msg::Point p1 = autoware_utils_geometry::create_point(0.0, 0.0, 0.0);
+ geometry_msgs::msg::Point p2 = autoware_utils_geometry::create_point(1.0, 1.0, 0.0);
+ geometry_msgs::msg::Point p3 = autoware_utils_geometry::create_point(2.0, 0.0, 0.0);
+ EXPECT_DOUBLE_EQ(calc_curvature(p1, p2, p3), -1.0);
+ }
+
+ // Clockwise Curved Road with a 5[m] radius
+ {
+ geometry_msgs::msg::Point p1 = autoware_utils_geometry::create_point(0.0, 0.0, 0.0);
+ geometry_msgs::msg::Point p2 = autoware_utils_geometry::create_point(5.0, 5.0, 0.0);
+ geometry_msgs::msg::Point p3 = autoware_utils_geometry::create_point(10.0, 0.0, 0.0);
+ EXPECT_DOUBLE_EQ(calc_curvature(p1, p2, p3), -0.2);
+ }
+
+ // Counter-Clockwise Curved Road with a 1[m] radius
+ {
+ geometry_msgs::msg::Point p1 = autoware_utils_geometry::create_point(0.0, 0.0, 0.0);
+ geometry_msgs::msg::Point p2 = autoware_utils_geometry::create_point(-1.0, 1.0, 0.0);
+ geometry_msgs::msg::Point p3 = autoware_utils_geometry::create_point(-2.0, 0.0, 0.0);
+ EXPECT_DOUBLE_EQ(calc_curvature(p1, p2, p3), 1.0);
+ }
+
+ // Counter-Clockwise Curved Road with a 5[m] radius
+ {
+ geometry_msgs::msg::Point p1 = autoware_utils_geometry::create_point(0.0, 0.0, 0.0);
+ geometry_msgs::msg::Point p2 = autoware_utils_geometry::create_point(-5.0, 5.0, 0.0);
+ geometry_msgs::msg::Point p3 = autoware_utils_geometry::create_point(-10.0, 0.0, 0.0);
+ EXPECT_DOUBLE_EQ(calc_curvature(p1, p2, p3), 0.2);
+ }
+
+ // Give same points
+ {
+ geometry_msgs::msg::Point p1 = autoware_utils_geometry::create_point(0.0, 0.0, 0.0);
+ geometry_msgs::msg::Point p2 = autoware_utils_geometry::create_point(1.0, 0.0, 0.0);
+ ASSERT_ANY_THROW(calc_curvature(p1, p1, p1));
+ ASSERT_ANY_THROW(calc_curvature(p1, p1, p2));
+ ASSERT_ANY_THROW(calc_curvature(p1, p2, p1));
+ ASSERT_ANY_THROW(calc_curvature(p1, p2, p2));
+ }
+}
+
+TEST(geometry, calc_offset_pose)
+{
+ using autoware_utils_geometry::calc_offset_pose;
+ using autoware_utils_geometry::create_point;
+ using autoware_utils_geometry::create_quaternion;
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_math::deg2rad;
+
+ // Only translation
+ {
+ geometry_msgs::msg::Pose p_in;
+ p_in.position = create_point(1.0, 2.0, 3.0);
+ p_in.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0);
+
+ const auto p_out = calc_offset_pose(p_in, 1.0, 1.0, 1.0);
+
+ EXPECT_DOUBLE_EQ(p_out.position.x, 2.0);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 3.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 4.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0);
+ }
+
+ {
+ geometry_msgs::msg::Pose p_in;
+ p_in.position = create_point(2.0, 3.0, 1.0);
+ p_in.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(90));
+
+ const auto p_out = calc_offset_pose(p_in, 2.0, 1.0, 3.0);
+
+ EXPECT_DOUBLE_EQ(p_out.position.x, 1.0);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 5.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 4.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.70710678118654757);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, 0.70710678118654757);
+ }
+
+ {
+ geometry_msgs::msg::Pose p_in;
+ p_in.position = create_point(2.0, 1.0, 1.0);
+ p_in.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(30));
+
+ const auto p_out = calc_offset_pose(p_in, 2.0, 0.0, -1.0);
+
+ EXPECT_DOUBLE_EQ(p_out.position.x, 3.73205080756887729);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 2.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.25881904510252068);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, 0.96592582628906831);
+ }
+
+ // Only rotation
+ {
+ geometry_msgs::msg::Pose p_in;
+ p_in.position = create_point(2.0, 1.0, 1.0);
+ p_in.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(30));
+
+ const auto p_out = calc_offset_pose(p_in, 0.0, 0.0, 0.0, deg2rad(20));
+
+ EXPECT_DOUBLE_EQ(p_out.position.x, 2.0);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 1.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 1.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+ EXPECT_NEAR(p_out.orientation.z, 0.42261826174069944, epsilon);
+ EXPECT_NEAR(p_out.orientation.w, 0.9063077870366499, epsilon);
+ }
+
+ // Both translation and rotation
+ {
+ geometry_msgs::msg::Pose p_in;
+ p_in.position = create_point(2.0, 1.0, 1.0);
+ p_in.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(30));
+
+ const auto p_out = calc_offset_pose(p_in, 2.0, 0.0, -1.0, deg2rad(20));
+
+ EXPECT_DOUBLE_EQ(p_out.position.x, 3.73205080756887729);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 2.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+ EXPECT_NEAR(p_out.orientation.z, 0.42261826174069944, epsilon);
+ EXPECT_NEAR(p_out.orientation.w, 0.9063077870366499, epsilon);
+ }
+}
+
+TEST(geometry, is_driving_forward)
+{
+ using autoware_utils_geometry::calc_interpolated_point;
+ using autoware_utils_geometry::create_point;
+ using autoware_utils_geometry::create_quaternion;
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_geometry::is_driving_forward;
+ using autoware_utils_math::deg2rad;
+
+ const double epsilon = 1e-3;
+
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0);
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(3.0, 0.0, 0.0);
+ dst_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0);
+
+ EXPECT_TRUE(is_driving_forward(src_pose, dst_pose));
+ }
+
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion_from_rpy(0.0, 0.0, deg2rad(180));
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(3.0, 0.0, 0.0);
+ dst_pose.orientation = create_quaternion_from_rpy(0.0, 0.0, deg2rad(180));
+
+ EXPECT_FALSE(is_driving_forward(src_pose, dst_pose));
+ }
+
+ // Boundary Condition
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion_from_rpy(0.0, 0.0, deg2rad(90));
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(3.0, 0.0, 0.0);
+ dst_pose.orientation = create_quaternion_from_rpy(0.0, 0.0, deg2rad(90));
+
+ EXPECT_TRUE(is_driving_forward(src_pose, dst_pose));
+ }
+
+ // Boundary Condition
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion_from_rpy(0.0, 0.0, deg2rad(90 + epsilon));
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(3.0, 0.0, 0.0);
+ dst_pose.orientation = create_quaternion_from_rpy(0.0, 0.0, deg2rad(90 + epsilon));
+
+ EXPECT_FALSE(is_driving_forward(src_pose, dst_pose));
+ }
+}
+
+TEST(geometry, calc_interpolated_point)
+{
+ using autoware_utils_geometry::calc_interpolated_point;
+ using autoware_utils_geometry::create_point;
+
+ {
+ const auto src_point = create_point(0.0, 0.0, 0.0);
+ const auto dst_point = create_point(3.0, 0.0, 0.0);
+
+ const double epsilon = 1e-3;
+ for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+ const auto p_out = calc_interpolated_point(src_point, dst_point, ratio);
+
+ EXPECT_DOUBLE_EQ(p_out.x, 3.0 * ratio);
+ EXPECT_DOUBLE_EQ(p_out.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.z, 0.0);
+ }
+ }
+
+ // Same points are given
+ {
+ const auto src_point = create_point(0.0, 0.0, 0.0);
+ const auto dst_point = create_point(0.0, 0.0, 0.0);
+
+ const double epsilon = 1e-3;
+ for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+ const auto p_out = calc_interpolated_point(src_point, dst_point, ratio);
+
+ EXPECT_DOUBLE_EQ(p_out.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.z, 0.0);
+ }
+ }
+
+ // Boundary Condition (Negative Ratio)
+ {
+ const auto src_point = create_point(0.0, 0.0, 0.0);
+ const auto dst_point = create_point(3.0, 0.0, 0.0);
+
+ const auto p_out = calc_interpolated_point(src_point, dst_point, -10.0);
+ EXPECT_DOUBLE_EQ(p_out.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.z, 0.0);
+ }
+
+ // Boundary Condition (Positive Ratio larger than 1.0)
+ {
+ const auto src_point = create_point(0.0, 0.0, 0.0);
+ const auto dst_point = create_point(3.0, 0.0, 0.0);
+
+ const auto p_out = calc_interpolated_point(src_point, dst_point, 10.0);
+ EXPECT_DOUBLE_EQ(p_out.x, 3.0);
+ EXPECT_DOUBLE_EQ(p_out.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.z, 0.0);
+ }
+}
+
+TEST(geometry, calc_interpolated_pose)
+{
+ using autoware_utils_geometry::calc_interpolated_pose;
+ using autoware_utils_geometry::create_point;
+ using autoware_utils_geometry::create_quaternion;
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_math::deg2rad;
+ const double epsilon = 1e-3;
+
+ // Position Interpolation
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0);
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(3.0, 0.0, 0.0);
+ dst_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0);
+
+ for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+ const auto p_out = calc_interpolated_pose(src_pose, dst_pose, ratio);
+
+ EXPECT_DOUBLE_EQ(p_out.position.x, 3.0 * ratio);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0);
+ }
+ }
+
+ // Boundary Condition (Negative Ratio)
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(0));
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(1.0, 1.0, 0.0);
+ dst_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(60));
+
+ const auto p_out = calc_interpolated_pose(src_pose, dst_pose, -10.0);
+
+ const auto ans_quat = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(45));
+ EXPECT_DOUBLE_EQ(p_out.position.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
+ }
+
+ // Boundary Condition (Positive Ratio larger than 1.0)
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(0));
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(1.0, 1.0, 0.0);
+ dst_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(60));
+
+ const auto p_out = calc_interpolated_pose(src_pose, dst_pose, 10.0);
+
+ const auto ans_quat = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(60));
+ EXPECT_DOUBLE_EQ(p_out.position.x, 1.0);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 1.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
+ }
+
+ // Quaternion Interpolation
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(0));
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(1.0, 1.0, 0.0);
+ dst_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(60));
+
+ for (double ratio = 0.0; ratio < 1.0 - epsilon; ratio += 0.1) {
+ const auto p_out = calc_interpolated_pose(src_pose, dst_pose, ratio);
+
+ const auto ans_quat = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(45));
+ EXPECT_DOUBLE_EQ(p_out.position.x, 1.0 * ratio);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 1.0 * ratio);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
+ }
+
+ // Boundary Condition (ratio = 1.0)
+ const auto p_out = calc_interpolated_pose(src_pose, dst_pose, 1.0);
+
+ const auto ans_quat = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(60));
+ EXPECT_DOUBLE_EQ(p_out.position.x, 1.0);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 1.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
+ }
+
+ // Same poses are given
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(0));
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(0.0, 0.0, 0.0);
+ dst_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(0));
+
+ for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+ const auto p_out = calc_interpolated_pose(src_pose, dst_pose, ratio);
+
+ EXPECT_DOUBLE_EQ(p_out.position.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0);
+ }
+ }
+
+ // Same points are given (different orientation)
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(0));
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(0.0, 0.0, 0.0);
+ dst_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(45));
+
+ for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+ const auto p_out = calc_interpolated_pose(src_pose, dst_pose, ratio);
+
+ const auto ans_quat = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(45));
+ EXPECT_DOUBLE_EQ(p_out.position.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
+ }
+ }
+
+ // Driving Backward
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(180));
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(5.0, 0.0, 0.0);
+ dst_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(180));
+
+ for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+ const auto p_out = calc_interpolated_pose(src_pose, dst_pose, ratio);
+
+ const auto ans_quat = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(180));
+ EXPECT_DOUBLE_EQ(p_out.position.x, 5.0 * ratio);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
+ }
+ }
+}
+
+TEST(geometry, calc_interpolated_pose_with_Spherical_Interpolation)
+{
+ using autoware_utils_geometry::calc_interpolated_pose;
+ using autoware_utils_geometry::create_point;
+ using autoware_utils_geometry::create_quaternion;
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_math::deg2rad;
+ const double epsilon = 1e-3;
+
+ // Position Interpolation
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0);
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(3.0, 0.0, 0.0);
+ dst_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0);
+
+ for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+ const auto p_out = calc_interpolated_pose(src_pose, dst_pose, ratio, false);
+
+ EXPECT_DOUBLE_EQ(p_out.position.x, 3.0 * ratio);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0);
+ }
+ }
+
+ // Boundary Condition (Negative Ratio)
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(0));
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(1.0, 1.0, 0.0);
+ dst_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(60));
+
+ const auto p_out = calc_interpolated_pose(src_pose, dst_pose, -10.0, false);
+
+ const auto ans_quat = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(0));
+ EXPECT_DOUBLE_EQ(p_out.position.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
+ }
+
+ // Boundary Condition (Positive Ratio larger than 1.0)
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(0));
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(1.0, 1.0, 0.0);
+ dst_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(60));
+
+ const auto p_out = calc_interpolated_pose(src_pose, dst_pose, 10.0, false);
+
+ const auto ans_quat = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(60));
+ EXPECT_DOUBLE_EQ(p_out.position.x, 1.0);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 1.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
+ }
+
+ // Quaternion Interpolation1
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(0));
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(0.0, 0.0, 0.0);
+ dst_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(90));
+
+ for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+ const auto p_out = calc_interpolated_pose(src_pose, dst_pose, ratio, false);
+
+ const auto ans_quat = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(90 * ratio));
+ EXPECT_DOUBLE_EQ(p_out.position.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
+ }
+ }
+
+ // Quaternion Interpolation2
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(0));
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(1.0, 1.0, 0.0);
+ dst_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(60));
+
+ for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+ const auto p_out = calc_interpolated_pose(src_pose, dst_pose, ratio, false);
+
+ const auto ans_quat = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(60 * ratio));
+ EXPECT_DOUBLE_EQ(p_out.position.x, 1.0 * ratio);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 1.0 * ratio);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
+ }
+ }
+
+ // Same points are given
+ {
+ geometry_msgs::msg::Pose src_pose;
+ src_pose.position = create_point(0.0, 0.0, 0.0);
+ src_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(0));
+
+ geometry_msgs::msg::Pose dst_pose;
+ dst_pose.position = create_point(0.0, 0.0, 0.0);
+ dst_pose.orientation = create_quaternion_from_rpy(deg2rad(0), deg2rad(0), deg2rad(0));
+
+ for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+ const auto p_out = calc_interpolated_pose(src_pose, dst_pose, ratio, false);
+
+ EXPECT_DOUBLE_EQ(p_out.position.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0);
+ }
+ }
+}
+
+TEST(geometry, getTwist)
+{
+ using autoware_utils_geometry::create_vector3;
+
+ geometry_msgs::msg::Vector3 velocity = create_vector3(1.0, 2.0, 3.0);
+ geometry_msgs::msg::Vector3 angular = create_vector3(0.1, 0.2, 0.3);
+
+ geometry_msgs::msg::Twist twist = geometry_msgs::build()
+ .linear(create_vector3(1.0, 2.0, 3.0))
+ .angular(create_vector3(0.1, 0.2, 0.3));
+
+ geometry_msgs::msg::TwistWithCovariance twist_with_covariance;
+ twist_with_covariance.twist = geometry_msgs::build()
+ .linear(create_vector3(1.0, 2.0, 3.0))
+ .angular(create_vector3(0.1, 0.2, 0.3));
+
+ // getTwist
+ {
+ auto t_out = autoware_utils_geometry::create_twist(velocity, angular);
+ EXPECT_DOUBLE_EQ(t_out.linear.x, twist.linear.x);
+ EXPECT_DOUBLE_EQ(t_out.linear.y, twist.linear.y);
+ EXPECT_DOUBLE_EQ(t_out.linear.z, twist.linear.z);
+ EXPECT_DOUBLE_EQ(t_out.angular.x, twist.angular.x);
+ EXPECT_DOUBLE_EQ(t_out.angular.y, twist.angular.y);
+ EXPECT_DOUBLE_EQ(t_out.angular.z, twist.angular.z);
+ }
+}
+
+TEST(geometry, getTwistNorm)
+{
+ using autoware_utils_geometry::create_vector3;
+ geometry_msgs::msg::TwistWithCovariance twist_with_covariance;
+ twist_with_covariance.twist = geometry_msgs::build()
+ .linear(create_vector3(3.0, 4.0, 0.0))
+ .angular(create_vector3(0.1, 0.2, 0.3));
+ EXPECT_NEAR(autoware_utils_geometry::calc_norm(twist_with_covariance.twist.linear), 5.0, epsilon);
+}
+
+TEST(geometry, is_twist_covariance_valid)
+{
+ using autoware_utils_geometry::create_vector3;
+ geometry_msgs::msg::TwistWithCovariance twist_with_covariance;
+ twist_with_covariance.twist = geometry_msgs::build()
+ .linear(create_vector3(1.0, 2.0, 3.0))
+ .angular(create_vector3(0.1, 0.2, 0.3));
+
+ EXPECT_EQ(autoware_utils_geometry::is_twist_covariance_valid(twist_with_covariance), false);
+
+ twist_with_covariance.covariance.at(0) = 1.0;
+ EXPECT_EQ(autoware_utils_geometry::is_twist_covariance_valid(twist_with_covariance), true);
+}
+
+TEST(geometry, intersect)
+{
+ using autoware_utils_geometry::create_point;
+ using autoware_utils_geometry::intersect;
+
+ { // Normally crossing
+ const auto p1 = create_point(0.0, -1.0, 0.0);
+ const auto p2 = create_point(0.0, 1.0, 0.0);
+ const auto p3 = create_point(-1.0, 0.0, 0.0);
+ const auto p4 = create_point(1.0, 0.0, 0.0);
+ const auto result = intersect(p1, p2, p3, p4);
+
+ EXPECT_TRUE(result);
+ EXPECT_NEAR(result->x, 0.0, epsilon);
+ EXPECT_NEAR(result->y, 0.0, epsilon);
+ EXPECT_NEAR(result->z, 0.0, epsilon);
+ }
+
+ { // No crossing
+ const auto p1 = create_point(0.0, -1.0, 0.0);
+ const auto p2 = create_point(0.0, 1.0, 0.0);
+ const auto p3 = create_point(1.0, 0.0, 0.0);
+ const auto p4 = create_point(3.0, 0.0, 0.0);
+ const auto result = intersect(p1, p2, p3, p4);
+
+ EXPECT_FALSE(result);
+ }
+
+ { // One segment is the point on the other's segment
+ const auto p1 = create_point(0.0, -1.0, 0.0);
+ const auto p2 = create_point(0.0, 1.0, 0.0);
+ const auto p3 = create_point(0.0, 0.0, 0.0);
+ const auto p4 = create_point(0.0, 0.0, 0.0);
+ const auto result = intersect(p1, p2, p3, p4);
+
+ EXPECT_FALSE(result);
+ }
+
+ { // One segment is the point not on the other's segment
+ const auto p1 = create_point(0.0, -1.0, 0.0);
+ const auto p2 = create_point(0.0, 1.0, 0.0);
+ const auto p3 = create_point(1.0, 0.0, 0.0);
+ const auto p4 = create_point(1.0, 0.0, 0.0);
+ const auto result = intersect(p1, p2, p3, p4);
+
+ EXPECT_FALSE(result);
+ }
+
+ { // Both segments are the points which are the same position
+ const auto p1 = create_point(0.0, 0.0, 0.0);
+ const auto p2 = create_point(0.0, 0.0, 0.0);
+ const auto p3 = create_point(0.0, 0.0, 0.0);
+ const auto p4 = create_point(0.0, 0.0, 0.0);
+ const auto result = intersect(p1, p2, p3, p4);
+
+ EXPECT_FALSE(result);
+ }
+
+ { // Both segments are the points which are different position
+ const auto p1 = create_point(0.0, 1.0, 0.0);
+ const auto p2 = create_point(0.0, 1.0, 0.0);
+ const auto p3 = create_point(1.0, 0.0, 0.0);
+ const auto p4 = create_point(1.0, 0.0, 0.0);
+ const auto result = intersect(p1, p2, p3, p4);
+
+ EXPECT_FALSE(result);
+ }
+
+ { // Segments are the same
+ const auto p1 = create_point(0.0, -1.0, 0.0);
+ const auto p2 = create_point(0.0, 1.0, 0.0);
+ const auto p3 = create_point(0.0, -1.0, 0.0);
+ const auto p4 = create_point(0.0, 1.0, 0.0);
+ const auto result = intersect(p1, p2, p3, p4);
+
+ EXPECT_FALSE(result);
+ }
+
+ { // One's edge is on the other's segment
+ const auto p1 = create_point(0.0, -1.0, 0.0);
+ const auto p2 = create_point(0.0, 1.0, 0.0);
+ const auto p3 = create_point(0.0, 0.0, 0.0);
+ const auto p4 = create_point(1.0, 0.0, 0.0);
+ const auto result = intersect(p1, p2, p3, p4);
+
+ EXPECT_TRUE(result);
+ EXPECT_NEAR(result->x, 0.0, epsilon);
+ EXPECT_NEAR(result->y, 0.0, epsilon);
+ EXPECT_NEAR(result->z, 0.0, epsilon);
+ }
+
+ { // One's edge is the same as the other's edge.
+ const auto p1 = create_point(0.0, -1.0, 0.0);
+ const auto p2 = create_point(0.0, 1.0, 0.0);
+ const auto p3 = create_point(0.0, -1.0, 0.0);
+ const auto p4 = create_point(2.0, -1.0, 0.0);
+ const auto result = intersect(p1, p2, p3, p4);
+
+ EXPECT_TRUE(result);
+ EXPECT_NEAR(result->x, 0.0, epsilon);
+ EXPECT_NEAR(result->y, -1.0, epsilon);
+ EXPECT_NEAR(result->z, 0.0, epsilon);
+ }
+}
+
+TEST(
+ geometry,
+ DISABLED_intersectPolygon) // GJK give different result for edge test (point sharing and edge
+ // sharing) compared to SAT and boost::geometry::intersect
+{
+ { // 2 triangles with intersection
+ autoware_utils_geometry::Polygon2d poly1;
+ autoware_utils_geometry::Polygon2d poly2;
+ poly1.outer().emplace_back(0, 2);
+ poly1.outer().emplace_back(2, 2);
+ poly1.outer().emplace_back(2, 0);
+ poly2.outer().emplace_back(1, 1);
+ poly2.outer().emplace_back(1, 0);
+ poly2.outer().emplace_back(0, 1);
+ boost::geometry::correct(poly1);
+ boost::geometry::correct(poly2);
+ EXPECT_TRUE(autoware_utils_geometry::intersects_convex(poly1, poly2));
+ EXPECT_TRUE(autoware_utils_geometry::sat::intersects(poly1, poly2));
+ }
+ { // 2 triangles with no intersection (but they share an edge)
+ autoware_utils_geometry::Polygon2d poly1;
+ autoware_utils_geometry::Polygon2d poly2;
+ poly1.outer().emplace_back(0, 2);
+ poly1.outer().emplace_back(2, 2);
+ poly1.outer().emplace_back(0, 0);
+ poly2.outer().emplace_back(0, 0);
+ poly2.outer().emplace_back(2, 0);
+ poly2.outer().emplace_back(2, 2);
+ boost::geometry::correct(poly1);
+ boost::geometry::correct(poly2);
+ EXPECT_FALSE(autoware_utils_geometry::intersects_convex(poly1, poly2));
+ EXPECT_FALSE(autoware_utils_geometry::sat::intersects(poly1, poly2));
+ }
+ { // 2 triangles with no intersection (but they share a point)
+ autoware_utils_geometry::Polygon2d poly1;
+ autoware_utils_geometry::Polygon2d poly2;
+ poly1.outer().emplace_back(0, 2);
+ poly1.outer().emplace_back(2, 2);
+ poly1.outer().emplace_back(0, 0);
+ poly2.outer().emplace_back(4, 4);
+ poly2.outer().emplace_back(4, 2);
+ poly2.outer().emplace_back(2, 2);
+ boost::geometry::correct(poly1);
+ boost::geometry::correct(poly2);
+ EXPECT_FALSE(autoware_utils_geometry::intersects_convex(poly1, poly2));
+ EXPECT_FALSE(autoware_utils_geometry::sat::intersects(poly1, poly2));
+ }
+ { // 2 triangles sharing a point and then with very small intersection
+ autoware_utils_geometry::Polygon2d poly1;
+ autoware_utils_geometry::Polygon2d poly2;
+ poly1.outer().emplace_back(0, 0);
+ poly1.outer().emplace_back(2, 2);
+ poly1.outer().emplace_back(4, 0);
+ poly2.outer().emplace_back(0, 4);
+ poly2.outer().emplace_back(2, 2);
+ poly2.outer().emplace_back(4, 4);
+ boost::geometry::correct(poly1);
+ boost::geometry::correct(poly2);
+ EXPECT_FALSE(autoware_utils_geometry::intersects_convex(poly1, poly2));
+ poly1.outer()[1].y() += 1e-12;
+ EXPECT_TRUE(autoware_utils_geometry::intersects_convex(poly1, poly2));
+ EXPECT_TRUE(autoware_utils_geometry::sat::intersects(poly1, poly2));
+ }
+ { // 2 triangles with no intersection and no touching
+ autoware_utils_geometry::Polygon2d poly1;
+ autoware_utils_geometry::Polygon2d poly2;
+ poly1.outer().emplace_back(0, 2);
+ poly1.outer().emplace_back(2, 2);
+ poly1.outer().emplace_back(0, 0);
+ poly2.outer().emplace_back(4, 4);
+ poly2.outer().emplace_back(5, 5);
+ poly2.outer().emplace_back(3, 5);
+ boost::geometry::correct(poly1);
+ boost::geometry::correct(poly2);
+ EXPECT_FALSE(autoware_utils_geometry::intersects_convex(poly1, poly2));
+ EXPECT_FALSE(autoware_utils_geometry::sat::intersects(poly1, poly2));
+ }
+ { // triangle and quadrilateral with intersection
+ autoware_utils_geometry::Polygon2d poly1;
+ autoware_utils_geometry::Polygon2d poly2;
+ poly1.outer().emplace_back(4, 11);
+ poly1.outer().emplace_back(4, 5);
+ poly1.outer().emplace_back(9, 9);
+ poly2.outer().emplace_back(5, 7);
+ poly2.outer().emplace_back(7, 3);
+ poly2.outer().emplace_back(10, 2);
+ poly2.outer().emplace_back(12, 7);
+ boost::geometry::correct(poly1);
+ boost::geometry::correct(poly2);
+ EXPECT_TRUE(autoware_utils_geometry::intersects_convex(poly1, poly2));
+ EXPECT_TRUE(autoware_utils_geometry::sat::intersects(poly1, poly2));
+ }
+}
+
+TEST(geometry, intersectPolygonRand)
+{
+ std::vector polygons;
+ constexpr auto polygons_nb = 100;
+ constexpr auto max_vertices = 10;
+ constexpr auto max_values = 1000;
+
+ autoware_utils_system::StopWatch sw;
+
+ for (auto vertices = 3UL; vertices < max_vertices; ++vertices) {
+ double ground_truth_intersect_ns = 0.0;
+ double ground_truth_no_intersect_ns = 0.0;
+ double gjk_intersect_ns = 0.0;
+ double gjk_no_intersect_ns = 0.0;
+ double sat_intersect_ns = 0.0;
+ double sat_no_intersect_ns = 0.0;
+ int intersect_count = 0;
+ polygons.clear();
+
+ for (auto i = 0; i < polygons_nb; ++i) {
+ polygons.push_back(autoware_utils_geometry::random_convex_polygon(vertices, max_values));
+ }
+
+ for (auto i = 0UL; i < polygons.size(); ++i) {
+ for (auto j = 0UL; j < polygons.size(); ++j) {
+ sw.tic();
+ const auto ground_truth = boost::geometry::intersects(polygons[i], polygons[j]);
+ if (ground_truth) {
+ ++intersect_count;
+ ground_truth_intersect_ns += sw.toc();
+ } else {
+ ground_truth_no_intersect_ns += sw.toc();
+ }
+
+ sw.tic();
+ const auto gjk = autoware_utils_geometry::intersects_convex(polygons[i], polygons[j]);
+ if (gjk) {
+ gjk_intersect_ns += sw.toc();
+ } else {
+ gjk_no_intersect_ns += sw.toc();
+ }
+
+ sw.tic();
+ const auto sat = autoware_utils_geometry::sat::intersects(polygons[i], polygons[j]);
+ if (sat) {
+ sat_intersect_ns += sw.toc();
+ } else {
+ sat_no_intersect_ns += sw.toc();
+ }
+
+ EXPECT_EQ(ground_truth, gjk);
+ EXPECT_EQ(ground_truth, sat);
+
+ if (ground_truth != gjk) {
+ std::cout << "Failed for the 2 polygons with GJK: ";
+ std::cout << boost::geometry::wkt(polygons[i]) << boost::geometry::wkt(polygons[j])
+ << std::endl;
+ }
+
+ if (ground_truth != sat) {
+ std::cout << "Failed for the 2 polygons with SAT: ";
+ std::cout << boost::geometry::wkt(polygons[i]) << boost::geometry::wkt(polygons[j])
+ << std::endl;
+ }
+ }
+ }
+
+ std::printf(
+ "polygons_nb = %d, vertices = %ld, %d / %d pairs with intersects\n", polygons_nb, vertices,
+ intersect_count, polygons_nb * polygons_nb);
+
+ std::printf(
+ "\tIntersect:\n\t\tBoost::geometry = %2.2f ms\n\t\tGJK = %2.2f ms\n\t\tSAT = %2.2f ms\n",
+ ground_truth_intersect_ns / 1e6, gjk_intersect_ns / 1e6, sat_intersect_ns / 1e6);
+
+ std::printf(
+ "\tNo Intersect:\n\t\tBoost::geometry = %2.2f ms\n\t\tGJK = %2.2f ms\n\t\tSAT = %2.2f ms\n",
+ ground_truth_no_intersect_ns / 1e6, gjk_no_intersect_ns / 1e6, sat_no_intersect_ns / 1e6);
+
+ std::printf(
+ "\tTotal:\n\t\tBoost::geometry = %2.2f ms\n\t\tGJK = %2.2f ms\n\t\tSAT = %2.2f ms\n",
+ (ground_truth_no_intersect_ns + ground_truth_intersect_ns) / 1e6,
+ (gjk_no_intersect_ns + gjk_intersect_ns) / 1e6,
+ (sat_no_intersect_ns + sat_intersect_ns) / 1e6);
+ }
+}
+
+double calculate_total_polygon_area(
+ const std::vector & polygons)
+{
+ double totalArea = 0.0;
+ for (const auto & polygon : polygons) {
+ totalArea += boost::geometry::area(polygon);
+ }
+ return totalArea;
+}
+
+TEST(geometry, PolygonTriangulation)
+{
+ using autoware_utils_geometry::Polygon2d;
+ using autoware_utils_geometry::triangulate;
+
+ { // concave polygon
+ Polygon2d poly;
+
+ poly.outer().emplace_back(0.0, 0.0);
+ poly.outer().emplace_back(4.0, 0.0);
+ poly.outer().emplace_back(4.0, 4.0);
+ poly.outer().emplace_back(2.0, 2.0);
+ poly.outer().emplace_back(0.0, 4.0);
+ boost::geometry::correct(poly);
+
+ const auto triangles = triangulate(poly);
+
+ const auto triangle_area = calculate_total_polygon_area(triangles);
+ const auto poly_area = boost::geometry::area(poly);
+ EXPECT_NEAR(triangle_area, poly_area, epsilon);
+ }
+
+ { // concave polygon with empty inners
+ Polygon2d poly;
+
+ poly.outer().emplace_back(0.0, 0.0);
+ poly.outer().emplace_back(4.0, 0.0);
+ poly.outer().emplace_back(4.0, 4.0);
+ poly.outer().emplace_back(2.0, 2.0);
+ poly.outer().emplace_back(0.0, 4.0);
+ boost::geometry::correct(poly);
+
+ poly.inners().emplace_back();
+
+ const auto triangles = triangulate(poly);
+
+ const auto triangle_area = calculate_total_polygon_area(triangles);
+ const auto poly_area = boost::geometry::area(poly);
+ EXPECT_NEAR(triangle_area, poly_area, epsilon);
+ }
+
+ { // concave polygon with hole
+ Polygon2d poly;
+
+ poly.outer().emplace_back(0.0, 0.0);
+ poly.outer().emplace_back(4.0, 0.0);
+ poly.outer().emplace_back(4.0, 4.0);
+ poly.outer().emplace_back(2.0, 2.0);
+ poly.outer().emplace_back(0.0, 4.0);
+
+ poly.inners().emplace_back();
+ poly.inners().back().emplace_back(1.0, 1.0);
+ poly.inners().back().emplace_back(1.5, 1.0);
+ poly.inners().back().emplace_back(1.5, 1.5);
+ poly.inners().back().emplace_back(1.0, 1.5);
+ boost::geometry::correct(poly);
+
+ const auto triangles = triangulate(poly);
+
+ const auto triangle_area = calculate_total_polygon_area(triangles);
+ const auto poly_area = boost::geometry::area(poly);
+ EXPECT_NEAR(triangle_area, poly_area, epsilon);
+ }
+
+ { // concave polygon with one empty inner followed by one hole
+ Polygon2d poly;
+
+ poly.outer().emplace_back(0.0, 0.0);
+ poly.outer().emplace_back(4.0, 0.0);
+ poly.outer().emplace_back(4.0, 4.0);
+ poly.outer().emplace_back(2.0, 2.0);
+ poly.outer().emplace_back(0.0, 4.0);
+
+ poly.inners().emplace_back();
+ poly.inners().emplace_back();
+ poly.inners().back().emplace_back(1.0, 1.0);
+ poly.inners().back().emplace_back(1.5, 1.0);
+ poly.inners().back().emplace_back(1.5, 1.5);
+ poly.inners().back().emplace_back(1.0, 1.5);
+ boost::geometry::correct(poly);
+
+ const auto triangles = triangulate(poly);
+
+ const auto triangle_area = calculate_total_polygon_area(triangles);
+ const auto poly_area = boost::geometry::area(poly);
+ EXPECT_NEAR(triangle_area, poly_area, epsilon);
+ }
+}
+
+TEST(geometry, intersectPolygonWithHoles)
+{
+ using autoware_utils_geometry::Polygon2d;
+ using autoware_utils_geometry::triangulate;
+
+ { // quadrilateral inside the hole of another quadrilateral (not intersecting)
+ Polygon2d outerConcave;
+ Polygon2d innerConcave;
+
+ outerConcave.outer().emplace_back(0.0, 0.0);
+ outerConcave.outer().emplace_back(4.0, 0.0);
+ outerConcave.outer().emplace_back(4.0, 4.0);
+ outerConcave.outer().emplace_back(2.0, 2.0);
+ outerConcave.outer().emplace_back(0.0, 4.0);
+
+ outerConcave.inners().emplace_back();
+ outerConcave.inners().back().emplace_back(1.0, 1.0);
+ outerConcave.inners().back().emplace_back(3.0, 1.0);
+ outerConcave.inners().back().emplace_back(3.0, 3.0);
+ outerConcave.inners().back().emplace_back(1.0, 3.0);
+
+ innerConcave.outer().emplace_back(1.5, 1.5);
+ innerConcave.outer().emplace_back(2.5, 1.5);
+ innerConcave.outer().emplace_back(2.5, 2.5);
+ innerConcave.outer().emplace_back(1.5, 2.5);
+
+ const auto triangles1 = triangulate(outerConcave);
+ const auto triangles2 = triangulate(innerConcave);
+
+ const bool gjk_intersect = autoware_utils_geometry::test_intersection(
+ triangles1, triangles2, autoware_utils_geometry::intersects_convex);
+ const bool sat_intersect = autoware_utils_geometry::test_intersection(
+ triangles1, triangles2, autoware_utils_geometry::sat::intersects);
+
+ EXPECT_FALSE(gjk_intersect);
+ EXPECT_FALSE(sat_intersect);
+ }
+
+ { // quadrilateral inside the hole of another quadrilateral (intersecting)
+ Polygon2d outerConcave;
+ Polygon2d intersectingInnerConcave;
+
+ outerConcave.outer().emplace_back(0.0, 0.0);
+ outerConcave.outer().emplace_back(4.0, 0.0);
+ outerConcave.outer().emplace_back(4.0, 4.0);
+ outerConcave.outer().emplace_back(2.0, 2.0);
+ outerConcave.outer().emplace_back(0.0, 4.0);
+
+ outerConcave.inners().emplace_back();
+ outerConcave.inners().back().emplace_back(1.0, 1.0);
+ outerConcave.inners().back().emplace_back(3.0, 1.0);
+ outerConcave.inners().back().emplace_back(3.0, 3.0);
+ outerConcave.inners().back().emplace_back(1.0, 3.0);
+
+ intersectingInnerConcave.outer().emplace_back(0.5, 0.5);
+ intersectingInnerConcave.outer().emplace_back(2.5, 0.5);
+ intersectingInnerConcave.outer().emplace_back(2.5, 2.0);
+ intersectingInnerConcave.outer().emplace_back(0.5, 2.0);
+
+ const auto triangles1 = triangulate(outerConcave);
+ const auto triangles2 = triangulate(intersectingInnerConcave);
+ const auto gjk_intersect = autoware_utils_geometry::test_intersection(
+ triangles1, triangles2, autoware_utils_geometry::intersects_convex);
+ const auto sat_intersect = autoware_utils_geometry::test_intersection(
+ triangles1, triangles2, autoware_utils_geometry::sat::intersects);
+
+ EXPECT_TRUE(gjk_intersect);
+ EXPECT_TRUE(sat_intersect);
+ }
+}
+
+TEST(
+ geometry,
+ DISABLED_intersectConcavePolygon) // GJK give different result for edge test (point sharing and
+ // edge sharing) compared to SAT and boost::geometry::intersect
+{
+ using autoware_utils_geometry::Polygon2d;
+ using autoware_utils_geometry::triangulate;
+
+ { // 2 Concave quadrilateral with intersection
+ Polygon2d poly1;
+ Polygon2d poly2;
+ poly1.outer().emplace_back(4, 11);
+ poly1.outer().emplace_back(4, 5);
+ poly1.outer().emplace_back(9, 9);
+ poly1.outer().emplace_back(2, 2);
+ poly2.outer().emplace_back(5, 7);
+ poly2.outer().emplace_back(7, 3);
+ poly2.outer().emplace_back(9, 6);
+ poly2.outer().emplace_back(12, 7);
+
+ boost::geometry::correct(poly1);
+ boost::geometry::correct(poly2);
+
+ const auto triangles1 = triangulate(poly1);
+ const auto triangles2 = triangulate(poly2);
+
+ const auto gjk_intersect = autoware_utils_geometry::test_intersection(
+ triangles1, triangles2, autoware_utils_geometry::intersects_convex);
+ const auto sat_intersect = autoware_utils_geometry::test_intersection(
+ triangles1, triangles2, autoware_utils_geometry::sat::intersects);
+
+ EXPECT_TRUE(gjk_intersect);
+ EXPECT_TRUE(sat_intersect);
+ }
+
+ { // 2 concave polygons with no intersection (but they share an edge)
+ Polygon2d poly1;
+ Polygon2d poly2;
+ poly1.outer().emplace_back(0, 2);
+ poly1.outer().emplace_back(2, 2);
+ poly1.outer().emplace_back(2, 0);
+ poly1.outer().emplace_back(0, 0);
+
+ poly2.outer().emplace_back(0, 0);
+ poly2.outer().emplace_back(2, 0);
+ poly2.outer().emplace_back(2, -2);
+ poly2.outer().emplace_back(0, -2);
+
+ boost::geometry::correct(poly1);
+ boost::geometry::correct(poly2);
+
+ const auto triangles1 = triangulate(poly1);
+ const auto triangles2 = triangulate(poly2);
+
+ const auto gjk_intersect = autoware_utils_geometry::test_intersection(
+ triangles1, triangles2, autoware_utils_geometry::intersects_convex);
+ const auto sat_intersect = autoware_utils_geometry::test_intersection(
+ triangles1, triangles2, autoware_utils_geometry::sat::intersects);
+
+ EXPECT_FALSE(gjk_intersect);
+ EXPECT_FALSE(sat_intersect);
+ }
+
+ { // 2 concave polygons with no intersection (but they share a point)
+ Polygon2d poly1;
+ Polygon2d poly2;
+ poly1.outer().emplace_back(0, 2);
+ poly1.outer().emplace_back(2, 2);
+ poly1.outer().emplace_back(0, 0);
+
+ poly2.outer().emplace_back(4, 4);
+ poly2.outer().emplace_back(4, 2);
+ poly2.outer().emplace_back(2, 2);
+ poly2.outer().emplace_back(2, 4);
+
+ boost::geometry::correct(poly1);
+ boost::geometry::correct(poly2);
+
+ auto triangles1 = triangulate(poly1);
+ auto triangles2 = triangulate(poly2);
+
+ const auto gjk_intersect = autoware_utils_geometry::test_intersection(
+ triangles1, triangles2, autoware_utils_geometry::intersects_convex);
+ const auto sat_intersect = autoware_utils_geometry::test_intersection(
+ triangles1, triangles2, autoware_utils_geometry::sat::intersects);
+
+ EXPECT_FALSE(gjk_intersect);
+ EXPECT_FALSE(sat_intersect);
+ }
+
+ { // 2 concave polygons sharing a point and then with very small intersection
+ Polygon2d poly1;
+ Polygon2d poly2;
+ poly1.outer().emplace_back(0, 0);
+ poly1.outer().emplace_back(2, 2);
+ poly1.outer().emplace_back(4, 0);
+ poly1.outer().emplace_back(2, -2);
+
+ poly2.outer().emplace_back(0, 4);
+ poly2.outer().emplace_back(2, 2);
+ poly2.outer().emplace_back(4, 4);
+ poly2.outer().emplace_back(2, 6);
+
+ boost::geometry::correct(poly1);
+ boost::geometry::correct(poly2);
+
+ const auto triangles1 = triangulate(poly1);
+ const auto triangles2 = triangulate(poly2);
+
+ const auto gjk_intersect = autoware_utils_geometry::test_intersection(
+ triangles1, triangles2, autoware_utils_geometry::intersects_convex);
+ const auto sat_intersect = autoware_utils_geometry::test_intersection(
+ triangles1, triangles2, autoware_utils_geometry::sat::intersects);
+
+ EXPECT_FALSE(gjk_intersect);
+ EXPECT_FALSE(sat_intersect);
+
+ // small intersection test
+ poly1.outer()[1].y() += 1e-12;
+ {
+ const auto triangles = triangulate(poly1);
+ const bool gjk_intersect = autoware_utils_geometry::test_intersection(
+ triangles, triangles2, autoware_utils_geometry::intersects_convex);
+ const bool sat_intersect = autoware_utils_geometry::test_intersection(
+ triangles, triangles2, autoware_utils_geometry::sat::intersects);
+ EXPECT_TRUE(gjk_intersect);
+ EXPECT_TRUE(sat_intersect);
+ }
+ }
+}
+
+TEST(geometry, intersectConcavePolygonRand)
+{
+ std::vector polygons;
+ std::vector> triangulations;
+ constexpr auto polygons_nb = 100;
+ constexpr auto max_vertices = 10;
+ constexpr auto max_values = 1000;
+
+ autoware_utils_system::StopWatch sw;
+
+ for (auto vertices = 4UL; vertices < max_vertices; ++vertices) {
+ double ground_truth_intersect_ns = 0.0;
+ double ground_truth_no_intersect_ns = 0.0;
+ double gjk_intersect_ns = 0.0;
+ double gjk_no_intersect_ns = 0.0;
+ double sat_intersect_ns = 0.0;
+ double sat_no_intersect_ns = 0.0;
+ double triangulation_ns = 0.0;
+ int intersect_count = 0;
+ polygons.clear();
+ triangulations.clear();
+
+ for (auto i = 0; i < polygons_nb; ++i) {
+ auto polygon_opt = autoware_utils_geometry::random_concave_polygon(vertices, max_values);
+ if (polygon_opt.has_value()) {
+ polygons.push_back(polygon_opt.value());
+ }
+ }
+
+ for (const auto & polygon : polygons) {
+ sw.tic();
+ std::vector triangles =
+ autoware_utils_geometry::triangulate(polygon);
+ triangulation_ns += sw.toc();
+ triangulations.push_back(triangles);
+ }
+
+ for (auto i = 0UL; i < polygons.size(); ++i) {
+ for (auto j = 0UL; j < polygons.size(); ++j) {
+ sw.tic();
+ const auto ground_truth = boost::geometry::intersects(polygons[i], polygons[j]);
+ if (ground_truth) {
+ ++intersect_count;
+ ground_truth_intersect_ns += sw.toc();
+ } else {
+ ground_truth_no_intersect_ns += sw.toc();
+ }
+
+ sw.tic();
+ bool gjk_intersect = autoware_utils_geometry::test_intersection(
+ triangulations[i], triangulations[j], autoware_utils_geometry::intersects_convex);
+ if (!gjk_intersect) {
+ gjk_no_intersect_ns += sw.toc();
+ } else {
+ gjk_intersect_ns += sw.toc();
+ }
+
+ sw.tic();
+ bool sat_intersect = autoware_utils_geometry::test_intersection(
+ triangulations[i], triangulations[j], autoware_utils_geometry::sat::intersects);
+ if (!sat_intersect) {
+ sat_no_intersect_ns += sw.toc();
+ } else {
+ sat_intersect_ns += sw.toc();
+ }
+
+ EXPECT_EQ(ground_truth, gjk_intersect);
+ EXPECT_EQ(ground_truth, sat_intersect);
+
+ if (ground_truth != gjk_intersect) {
+ std::cout << "Failed for the 2 polygons with GJK: ";
+ std::cout << boost::geometry::wkt(polygons[i]) << boost::geometry::wkt(polygons[j])
+ << std::endl;
+ }
+
+ if (ground_truth != sat_intersect) {
+ std::cout << "Failed for the 2 polygons with SAT: ";
+ std::cout << boost::geometry::wkt(polygons[i]) << boost::geometry::wkt(polygons[j])
+ << std::endl;
+ }
+ }
+ }
+
+ std::printf(
+ "polygons_nb = %d, vertices = %ld, %d / %d pairs with intersects\n", polygons_nb, vertices,
+ intersect_count, polygons_nb * polygons_nb);
+
+ std::printf(
+ "\tIntersect:\n\t\tBoost::geometry = %2.2f ms\n\t\tGJK = %2.2f ms\n\t\tSAT = %2.2f ms\n",
+ ground_truth_intersect_ns / 1e6, gjk_intersect_ns / 1e6, sat_intersect_ns / 1e6);
+
+ std::printf(
+ "\tNo Intersect:\n\t\tBoost::geometry = %2.2f ms\n\t\tGJK = %2.2f ms\n\t\tSAT = %2.2f ms\n",
+ ground_truth_no_intersect_ns / 1e6, gjk_no_intersect_ns / 1e6, sat_no_intersect_ns / 1e6);
+
+ std::printf("\tTotal:\n\t\tTriangulation = %2.2f ms\n", triangulation_ns / 1e6);
+ }
+}
diff --git a/autoware_utils_geometry/test/cases/path_with_lane_id_geometry.cpp b/autoware_utils_geometry/test/cases/path_with_lane_id_geometry.cpp
new file mode 100644
index 0000000..c94d494
--- /dev/null
+++ b/autoware_utils_geometry/test/cases/path_with_lane_id_geometry.cpp
@@ -0,0 +1,119 @@
+// Copyright 2022 TIER IV, 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.
+
+#include "autoware_utils_geometry/geometry.hpp"
+
+#include
+
+TEST(geometry, get_point_PathWithLaneId)
+{
+ using autoware_utils_geometry::get_point;
+
+ const double x_ans = 1.0;
+ const double y_ans = 2.0;
+ const double z_ans = 3.0;
+
+ autoware_internal_planning_msgs::msg::PathPointWithLaneId p;
+ p.point.pose.position.x = x_ans;
+ p.point.pose.position.y = y_ans;
+ p.point.pose.position.z = z_ans;
+ const geometry_msgs::msg::Point p_out = get_point(p);
+ EXPECT_DOUBLE_EQ(p_out.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.z, z_ans);
+}
+
+TEST(geometry, get_pose_PathWithLaneId)
+{
+ using autoware_utils_geometry::get_pose;
+
+ const double x_ans = 1.0;
+ const double y_ans = 2.0;
+ const double z_ans = 3.0;
+ const double q_x_ans = 0.1;
+ const double q_y_ans = 0.2;
+ const double q_z_ans = 0.3;
+ const double q_w_ans = 0.4;
+
+ autoware_internal_planning_msgs::msg::PathPointWithLaneId p;
+ p.point.pose.position.x = x_ans;
+ p.point.pose.position.y = y_ans;
+ p.point.pose.position.z = z_ans;
+ p.point.pose.orientation.x = q_x_ans;
+ p.point.pose.orientation.y = q_y_ans;
+ p.point.pose.orientation.z = q_z_ans;
+ p.point.pose.orientation.w = q_w_ans;
+ const geometry_msgs::msg::Pose p_out = get_pose(p);
+ EXPECT_DOUBLE_EQ(p_out.position.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.position.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.position.z, z_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans);
+ EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans);
+}
+
+TEST(geometry, get_longitudinal_velocity_PathWithLaneId)
+{
+ using autoware_utils_geometry::get_longitudinal_velocity;
+
+ const double velocity = 1.0;
+
+ autoware_internal_planning_msgs::msg::PathPointWithLaneId p;
+ p.point.longitudinal_velocity_mps = velocity;
+ EXPECT_DOUBLE_EQ(get_longitudinal_velocity(p), velocity);
+}
+
+TEST(geometry, set_pose_PathWithLaneId)
+{
+ using autoware_utils_geometry::set_pose;
+
+ const double x_ans = 1.0;
+ const double y_ans = 2.0;
+ const double z_ans = 3.0;
+ const double q_x_ans = 0.1;
+ const double q_y_ans = 0.2;
+ const double q_z_ans = 0.3;
+ const double q_w_ans = 0.4;
+
+ geometry_msgs::msg::Pose p;
+ p.position.x = x_ans;
+ p.position.y = y_ans;
+ p.position.z = z_ans;
+ p.orientation.x = q_x_ans;
+ p.orientation.y = q_y_ans;
+ p.orientation.z = q_z_ans;
+ p.orientation.w = q_w_ans;
+
+ autoware_internal_planning_msgs::msg::PathPointWithLaneId p_out{};
+ set_pose(p, p_out);
+ EXPECT_DOUBLE_EQ(p_out.point.pose.position.x, x_ans);
+ EXPECT_DOUBLE_EQ(p_out.point.pose.position.y, y_ans);
+ EXPECT_DOUBLE_EQ(p_out.point.pose.position.z, z_ans);
+ EXPECT_DOUBLE_EQ(p_out.point.pose.orientation.x, q_x_ans);
+ EXPECT_DOUBLE_EQ(p_out.point.pose.orientation.y, q_y_ans);
+ EXPECT_DOUBLE_EQ(p_out.point.pose.orientation.z, q_z_ans);
+ EXPECT_DOUBLE_EQ(p_out.point.pose.orientation.w, q_w_ans);
+}
+
+TEST(geometry, set_longitudinal_velocity_PathWithLaneId)
+{
+ using autoware_utils_geometry::set_longitudinal_velocity;
+
+ const double velocity = 1.0;
+
+ autoware_internal_planning_msgs::msg::PathPointWithLaneId p{};
+ set_longitudinal_velocity(velocity, p);
+ EXPECT_DOUBLE_EQ(p.point.longitudinal_velocity_mps, velocity);
+}
diff --git a/autoware_utils_geometry/test/cases/pose_deviation.cpp b/autoware_utils_geometry/test/cases/pose_deviation.cpp
new file mode 100644
index 0000000..d090366
--- /dev/null
+++ b/autoware_utils_geometry/test/cases/pose_deviation.cpp
@@ -0,0 +1,44 @@
+// Copyright 2020 Tier IV, 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.
+
+#include "autoware_utils_geometry/pose_deviation.hpp"
+
+#include "autoware_utils_geometry/geometry.hpp"
+#include "autoware_utils_math/unit_conversion.hpp"
+
+#include
+
+TEST(geometry, pose_deviation)
+{
+ using autoware_utils_geometry::calc_pose_deviation;
+ using autoware_utils_geometry::create_quaternion_from_rpy;
+ using autoware_utils_math::deg2rad;
+
+ geometry_msgs::msg::Pose pose1;
+ pose1.position.x = 1.0;
+ pose1.position.y = 2.0;
+ pose1.position.z = 3.0;
+ pose1.orientation = create_quaternion_from_rpy(0, 0, deg2rad(45));
+
+ geometry_msgs::msg::Pose pose2;
+ pose2.position.x = 2.0;
+ pose2.position.y = 4.0;
+ pose2.position.z = 6.0;
+ pose2.orientation = create_quaternion_from_rpy(0, 0, deg2rad(60));
+
+ const auto deviation = calc_pose_deviation(pose1, pose2);
+ EXPECT_DOUBLE_EQ(deviation.lateral, 0.70710678118654735);
+ EXPECT_DOUBLE_EQ(deviation.longitudinal, 2.1213203435596428);
+ EXPECT_DOUBLE_EQ(deviation.yaw, deg2rad(15));
+}
diff --git a/autoware_utils_pcl/CMakeLists.txt b/autoware_utils_pcl/CMakeLists.txt
index 516f373..cb8e3e7 100644
--- a/autoware_utils_pcl/CMakeLists.txt
+++ b/autoware_utils_pcl/CMakeLists.txt
@@ -5,6 +5,8 @@ find_package(autoware_cmake REQUIRED)
autoware_package()
if(BUILD_TESTING)
-endif()
+ file(GLOB_RECURSE test_files test/*.cpp)
+ ament_auto_add_gtest(test_${PROJECT_NAME} ${test_files})
+endif()
ament_auto_package()
diff --git a/autoware_utils_pcl/test/cases/transform.cpp b/autoware_utils_pcl/test/cases/transform.cpp
new file mode 100644
index 0000000..0414391
--- /dev/null
+++ b/autoware_utils_pcl/test/cases/transform.cpp
@@ -0,0 +1,59 @@
+// Copyright 2020 Tier IV, 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.
+
+#include "autoware_utils_pcl/transforms.hpp"
+
+#include
+#include
+#include
+
+#include
+
+TEST(system, transform_point_cloud)
+{
+ pcl::PointCloud cloud;
+ cloud.push_back(pcl::PointXYZI(10.055880, -42.758892, -10.636949, 4));
+ cloud.push_back(pcl::PointXYZI(23.282284, -29.485722, -11.468469, 5));
+
+ Eigen::Matrix transform;
+ transform << 0.834513, -0.550923, -0.008474, 89571.148438, 0.550986, 0.834372, 0.015428,
+ 42301.179688, -0.001429, -0.017543, 0.999845, -3.157415, 0.000000, 0.000000, 0.000000, 1.000000;
+
+ pcl::PointCloud cloud_transformed;
+ autoware_utils_pcl::transform_pointcloud(cloud, cloud_transformed, transform);
+
+ pcl::PointXYZI pt1_gt(89603.187500, 42270.878906, -13.056946, 4);
+
+ constexpr float float_error = 0.0001;
+ EXPECT_NEAR(cloud_transformed[0].x, pt1_gt.x, float_error);
+ EXPECT_NEAR(cloud_transformed[0].y, pt1_gt.y, float_error);
+ EXPECT_NEAR(cloud_transformed[0].z, pt1_gt.z, float_error);
+ EXPECT_EQ(cloud_transformed[0].intensity, pt1_gt.intensity);
+}
+
+TEST(system, empty_point_cloud)
+{
+ pcl::PointCloud cloud;
+
+ Eigen::Matrix transform;
+ transform << 0.834513, -0.550923, -0.008474, 89571.148438, 0.550986, 0.834372, 0.015428,
+ 42301.179688, -0.001429, -0.017543, 0.999845, -3.157415, 0.000000, 0.000000, 0.000000, 1.000000;
+
+ pcl::PointCloud cloud_transformed;
+
+ EXPECT_NO_THROW(autoware_utils_pcl::transform_pointcloud(cloud, cloud_transformed, transform));
+ EXPECT_NO_FATAL_FAILURE(
+ autoware_utils_pcl::transform_pointcloud(cloud, cloud_transformed, transform));
+ EXPECT_EQ(cloud_transformed.size(), 0ul);
+}