Implement signed curvature (#355)

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

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>

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

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) {