Adds RoadGeometry::inertial_to_backend_frame_translation()

maliput/include/maliput/api/maliput_design.h

@@ -155,11 +155,20 @@
 /// > altitude or elevation.  @f$\hat{x}@f$ and @f$\hat{y}@f$ span the horizontal
 /// > plane.  Typically, the "ENU" convention is used: @f$\hat{x}@f$ points *East*
 /// > and @f$\hat{y}@f$ points *North*.
-/// >
-/// > *In the future:* the `maliput` API will be extended to provide a
-/// > description of the geographic coordinate system (if any) used by a
-/// > `RoadGeometry`, as well as a local gravity vector as a function of
-/// > position.
+///
+/// At the API level, we will refer to `Inertial Frame` as a synonym of
+/// `World`-frame.
+///
+/// Another frame will be defined, the `Backend Frame` which is
+/// different from the `World`-frame by an isometric transform. This frame is
+/// also a right-handed 3D inertial Cartesian coordinate system with an
+/// orthonormal basis. It exists and potentially differs from the
+/// `Inertial Frame` because of differing contexts. Typically, the
+/// `Inertial Frame` chosen matches that of the client, e.g., a
+/// simulator that uses Maliput. Meanwhile, the `Backend Frame` typically
+/// matches the underlying data used by a particular backend, e.g., an
+/// OpenDRIVE file. The `Backend Frame` will use `Vector3` for coordinates to
+/// properly differentiate from the `World`-frame type, i.e. `InertialPosition`.
 ///
 /// A `Lane`-frame is a right-handed orthonormal curvilinear coordinate system, with
 /// positions expressed as coordinates @f$(s,r,h)@f$.  Each `Lane` in a `RoadGeometry`

maliput/include/maliput/api/road_geometry.h

@@ -14,6 +14,7 @@
 #include "maliput/api/type_specific_identifier.h"
 #include "maliput/common/maliput_copyable.h"
 #include "maliput/common/maliput_throw.h"
+#include "maliput/math/vector.h"
 
 namespace maliput {
 namespace api {
@@ -176,6 +177,18 @@ class RoadGeometry {
     return DoSampleAheadWaypoints(lane_s_route, path_length_sampling_rate);
   }
 
+  /// The Backend Frame is an inertial frame similar to the Inertial Frame that
+  /// differ one from another by an isometric transformation. This method
+  /// returns the translation vector between both frames.
+  ///
+  /// For an explanation on the two different frames, see the explanation in
+  /// maliput_design.h.
+  ///
+  /// @return maliput's Inertial Frame to Backend Frame translation vector.
+  math::Vector3 inertial_to_backend_frame_translation() const { return do_inertial_to_backend_frame_translation(); }
+
+  // TODO(#400): Add RollPitchYaw inertial_to_backend_frame_rotation() const.
+
  protected:
   RoadGeometry() = default;
 
@@ -210,6 +223,8 @@ class RoadGeometry {
 
   virtual std::vector<InertialPosition> DoSampleAheadWaypoints(const LaneSRoute&,
                                                                double path_length_sampling_rate) const;
+
+  virtual math::Vector3 do_inertial_to_backend_frame_translation() const = 0;
   ///@}
 };
 

maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h

@@ -12,8 +12,11 @@ namespace geometry_base {
 /// that exhaustively calls `ToLanePosition()` on all lanes and checks
 /// _distance_ with `radius`.
 ///
-/// Maliput backends could avoid implementing a custom implementation that knows
-/// about the geometry internals by forwarding calls to this function.
+/// @note Maliput backends could avoid implementing a custom implementation of
+/// RoadGeometry::FindRoadPositions() that knows about the geometry internals by
+/// forwarding calls to this function. On the contrary, backends might decide
+/// not to use this function because, for example, time complexity which is at
+/// least O(n^3).
 ///
 /// @param rg The RoadGeometry over all these operations are performed. It
 ///        must not be nullptr.

maliput/include/maliput/geometry_base/lane.h

@@ -4,6 +4,7 @@
 
 #include "maliput/api/branch_point.h"
 #include "maliput/api/lane.h"
+#include "maliput/api/lane_data.h"
 #include "maliput/api/segment.h"
 #include "maliput/common/maliput_copyable.h"
 #include "maliput/common/passkey.h"
@@ -87,6 +88,70 @@ class Lane : public api::Lane {
 
   std::optional<api::LaneEnd> DoGetDefaultBranch(const api::LaneEnd::Which which_end) const override;
 
+  // @{
+  // Maps @p lane_pos into the Inertial Frame.
+  //
+  // @details Forwards the call to DoToBackendPosition(lane_pos) and translates
+  //          the result to account for api::RoadGeometry::inertial_to_backend_frame_translation().
+  //
+  // @note Because of performance constraints, backends must decide whether to
+  //       exclusively override DoToInertialPosition() or DoToBackendPosition().
+  //       When overriding DoToInertialPosition(), a better performance is
+  //       expected than the generic transform applied here.
+  virtual api::InertialPosition DoToInertialPosition(const api::LanePosition& lane_pos) const override;
+
+  // Maps @p lane_pos into the Backend Frame.
+  //
+  // @note This method implementation @throws maliput::common::assertion_error
+  //       as it should only be called when it is overridden by a derived class.
+  //       @see DoToInertialPosition() docstring to understand when to override
+  //       each method accordingly.
+  //
+  // @return The mapped @p lane_pos into the Backend Frame.
+  virtual math::Vector3 DoToBackendPosition(const api::LanePosition& lane_pos) const;
+  // @}
+
+  // @{
+  // Maps @p inertial_pos (measured in the Inertial Frame) into this Lane Frame.
+  //
+  // @details Translates @p inertial_pos with api::RoadGeometry::internal_inertial_frame_translation()
+  //          and calls DoToLanePositionBackend(). The returned values are
+  //          used to build an api::LanePositionResult. Note that
+  //          `nearest_backend_pos` is converted back to the Inertial Frame
+  //          before returning the final result.
+  //
+  // @note Because of performance constraints, backends must decide whether to
+  //       exclusively override DoToLanePosition() or DoToLanePositionBackend().
+  //       When overriding DoToLanePosition(), a better performance is
+  //       expected than the generic transform applied here.
+  virtual api::LanePositionResult DoToLanePosition(const api::InertialPosition& inertial_pos) const override;
+
+  // Maps @p backend_pos (measured in the Backend Frame) into this Lane Frame.
+  //
+  // @note This method implementation @throws maliput::common::assertion_error
+  //       as it should only be called when it is overridden by a derived class.
+  //       @see DoToLanePosition() docstring to understand when to override each
+  //       method accordingly.
+  //
+  // @param backend_pos The Backend Frame coordinate to map into this Lane
+  //        Frame.
+  // @param lane_pos The candidate LanePosition within the Lane' segment-
+  //        bounds which is closest closest to @p backend_pos. It must not be
+  //        nullptr.
+  // @param nearest_backend_pos The position that exactly corresponds to
+  //        @p lane_pos. It must not be nullptr.
+  // @param distance The Cartesian distance between `nearest_position` and the
+  //        Inertial Frame position supplied to Lane::ToLanePosition() and then
+  //        converted into the Backend Frame. It must not be nullptr.
+  //
+  // @throws When any of @p backend_pos, @p lane_pos, or @p distance is
+  //         nullptr.
+  //
+  // @return The @p lane_pos, @p nearest_backend_pos and @p distance.
+  virtual void DoToLanePositionBackend(const math::Vector3& backend_pos, api::LanePosition* lane_pos,
+                                       math::Vector3* nearest_backend_pos, double* distance) const;
+  // @}
+
   const api::LaneId id_;
   const api::Segment* segment_{};
   int index_{-1};

maliput/include/maliput/geometry_base/road_geometry.h

@@ -13,6 +13,7 @@
 #include "maliput/common/maliput_throw.h"
 #include "maliput/geometry_base/branch_point.h"
 #include "maliput/geometry_base/junction.h"
+#include "maliput/math/vector.h"
 
 namespace maliput {
 namespace geometry_base {
@@ -46,14 +47,18 @@ class RoadGeometry : public api::RoadGeometry {
   /// @param linear_tolerance the linear tolerance
   /// @param angular_tolerance the angular tolerance
   /// @param scale_length the scale length
+  /// @param inertial_to_backend_frame_translation the Inertial Frame to Backend
+  ///        Frame translation vector
   ///
   /// @throws maliput::common::assertion_error if either `linear_tolerance` or
   ///         `angular_tolerance` or `scale_length` is non-positive.
-  RoadGeometry(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, double scale_length)
+  RoadGeometry(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, double scale_length,
+               const math::Vector3& inertial_to_backend_frame_translation)
       : id_(id),
         linear_tolerance_(linear_tolerance),
         angular_tolerance_(angular_tolerance),
-        scale_length_(scale_length) {
+        scale_length_(scale_length),
+        inertial_to_backend_frame_translation_(inertial_to_backend_frame_translation) {
     MALIPUT_THROW_UNLESS(linear_tolerance_ > 0.);
     MALIPUT_THROW_UNLESS(angular_tolerance_ > 0.);
     MALIPUT_THROW_UNLESS(scale_length_ > 0.);
@@ -122,10 +127,15 @@ class RoadGeometry : public api::RoadGeometry {
 
   double do_scale_length() const override { return scale_length_; }
 
+  math::Vector3 do_inertial_to_backend_frame_translation() const override {
+    return inertial_to_backend_frame_translation_;
+  }
+
   api::RoadGeometryId id_;
   double linear_tolerance_{};
   double angular_tolerance_{};
   double scale_length_{};
+  math::Vector3 inertial_to_backend_frame_translation_{};
   std::vector<std::unique_ptr<Junction>> junctions_;
   std::vector<std::unique_ptr<BranchPoint>> branch_points_;
   api::BasicIdIndex id_index_;

maliput/include/maliput/test_utilities/mock.h

@@ -51,6 +51,7 @@ struct RoadGeometryContiguityBuildFlags {
   bool add_angular_mismatch{false};
   double linear_tolerance{0};
   double angular_tolerance{0};
+  math::Vector3 inertial_to_backend_frame_translation{0., 0., 0.};
 };
 
 /// Holds RoadRulebook contiguity build configuration.
@@ -301,8 +302,12 @@ class MockRoadGeometry : public RoadGeometry {
  public:
   MALIPUT_NO_COPY_NO_MOVE_NO_ASSIGN(MockRoadGeometry)
   MockRoadGeometry(const RoadGeometryId& id) : id_(id) {}
-  MockRoadGeometry(const RoadGeometryId& id, const double& linear_tolerance, const double& angular_tolerance)
-      : id_(id), linear_tolerance_(linear_tolerance), angular_tolerance_(angular_tolerance) {}
+  MockRoadGeometry(const RoadGeometryId& id, const double& linear_tolerance, const double& angular_tolerance,
+                   const math::Vector3& inertial_to_backend_frame_translation)
+      : id_(id),
+        linear_tolerance_(linear_tolerance),
+        angular_tolerance_(angular_tolerance),
+        inertial_to_backend_frame_translation_(inertial_to_backend_frame_translation) {}
 
   void add_junction(std::unique_ptr<MockJunction> junction) { junctions_.push_back(std::move(junction)); }
   void set_start_bp(std::unique_ptr<MockBranchPoint> start_bp) { start_bp_ = std::move(start_bp); }
@@ -341,11 +346,15 @@ class MockRoadGeometry : public RoadGeometry {
   double do_linear_tolerance() const override { return linear_tolerance_; }
   double do_angular_tolerance() const override { return angular_tolerance_; }
   double do_scale_length() const override { return 0; }
+  math::Vector3 do_inertial_to_backend_frame_translation() const override {
+    return inertial_to_backend_frame_translation_;
+  }
 
   MockIdIndex mock_id_index_;
   RoadGeometryId id_;
   double linear_tolerance_{0};
   double angular_tolerance_{0};
+  math::Vector3 inertial_to_backend_frame_translation_{};
   std::vector<std::unique_ptr<MockJunction>> junctions_;
   std::unique_ptr<MockBranchPoint> start_bp_;
   std::unique_ptr<MockBranchPoint> end_bp_;

maliput/include/maliput/test_utilities/mock_geometry.h

@@ -9,6 +9,7 @@
 #include "maliput/geometry_base/lane.h"
 #include "maliput/geometry_base/road_geometry.h"
 #include "maliput/geometry_base/segment.h"
+#include "maliput/math/vector.h"
 
 namespace maliput {
 namespace geometry_base {
@@ -43,12 +44,15 @@ class MockRoadGeometry : public geometry_base::RoadGeometry {
   /// @param linear_tolerance the linear tolerance
   /// @param angular_tolerance the angular tolerance
   /// @param scale_length the scale length
+  /// @param inertial_to_backend_frame_translation the Inertial to Backend Frame
+  ///        translation
   ///
   /// @throws std::exception if either `linear_tolerance` or
   ///         `angular_tolerance` or `scale_length` is non-positive.
   MockRoadGeometry(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance,
-                   double scale_length)
-      : geometry_base::RoadGeometry(id, linear_tolerance, angular_tolerance, scale_length) {}
+                   double scale_length, const math::Vector3& inertial_to_backend_frame_translation)
+      : geometry_base::RoadGeometry(id, linear_tolerance, angular_tolerance, scale_length,
+                                    inertial_to_backend_frame_translation) {}
 
  private:
   api::RoadPositionResult DoToRoadPosition(const api::InertialPosition& inertial_position,

maliput/src/geometry_base/lane.cc

@@ -83,5 +83,37 @@ std::optional<api::LaneEnd> Lane::DoGetDefaultBranch(api::LaneEnd::Which which_e
   return GetBranchPoint(which_end)->GetDefaultBranch({this, which_end});
 }
 
+api::InertialPosition Lane::DoToInertialPosition(const api::LanePosition& lane_pos) const {
+  const math::Vector3 backend_pos = DoToBackendPosition(lane_pos);
+  return api::InertialPosition::FromXyz(
+      backend_pos + segment()->junction()->road_geometry()->inertial_to_backend_frame_translation());
+}
+
+math::Vector3 Lane::DoToBackendPosition(const api::LanePosition& lane_pos) const {
+  MALIPUT_THROW_MESSAGE(
+      "Unimplemented method. Please check the documentation of "
+      "maliput::geometry_base::Lane::DoToInertialPosition().");
+}
+
+api::LanePositionResult Lane::DoToLanePosition(const api::InertialPosition& inertial_pos) const {
+  const math::Vector3 inertial_to_backend_frame_translation =
+      segment()->junction()->road_geometry()->inertial_to_backend_frame_translation();
+  const math::Vector3 backend_pos = inertial_pos.xyz() - inertial_to_backend_frame_translation;
+
+  api::LanePosition lane_pos{};
+  math::Vector3 nearest_backend_pos{};
+  double distance{};
+  DoToLanePositionBackend(backend_pos, &lane_pos, &nearest_backend_pos, &distance);
+  return {lane_pos, api::InertialPosition::FromXyz(nearest_backend_pos + inertial_to_backend_frame_translation),
+          distance};
+}
+
+void Lane::DoToLanePositionBackend(const math::Vector3& backend_pos, api::LanePosition* lane_position,
+                                   math::Vector3* nearest_backend_pos, double* distance) const {
+  MALIPUT_THROW_MESSAGE(
+      "Unimplemented method. Please check the documentation of "
+      "maliput::geometry_base::Lane::DoToLanePosition().");
+}
+
 }  // namespace geometry_base
 }  // namespace maliput

maliput/src/test_utilities/mock.cc

@@ -72,6 +72,8 @@ class MockOneLaneRoadGeometry final : public RoadGeometry {
   double do_linear_tolerance() const override { return 0; }
   double do_angular_tolerance() const override { return 0; }
   double do_scale_length() const override { return 0; }
+  math::Vector3 do_inertial_to_backend_frame_translation() const override { return math::Vector3{0., 0., 0.}; }
+
   MockOneLaneIdIndex mock_id_index_;
 };
 
@@ -533,9 +535,10 @@ std::unique_ptr<RoadGeometry> CreateRoadGeometry(const RoadGeometryBuildFlags& b
 // Return values when ToLanePosition method is called are hardcoded.
 std::unique_ptr<RoadGeometry> CreateTwoLanesRoadGeometry() {
   constexpr double kArbitrary{1.};
+  const math::Vector3 kZero{0., 0., 0.};
   const LanePositionResult lane_position_result_a{{10., 20., 30.}, {12., 89., 1.}, 0.5};
   const LanePositionResult lane_position_result_b{{40., 50., 60.}, {50., 1., 45.}, 30.};
-  auto rg = std::make_unique<MockRoadGeometry>(RoadGeometryId("road_geometry"), kArbitrary, kArbitrary);
+  auto rg = std::make_unique<MockRoadGeometry>(RoadGeometryId("road_geometry"), kArbitrary, kArbitrary, kZero);
   auto junction_a = std::make_unique<MockJunction>(JunctionId("junction_a"));
   auto junction_b = std::make_unique<MockJunction>(JunctionId("junction_b"));
   auto segment_a = std::make_unique<MockSegment>(SegmentId("segment_a"));
@@ -564,7 +567,8 @@ std::unique_ptr<RoadGeometry> CreateTwoLanesRoadGeometry() {
 std::unique_ptr<RoadGeometry> CreateMockContiguousRoadGeometry(const RoadGeometryContiguityBuildFlags& build_flags) {
   // Creates the road geometry.
   auto rg = std::make_unique<MockRoadGeometry>(RoadGeometryId("mock"), build_flags.linear_tolerance,
-                                               build_flags.angular_tolerance);
+                                               build_flags.angular_tolerance,
+                                               build_flags.inertial_to_backend_frame_translation);
   // Creates the Junctions.
   auto junction_a = std::make_unique<MockJunction>(JunctionId("mock_a"));
   junction_a->set_road_geometry(rg.get());

maliput/test/api/lane_test.cc

@@ -30,6 +30,7 @@ class LaneTest : public ::testing::Test {
   const double s{1};
   const double r{2};
   const double h{3};
+  const math::Vector3 inertial_to_backend_frame_translation{0., 0., 0.};
 };
 
 class LaneMock final : public geometry_base::test::MockLane {
@@ -55,8 +56,9 @@ class LaneMock final : public geometry_base::test::MockLane {
 class RoadGeometryMock final : public geometry_base::test::MockRoadGeometry {
  public:
   explicit RoadGeometryMock(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance,
-                            double scale_length)
-      : MockRoadGeometry(id, linear_tolerance, angular_tolerance, scale_length) {}
+                            double scale_length, const math::Vector3& inertial_to_backend_frame_translation)
+      : MockRoadGeometry(id, linear_tolerance, angular_tolerance, scale_length, inertial_to_backend_frame_translation) {
+  }
   void set_lanes(std::vector<LaneMock*> lanes) { lanes_.assign(lanes.begin(), lanes.end()); }
   std::vector<LaneMock*> get_lanes() { return lanes_; }
 
@@ -65,8 +67,10 @@ class RoadGeometryMock final : public geometry_base::test::MockRoadGeometry {
 };
 
 std::unique_ptr<RoadGeometryMock> MakeFullRoadGeometry(const api::RoadGeometryId& id, double linear_tolerance,
-                                                       double angular_tolerance, double scale_length) {
-  auto road_geometry = std::make_unique<RoadGeometryMock>(id, linear_tolerance, angular_tolerance, scale_length);
+                                                       double angular_tolerance, double scale_length,
+                                                       const math::Vector3& inertial_to_backend_frame_translation) {
+  auto road_geometry = std::make_unique<RoadGeometryMock>(id, linear_tolerance, angular_tolerance, scale_length,
+                                                          inertial_to_backend_frame_translation);
   std::vector<LaneMock*> lanes;
 
   auto lane0 = std::make_unique<LaneMock>(api::LaneId("lane0"));
@@ -99,7 +103,7 @@ TEST_F(LaneTest, Contains) {
   const LanePosition false_lane_position = LanePosition(s + kLaneLength + linear_tolerance, r, h);
 
   auto rg = MakeFullRoadGeometry(api::RoadGeometryId("mock_road_geometry"), linear_tolerance, angular_tolerance,
-                                 scale_length);
+                                 scale_length, inertial_to_backend_frame_translation);
 
   const std::vector<LaneMock*> lanes = rg.get()->get_lanes();
 

maliput/test/api/road_network_test.cc

@@ -32,6 +32,7 @@ class RoadNetworkTest : public ::testing::Test {
   const double linear_tolerance{1.};
   const double angular_tolerance{1.};
   const double scale_length{1.};
+  const math::Vector3 inertial_to_backend_frame_translation{0., 0., 0.};
 
   virtual void SetUp() {
     road_geometry_ = test::CreateRoadGeometry();
@@ -175,7 +176,8 @@ TEST_F(RoadNetworkTest, TestMemberMethodAccess) {
 
 TEST_F(RoadNetworkTest, Contains) {
   auto mock_road_geometry = std::make_unique<geometry_base::test::MockRoadGeometry>(
-      api::RoadGeometryId{"mock_road_geometry"}, linear_tolerance, angular_tolerance, scale_length);
+      api::RoadGeometryId{"mock_road_geometry"}, linear_tolerance, angular_tolerance, scale_length,
+      inertial_to_backend_frame_translation);
   auto mock_lane = std::make_unique<geometry_base::test::MockLane>(api::LaneId{"mock_lane"});
   auto mock_segment = std::make_unique<geometry_base::test::MockSegment>(api::SegmentId{"mock_segment"});
   auto mock_junction = std::make_unique<geometry_base::test::MockJunction>(api::JunctionId{"mock_junction"});