From c76d3f72edfdac4734a7e470babf9c0f1e0f2bdb Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 13 Feb 2023 17:38:14 +0900 Subject: [PATCH] feat(lanelet2_extension): copy the z values in getExpandedLanelet (#141) * feat(lanelet2_extension): copy the z values in getExpandedLanelet Signed-off-by: Maxime CLEMENT * Make templated function and add tests Signed-off-by: Maxime CLEMENT * Fix clang-tidy warning for new gtest Signed-off-by: Maxime CLEMENT * Fix clang-tidy warnings caused by gtest asserts Signed-off-by: Maxime CLEMENT --------- Signed-off-by: Maxime CLEMENT --- .../lanelet2_extension/utility/utilities.hpp | 30 ++++++++++++- tmp/lanelet2_extension/lib/utilities.cpp | 13 +++--- .../test/src/test_utilities.cpp | 45 ++++++++++++++++++- 3 files changed, 78 insertions(+), 10 deletions(-) diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp index a31d9d69..08b8e4c1 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp @@ -1,4 +1,4 @@ -// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// Copyright 2015-2023 Autoware Foundation. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -46,6 +47,33 @@ lanelet::ConstLanelet getExpandedLanelet( lanelet::ConstLanelets getExpandedLanelets( const lanelet::ConstLanelets & lanelet_obj, const double left_offset, const double right_offset); +/// @brief copy the z values between 2 containers based on the 2D arc lengths +/// @tparam T1 a container of 3D points +/// @tparam T2 a container of 3D points +/// @param from points from which the z values will be copied +/// @param to points to which the z values will be copied +template +void copyZ(const T1 & from, T2 & to) +{ + if (from.empty() || to.empty()) return; + to.front().z() = from.front().z(); + if (from.size() < 2 || to.size() < 2) return; + to.back().z() = from.back().z(); + auto i_from = 1lu; + auto s_from = lanelet::geometry::distance2d(from[0], from[1]); + auto s_to = 0.0; + auto s_from_prev = 0.0; + for (auto i_to = 1lu; i_to + 1 < to.size(); ++i_to) { + s_to += lanelet::geometry::distance2d(to[i_to - 1], to[i_to]); + for (; s_from < s_to && i_from + 1 < from.size(); ++i_from) { + s_from_prev = s_from; + s_from += lanelet::geometry::distance2d(from[i_from], from[i_from + 1]); + } + const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev); + to[i_to].z() = from[i_from - 1].z() + ratio * (from[i_from].z() - from[i_from - 1].z()); + } +} + /** * @brief Apply a patch for centerline because the original implementation * doesn't have enough quality diff --git a/tmp/lanelet2_extension/lib/utilities.cpp b/tmp/lanelet2_extension/lib/utilities.cpp index 808f8f5a..b22bcf50 100644 --- a/tmp/lanelet2_extension/lib/utilities.cpp +++ b/tmp/lanelet2_extension/lib/utilities.cpp @@ -434,19 +434,18 @@ lanelet::ConstLanelet getExpandedLanelet( orig_left_bound_2d.back().y() - left_offset * std::sin(theta); } - const auto toPoints3d = [](const lanelet::BasicLineString2d & ls2d, const double z) { + const auto toPoints3d = [](const lanelet::BasicLineString2d & ls2d) { lanelet::Points3d output; for (const auto & pt : ls2d) { - output.push_back(lanelet::Point3d(lanelet::InvalId, pt.x(), pt.y(), z)); + output.push_back(lanelet::Point3d(lanelet::InvalId, pt.x(), pt.y(), 0.0)); } return output; }; - // Original z value cannot be used directly since the offset function can vary the points size. - const lanelet::Points3d ex_lefts = - toPoints3d(expanded_left_bound_2d, lanelet_obj.leftBound3d().basicLineString().at(0).z()); - const lanelet::Points3d ex_rights = - toPoints3d(expanded_right_bound_2d, lanelet_obj.rightBound3d().basicLineString().at(0).z()); + lanelet::Points3d ex_lefts = toPoints3d(expanded_left_bound_2d); + lanelet::Points3d ex_rights = toPoints3d(expanded_right_bound_2d); + copyZ(lanelet_obj.leftBound3d(), ex_lefts); + copyZ(lanelet_obj.rightBound3d(), ex_rights); const auto & extended_left_bound_3d = lanelet::LineString3d(lanelet::InvalId, ex_lefts); const auto & expanded_right_bound_3d = lanelet::LineString3d(lanelet::InvalId, ex_rights); diff --git a/tmp/lanelet2_extension/test/src/test_utilities.cpp b/tmp/lanelet2_extension/test/src/test_utilities.cpp index 10103d55..de310981 100644 --- a/tmp/lanelet2_extension/test/src/test_utilities.cpp +++ b/tmp/lanelet2_extension/test/src/test_utilities.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -// NOLINTBEGIN(readability-identifier-naming) +// NOLINTBEGIN(readability-identifier-naming, cppcoreguidelines-avoid-goto) #include "lanelet2_extension/utility/utilities.hpp" @@ -114,10 +114,51 @@ TEST_F(TestSuite, OverwriteLaneletsCenterline) // NOLINT for gtest } } +TEST(Utilities, copyZ) // NOLINT for gtest +{ + using lanelet::utils::copyZ; + + LineString3d ls1; + LineString3d ls2; + + ASSERT_NO_THROW(copyZ(ls1, ls2)); + ls1.push_back(Point3d(lanelet::InvalId, 0.0, 0.0, 5.0)); + ASSERT_NO_THROW(copyZ(ls1, ls2)); + ls2.push_back(Point3d(lanelet::InvalId, 0.0, 0.0, 0.0)); + ASSERT_NO_THROW(copyZ(ls1, ls2)); + EXPECT_EQ(ls2.front().z(), ls1.front().z()); + ls1.push_back(Point3d(lanelet::InvalId, 1.0, 0.0, 2.0)); + ls2.push_back(Point3d(lanelet::InvalId, 3.0, 0.0, 0.0)); + ASSERT_NO_THROW(copyZ(ls1, ls2)); + EXPECT_EQ(ls2.front().z(), ls1.front().z()); + EXPECT_EQ(ls2.back().z(), ls1.back().z()); + + ls1.push_back(Point3d(lanelet::InvalId, 2.0, 0.0, 0.0)); + ls1.push_back(Point3d(lanelet::InvalId, 3.0, 0.0, 3.0)); + lanelet::Points3d points; + points.emplace_back(lanelet::InvalId, 0.0, 0.0, 0.0); + points.emplace_back(lanelet::InvalId, 0.0, 0.5, 0.0); + points.emplace_back(lanelet::InvalId, 0.0, 1.0, 0.0); + points.emplace_back(lanelet::InvalId, 0.0, 1.5, 0.0); + points.emplace_back(lanelet::InvalId, 0.0, 2.0, 0.0); + points.emplace_back(lanelet::InvalId, 0.0, 2.5, 0.0); + points.emplace_back(lanelet::InvalId, 0.0, 3.0, 0.0); + points.emplace_back(lanelet::InvalId, 0.0, 3.5, 0.0); + ASSERT_NO_THROW(copyZ(ls1, points)); + EXPECT_EQ(points[0].z(), 5.0); + EXPECT_EQ(points[1].z(), 3.5); + EXPECT_EQ(points[2].z(), 2.0); + EXPECT_EQ(points[3].z(), 1.0); + EXPECT_EQ(points[4].z(), 0.0); + EXPECT_EQ(points[5].z(), 1.5); + EXPECT_EQ(points[6].z(), 3.0); + EXPECT_EQ(points[7].z(), 3.0); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } -// NOLINTEND(readability-identifier-naming) +// NOLINTEND(readability-identifier-naming, cppcoreguidelines-avoid-goto)