From 3573ce842823dfa1d9a648c5bbd4918eba14ae59 Mon Sep 17 00:00:00 2001
From: johannes-fischer <42044738+johannes-fischer@users.noreply.github.com>
Date: Fri, 25 Oct 2024 16:48:43 +0200
Subject: [PATCH] Implement signed curvature (#355)

---
 .../include/lanelet2_core/geometry/LineString.h      | 12 ++++++++++++
 .../include/lanelet2_core/geometry/impl/LineString.h |  7 ++++++-
 lanelet2_core/test/test_linestring.cpp               |  6 ++++++
 lanelet2_python/python_api/geometry.cpp              |  6 ++++++
 4 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/lanelet2_core/include/lanelet2_core/geometry/LineString.h b/lanelet2_core/include/lanelet2_core/geometry/LineString.h
index 8bc7e656..0b929e75 100644
--- a/lanelet2_core/include/lanelet2_core/geometry/LineString.h
+++ b/lanelet2_core/include/lanelet2_core/geometry/LineString.h
@@ -110,6 +110,18 @@ double signedDistance(const LineString2dT& lineString, const BasicPoint2d& p);
 template <typename Point2dT>
 double curvature2d(const Point2dT& p1, const Point2dT& p2, const Point2dT& p3);
 
+/**
+ * Calculate the signed curvature value given 3 consecutive points.
+ * The signed curvature value is positive if the consecutive points describe a left turn, negative otherwise.
+ *
+ * @param p1, p2, p3 are the points
+ * @return curvature value.
+ *
+ * If any 2 of the 3 points duplicate, return infinity.
+ */
+template <typename Point2dT>
+double signedCurvature2d(const Point2dT& p1, const Point2dT& p2, const Point2dT& p3);
+
 /**
  *
  * @brief Transform a point to the coordinates of the linestring
diff --git a/lanelet2_core/include/lanelet2_core/geometry/impl/LineString.h b/lanelet2_core/include/lanelet2_core/geometry/impl/LineString.h
index 8c44afac..a6461cfc 100644
--- a/lanelet2_core/include/lanelet2_core/geometry/impl/LineString.h
+++ b/lanelet2_core/include/lanelet2_core/geometry/impl/LineString.h
@@ -703,6 +703,11 @@ double signedDistance(const LineString2dT& lineString, const BasicPoint2d& p) {
 
 template <typename Point2dT>
 double curvature2d(const Point2dT& p1, const Point2dT& p2, const Point2dT& p3) {
+  return std::fabs(signedCurvature2d(p1, p2, p3));
+}
+
+template <typename Point2dT>
+double signedCurvature2d(const Point2dT& p1, const Point2dT& p2, const Point2dT& p3) {
   // see https://en.wikipedia.org/wiki/Menger_curvature#Definition
   const double area = 0.5 * ((p2.x() - p1.x()) * (p3.y() - p1.y()) - (p2.y() - p1.y()) * (p3.x() - p1.x()));
   const double side1 = distance(p1, p2);
@@ -712,7 +717,7 @@ double curvature2d(const Point2dT& p1, const Point2dT& p2, const Point2dT& p3) {
   if (product < 1e-20) {
     return std::numeric_limits<double>::infinity();
   }
-  return std::fabs(4 * area / product);
+  return 4 * area / product;
 }
 
 template <typename LineString2dT>
diff --git a/lanelet2_core/test/test_linestring.cpp b/lanelet2_core/test/test_linestring.cpp
index b8242ee2..7ef3d7c9 100644
--- a/lanelet2_core/test/test_linestring.cpp
+++ b/lanelet2_core/test/test_linestring.cpp
@@ -413,6 +413,12 @@ TYPED_TEST(AllLineStringsTest, segmentsInverse) {  // NOLINT
 TYPED_TEST(TwoDPointsTest, checkCurvature) {
   EXPECT_DOUBLE_EQ(1., geometry::curvature2d(this->p1, this->p2, this->p3));
   EXPECT_DOUBLE_EQ(std::numeric_limits<double>::infinity(), geometry::curvature2d(this->p1, this->p2, this->p4));
+  EXPECT_DOUBLE_EQ(1., geometry::curvature2d(this->p3, this->p2, this->p1));
+  EXPECT_DOUBLE_EQ(std::numeric_limits<double>::infinity(), geometry::curvature2d(this->p3, this->p2, this->p4));
+  EXPECT_DOUBLE_EQ(1., geometry::signedCurvature2d(this->p1, this->p2, this->p3));
+  EXPECT_DOUBLE_EQ(std::numeric_limits<double>::infinity(), geometry::signedCurvature2d(this->p1, this->p2, this->p4));
+  EXPECT_DOUBLE_EQ(-1., geometry::signedCurvature2d(this->p3, this->p2, this->p1));
+  EXPECT_DOUBLE_EQ(std::numeric_limits<double>::infinity(), geometry::signedCurvature2d(this->p3, this->p2, this->p4));
 }
 
 TYPED_TEST(TwoDLineStringsTest, signedDistance) {  // NOLINT
diff --git a/lanelet2_python/python_api/geometry.cpp b/lanelet2_python/python_api/geometry.cpp
index ea977664..d4bf866a 100644
--- a/lanelet2_python/python_api/geometry.cpp
+++ b/lanelet2_python/python_api/geometry.cpp
@@ -288,6 +288,12 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) {  // NOLINT
   def("area", boost::geometry::area<BasicPolygon2d>);
   def("area", boost::geometry::area<ConstHybridPolygon2d>);
 
+  def("curvature2d", lg::curvature2d<BasicPoint2d>);
+  def("curvature2d", lg::curvature2d<ConstPoint2d>);
+
+  def("signedCurvature2d", lg::signedCurvature2d<BasicPoint2d>);
+  def("signedCurvature2d", lg::signedCurvature2d<ConstPoint2d>);
+
   class_<ArcCoordinates>("ArcCoordinates", "Coordinates along an arc", no_init)
       .def("__init__", make_constructor(
                            +[](double length, double distance) {