feat(autoware_geography_utils): add support for local cartesian proje…

…ction (#181)

* feat(autoware_geography_utils): add support for local cartesian projection

Signed-off-by: Sebastian Zęderowski <szederowski@autonomous-systems.pl>

* feat(autoware_geography_utils): add support for local cartesian projection

Signed-off-by: Sebastian Zęderowski <szederowski@autonomous-systems.pl>

* style(pre-commit): autofix

* feat(autoware_geography_utils): add support for local cartesian projection

Signed-off-by: Sebastian Zęderowski <szederowski@autonomous-systems.pl>

* add tests

Signed-off-by: Mete Fatih Cırıt <mfc@autoware.org>

* fix the test

Signed-off-by: Mete Fatih Cırıt <mfc@autoware.org>

* Update common/autoware_geography_utils/test/test_lanelet2_projector.cpp

---------

Signed-off-by: Sebastian Zęderowski <szederowski@autonomous-systems.pl>
Signed-off-by: Mete Fatih Cırıt <mfc@autoware.org>
Co-authored-by: Sebastian Zęderowski <szederowski@autonomous-systems.pl>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Mete Fatih Cırıt <mfc@autoware.org>
Co-authored-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>

common/autoware_geography_utils/src/lanelet2_projector.cpp

@@ -17,6 +17,7 @@
 #include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
 #include <autoware_lanelet2_extension/projection/transverse_mercator_projector.hpp>
 
+#include <lanelet2_projection/LocalCartesian.h>
 #include <lanelet2_projection/UTM.h>
 
 #include <memory>
@@ -51,9 +52,18 @@ std::unique_ptr<lanelet::Projector> get_lanelet2_projector(const MapProjectorInf
     return std::make_unique<lanelet::projection::TransverseMercatorProjector>(projector);
   }
 
+  if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN) {
+    const lanelet::GPSPoint position{
+      projector_info.map_origin.latitude, projector_info.map_origin.longitude,
+      projector_info.map_origin.altitude};
+    const lanelet::Origin origin{position};
+    const lanelet::projection::LocalCartesianProjector projector{origin};
+    return std::make_unique<lanelet::projection::LocalCartesianProjector>(projector);
+  }
+
   throw std::invalid_argument(
     "Invalid map projector type: " + projector_info.projector_type +
-    ". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator");
+    ". Currently supported types: MGRS, LocalCartesianUTM, LocalCartesian and TransverseMercator");
 }
 
 }  // namespace autoware::geography_utils

common/autoware_geography_utils/test/test_lanelet2_projector.cpp

@@ -17,6 +17,7 @@
 #include <autoware_lanelet2_extension/projection/transverse_mercator_projector.hpp>
 
 #include <gtest/gtest.h>
+#include <lanelet2_projection/LocalCartesian.h>
 #include <lanelet2_projection/UTM.h>
 
 #include <memory>
@@ -52,6 +53,22 @@ TEST(GeographyUtilsLanelet2Projector, GetLocalCartesianUTMProjector)
   EXPECT_NE(dynamic_cast<lanelet::projection::UtmProjector *>(projector.get()), nullptr);
 }
 
+TEST(GeographyUtilsLanelet2Projector, GetLocalCartesianProjector)
+{
+  autoware_map_msgs::msg::MapProjectorInfo projector_info;
+  projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN;
+  projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84;
+  projector_info.map_origin.latitude = 35.62426;
+  projector_info.map_origin.longitude = 139.74252;
+  projector_info.map_origin.altitude = 0.0;
+
+  std::unique_ptr<lanelet::Projector> projector =
+    autoware::geography_utils::get_lanelet2_projector(projector_info);
+
+  // Check if the returned projector is of type LocalCartesianProjector
+  EXPECT_NE(dynamic_cast<lanelet::projection::LocalCartesianProjector *>(projector.get()), nullptr);
+}
+
 TEST(GeographyUtilsLanelet2Projector, GetTransverseMercatorProjector)
 {
   autoware_map_msgs::msg::MapProjectorInfo projector_info;

common/autoware_geography_utils/test/test_projection.cpp

@@ -159,3 +159,61 @@ TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin)
   EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001);
   EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001);
 }
+
+TEST(GeographyUtilsProjection, ProjectForwardToLocalCartesianOrigin)
+{
+  // source point
+  geographic_msgs::msg::GeoPoint geo_point;
+  geo_point.latitude = 35.62406;
+  geo_point.longitude = 139.74252;
+  geo_point.altitude = 10.0;
+
+  // target point
+  geometry_msgs::msg::Point local_point;
+  local_point.x = 0.0;
+  local_point.y = -22.18;
+  local_point.z = 20.0;
+
+  // projector info
+  autoware_map_msgs::msg::MapProjectorInfo projector_info;
+  projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN;
+  projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84;
+  projector_info.map_origin.latitude = 35.62426;
+  projector_info.map_origin.longitude = 139.74252;
+  projector_info.map_origin.altitude = -10.0;
+
+  // conversion
+  const geometry_msgs::msg::Point converted_point =
+    autoware::geography_utils::project_forward(geo_point, projector_info);
+
+  EXPECT_NEAR(converted_point.x, local_point.x, 1.0);
+  EXPECT_NEAR(converted_point.y, local_point.y, 1.0);
+  EXPECT_NEAR(converted_point.z, local_point.z, 1.0);
+}
+
+TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianOrigin)
+{
+  // source point
+  geographic_msgs::msg::GeoPoint geo_point;
+  geo_point.latitude = 35.62426;
+  geo_point.longitude = 139.74252;
+  geo_point.altitude = 10.0;
+
+  // projector info
+  autoware_map_msgs::msg::MapProjectorInfo projector_info;
+  projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN;
+  projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84;
+  projector_info.map_origin.latitude = 35.0;
+  projector_info.map_origin.longitude = 139.0;
+  projector_info.map_origin.altitude = 0.0;
+
+  // conversion
+  const geometry_msgs::msg::Point converted_local_point =
+    autoware::geography_utils::project_forward(geo_point, projector_info);
+  const geographic_msgs::msg::GeoPoint converted_geo_point =
+    autoware::geography_utils::project_reverse(converted_local_point, projector_info);
+
+  EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001);
+  EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001);
+  EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001);
+}