From 1b7c600e82cacdeaf540f7ff5bce1523c6c2af9a Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Mon, 3 Jun 2024 10:38:55 +0900 Subject: [PATCH] feat: remove autoware_auto_geometry package (#7217) * feat!: remove autoware_auto_geometry package Signed-off-by: Ryohsuke Mitsudome * docs: remove autoware_auto_geometry package from docs Signed-off-by: Ryohsuke Mitsudome --------- Signed-off-by: Ryohsuke Mitsudome --- .github/CODEOWNERS | 1 - common/.pages | 4 - common/autoware_auto_geometry/CMakeLists.txt | 39 -- .../autoware_auto_geometry/design/interval.md | 111 ---- .../design/polygon_intersection_2d-design.md | 51 -- .../design/spatial-hash-design.md | 99 --- .../bounding_box/bounding_box_common.hpp | 188 ------ .../bounding_box/eigenbox_2d.hpp | 247 -------- .../bounding_box/lfit.hpp | 281 --------- .../bounding_box/rotating_calipers.hpp | 280 --------- .../bounding_box_2d.hpp | 34 - .../autoware_auto_geometry/common_2d.hpp | 587 ------------------ .../autoware_auto_geometry/common_3d.hpp | 77 --- .../autoware_auto_geometry/convex_hull.hpp | 195 ------ .../autoware_auto_geometry/hull_pockets.hpp | 111 ---- .../autoware_auto_geometry/intersection.hpp | 312 ---------- .../autoware_auto_geometry/interval.hpp | 358 ----------- .../autoware_auto_geometry/lookup_table.hpp | 179 ------ .../autoware_auto_geometry/spatial_hash.hpp | 332 ---------- .../spatial_hash_config.hpp | 450 -------------- .../visibility_control.hpp | 41 -- common/autoware_auto_geometry/package.xml | 30 - .../src/bounding_box.cpp | 157 ----- .../src/spatial_hash.cpp | 104 ---- .../test/include/test_bounding_box.hpp | 550 ---------------- .../test/include/test_spatial_hash.hpp | 260 -------- .../test/src/lookup_table.cpp | 166 ----- .../test/src/test_area.cpp | 132 ---- .../test/src/test_common_2d.cpp | 197 ------ .../test/src/test_convex_hull.cpp | 372 ----------- .../test/src/test_geometry.cpp | 25 - .../test/src/test_hull_pockets.cpp | 182 ------ .../test/src/test_intersection.cpp | 155 ----- .../test/src/test_interval.cpp | 259 -------- .../package.xml | 1 - 35 files changed, 6567 deletions(-) delete mode 100644 common/autoware_auto_geometry/CMakeLists.txt delete mode 100644 common/autoware_auto_geometry/design/interval.md delete mode 100644 common/autoware_auto_geometry/design/polygon_intersection_2d-design.md delete mode 100644 common/autoware_auto_geometry/design/spatial-hash-design.md delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp delete mode 100644 common/autoware_auto_geometry/package.xml delete mode 100644 common/autoware_auto_geometry/src/bounding_box.cpp delete mode 100644 common/autoware_auto_geometry/src/spatial_hash.cpp delete mode 100644 common/autoware_auto_geometry/test/include/test_bounding_box.hpp delete mode 100644 common/autoware_auto_geometry/test/include/test_spatial_hash.hpp delete mode 100644 common/autoware_auto_geometry/test/src/lookup_table.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_area.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_common_2d.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_convex_hull.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_geometry.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_hull_pockets.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_intersection.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_interval.cpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 5f5d5008c0f1c..e60173068f028 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,7 +1,6 @@ ### Automatically generated from package.xml ### common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -common/autoware_auto_geometry/** satoshi.ota@tier4.jp common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp common/autoware_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai diff --git a/common/.pages b/common/.pages index f1ecfc71f96b6..5ce88e0561c34 100644 --- a/common/.pages +++ b/common/.pages @@ -6,10 +6,6 @@ nav: - 'Common Libraries': - 'autoware_auto_common': - 'comparisons': common/autoware_auto_common/design/comparisons - - 'autoware_auto_geometry': - - 'interval': common/autoware_auto_geometry/design/interval - - 'polygon intersection 2d': common/autoware_auto_geometry/design/polygon_intersection_2d-design - - 'spatial hash': common/autoware_auto_geometry/design/spatial-hash-design - 'autoware_auto_tf2': common/autoware_auto_tf2/design/autoware-auto-tf2-design - 'autoware_point_types': common/autoware_point_types - 'Cuda Utils': common/cuda_utils diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt deleted file mode 100644 index ee819b7cd797c..0000000000000 --- a/common/autoware_auto_geometry/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_auto_geometry) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - include/autoware_auto_geometry/spatial_hash.hpp - include/autoware_auto_geometry/intersection.hpp - include/autoware_auto_geometry/spatial_hash_config.hpp - src/spatial_hash.cpp - src/bounding_box.cpp -) - -if(BUILD_TESTING) - set(GEOMETRY_GTEST geometry_gtest) - set(GEOMETRY_SRC test/src/test_geometry.cpp - test/src/test_convex_hull.cpp - test/src/test_hull_pockets.cpp - test/src/test_interval.cpp - test/src/lookup_table.cpp - test/src/test_area.cpp - test/src/test_common_2d.cpp - test/src/test_intersection.cpp - ) - ament_add_ros_isolated_gtest(${GEOMETRY_GTEST} ${GEOMETRY_SRC}) - target_compile_options(${GEOMETRY_GTEST} PRIVATE -Wno-conversion -Wno-sign-conversion) - target_include_directories(${GEOMETRY_GTEST} PRIVATE "test/include" "include") - ament_target_dependencies(${GEOMETRY_GTEST} - "autoware_auto_common" - "autoware_auto_geometry_msgs" - "autoware_auto_planning_msgs" - "autoware_auto_vehicle_msgs" - "geometry_msgs" - "osrf_testing_tools_cpp") - target_link_libraries(${GEOMETRY_GTEST} ${PROJECT_NAME}) -endif() - -ament_auto_package() diff --git a/common/autoware_auto_geometry/design/interval.md b/common/autoware_auto_geometry/design/interval.md deleted file mode 100644 index 4fe65ff8e0310..0000000000000 --- a/common/autoware_auto_geometry/design/interval.md +++ /dev/null @@ -1,111 +0,0 @@ -# Interval - -The interval is a standard 1D real-valued interval. -The class implements a representation and operations on the interval type and guarantees interval validity on construction. -Basic operations and accessors are implemented, as well as other common operations. -See 'Example Usage' below. - -## Target use cases - -- Range or containment checks. - The interval class simplifies code that involves checking membership of a value to a range, or intersecting two ranges. - It also provides consistent behavior and consistent handling of edge cases. - -## Properties - -- **empty**: An empty interval is equivalent to an empty set. - It contains no elements. - It is a valid interval, but because it is empty, the notion of measure (length) is undefined; the measure of an empty interval is _not_ zero. - The implementation represents the measure of an empty interval with `NaN`. -- **zero measure**: An interval with zero measure is an interval whose bounds are exactly equal. - The measure is zero because the interval contains only a single point, and points have zero measure. - However, because it does contain a single element, the interval is _not_ empty. -- **valid**: A valid interval is either empty or has min/max bounds such that (min <= max). On construction, interval objects are guaranteed to be valid. - An attempt to construct an invalid interval results in a runtime_error exception being thrown. -- **pseudo-immutable**: Once constructed the only way to change the value of an interval is to overwrite it with a new one; an existing object cannot be modified. - -## Conventions - -- All operations on interval objects are defined as static class methods on the interval class. - This is a functional-style of programming that basically turns the class into a namespace that grants functions access to private member variables of the object they operate on. - -## Assumptions - -- The interval is only intended for floating point types. - This is enforced via static assertion. -- The constructor for non-empty intervals takes two arguments 'min' and 'max', and they must be ordered (i.e., min <= max). - If this assumption is violated, an exception is emitted and construction fails. - -## Example Usage - -```c++ -#include "autoware_auto_geometry/interval.hpp" - -#include - -// using-directive is just for illustration; don't do this in practice -using namespace autoware::common::geometry; - -// bounds for example interval -constexpr auto MIN = 0.0; -constexpr auto MAX = 1.0; - -// -// Try to construct an invalid interval. This will give the following error: -// 'Attempted to construct an invalid interval: {"min": 1.0, "max": 0.0}' -// - -try { - const auto i = Interval_d(MAX, MIN); -} catch (const std::runtime_error& e) { - std::cerr << e.what(); -} - -// -// Construct a double precision interval from 0 to 1 -// - -const auto i = Interval_d(MIN, MAX); - -// -// Test accessors and properties -// - -std::cout << Interval_d::min(i) << " " << Interval_d::max(i) << "\n"; -// Prints: 0.0 1.0 - -std::cout << Interval_d::empty(i) << " " << Interval_d::length(i) << "\n"; -// Prints: false 1.0 - -std::cout << Interval_d::contains(i, 0.3) << "\n"; -// Prints: true - -std::cout << Interval_d::is_subset_eq(Interval_d(0.2, 0.4), i) << "\n"; -// Prints: true - -// -// Test operations. -// - -std::cout << Interval_d::intersect(i, Interval(-1.0, 0.3)) << "\n"; -// Prints: {"min": 0.0, "max": 0.3} - -std::cout << Interval_d::project_to_interval(i, 0.5) << " " - << Interval_d::project_to_interval(i, -1.3) << "\n"; -// Prints: 0.5 0.0 - -// -// Distinguish empty/zero measure -// - -const auto i_empty = Interval(); -const auto i_zero_length = Interval(0.0, 0.0); - -std::cout << Interval_d::empty(i_empty) << " " - << Interval_d::empty(i_zero_length) << "\n"; -// Prints: true false - -std::cout << Interval_d::zero_measure(i_empty) << " " - << Interval_d::zero_measure(i_zero_length) << "\n"; -// Prints: false false -``` diff --git a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md b/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md deleted file mode 100644 index f651c218bc80d..0000000000000 --- a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md +++ /dev/null @@ -1,51 +0,0 @@ -# 2D Convex Polygon Intersection - -Two convex polygon's intersection can be visualized on the image below as the blue area: - - - -## Purpose / Use cases - -Computing the intersection between two polygons can be useful in many applications of scene -understanding. It can be used to estimate collision detection, shape alignment, shape -association and in any application that deals with the objects around the perceiving agent. - -## Design - -[\(Livermore, Calif, 1977\)](https://doi.org/10.2172/7309916) mention the following -observations about convex polygon intersection: - -- Intersection of two convex polygons is a convex polygon -- A vertex from a polygon that is contained in the other polygon is a vertex of the intersection - shape. (Vertices A, C, D in the shape above) -- An edge from a polygon that is contained in the other polygon is an edge in the intersection - shape. (edge C-D in the shape above) -- Edge intersections between two polygons are vertices in the intersection shape. (Vertices B, - E in the shape above.) - -### Inner-workings / Algorithms - -With the observation mentioned above, the current algorithm operates in the following way: - -- Compute and find the vertices from each polygon that is contained in the other polygon - (Vertices A, C, D) -- Compute and find the intersection points between each polygon (Vertices B, E) -- Compute the convex hull shaped by these vertices by ordering them CCW. - -### Inputs / Outputs / API - -Inputs: - -- Two iterables that contain vertices of the convex polygons ordered in the CCW direction. - -Outputs: - -- A list of vertices of the intersection shape ordered in the CCW direction. - -## Future Work - -- #1230: Applying efficient algorithms. - -## Related issues - -- #983: Integrate vision detections in object tracker diff --git a/common/autoware_auto_geometry/design/spatial-hash-design.md b/common/autoware_auto_geometry/design/spatial-hash-design.md deleted file mode 100644 index 58eecf3ee841b..0000000000000 --- a/common/autoware_auto_geometry/design/spatial-hash-design.md +++ /dev/null @@ -1,99 +0,0 @@ -# Spatial Hash - -The spatial hash is a data structure designed for efficient fixed-radius near-neighbor queries in -low dimensions. - -The fixed-radius near-neighbors problem is defined as follows: - -`For point p, find all points p' s.t. d(p, p') < r` - -Where in this case `d(p, p')` is euclidean distance, and `r` is the fixed -radius. - -For `n` points with an average of `k` neighbors each, this data structure can -perform `m` near-neighbor queries (to generate lists of near-neighbors for `m` -different points) in `O(mk)` time. - -By contrast, using a k-d tree for successive nearest-neighbor queries results in -a running time of `O(m log n)`. - -The spatial hash works as follows: - -- Each point is assigned to a bin in the predefined bounding area defined by - `x_min/x_max` and `y_min/y_max` -- This can be done by converting x and y position into x and y index - respectively - - For example with the bin containing `x_min` and `y_min` as index `(0, 0)` - - The two (or more) indices can then be converted into a single index -- Once every point of interest has been inserted into the hash, near-neighbor - queries can begin: - - The bin of the reference point is first computed - - For each point in each adjacent bin, perform an explicit distance computation - between said point and the reference point. If the distance is below the given - radius, said point is considered to be a near-neighbor - -Under the hood, an `std::unordered_multimap` is used, where the key is a bin/voxel index. -The bin size was computed to be the same as the lookup distance. - - - -In addition, this data structure can support 2D or 3D queries. This is determined during -configuration, and baked into the data structure via the configuration class. The purpose of -this was to avoid if statements in tight loops. The configuration class specializations themselves -use CRTP (Curiously Recurring Template Patterns) to do "static polymorphism", and avoid -a dispatching call. - -## Performance characterization - -### Time - -Insertion is `O(n)` because lookup time for the underlying hashmap is `O(n)` for -hashmaps. In practice, lookup time for hashmaps and thus insertion time should -be `O(1)`. - -Removing a point is `O(1)` because the current API only supports removal via -direct reference to a node. - -Finding `k` near-neighbors is worst case `O(n)` in the case of an adversarial -example, but in practice `O(k)`. - -### Space - -The module consists of the following components: - -- The internal hashmap is `O(n + n + A * n)`, where `A` is an arbitrary - constant (load factor) -- The other components of the spatial hash are `O(n + n)` - -This results in `O(n)` space complexity. - -## States - -The spatial hash's state is dictated by the status of the underlying unordered_multimap. - -The data structure is wholly configured by a -[config](@ref autoware::common::geometry::spatial_hash::Config) class. The constructor -of the class determines in the data structure accepts strictly 2D or strictly 3D queries. - -## Inputs - -The primary method of introducing data into the data structure is via the -[insert](@ref autoware::common::geometry::spatial_hash::SpatialHashBase::insert) method. - -## Outputs - -The primary method of retrieving data from the data structure is via the -[near](@ref autoware::common::geometry::spatial_hash::SpatialHash::near)\(2D -configuration\) -or [near](@ref autoware::common::geometry::spatial_hash::SpatialHash::near) -\(3D configuration\) method. - -The whole data structure can also be traversed using standard constant iterators. - -## Future Work - -- Performance tuning and optimization - -## Related issues - -- #28: Port to autoware.Auto diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp deleted file mode 100644 index 0f3a68e14d442..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp +++ /dev/null @@ -1,188 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief Common functionality for bounding box computation algorithms - -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include -#include -#include -#include - -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief Functions and types for generating enclosing bounding boxes around a set of points -namespace bounding_box -{ -using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; - -/// \brief Computes height of bounding box given a full list of points -/// \param[in] begin The start of the list of points -/// \param[in] end One past the end of the list of points -/// \param[out] box A box for which the z component of centroid, corners, and size gets filled -/// \tparam IT An iterator type, must dereference into a point type with float member z, or -/// appropriate point adapter defined -template -void compute_height(const IT begin, const IT end, BoundingBox & box) -{ - float32_t max_z = -std::numeric_limits::max(); - float32_t min_z = std::numeric_limits::max(); - for (auto it = begin; it != end; ++it) { - const float32_t z = point_adapter::z_(*it); - if (z <= min_z) { - min_z = z; - } - if (z >= max_z) { - max_z = z; - } - } - box.centroid.z = (max_z + min_z) * 0.5F; - for (auto & corner : box.corners) { - corner.z = box.centroid.z; - } - box.size.z = (max_z - min_z) * 0.5F; -} - -/// \brief Computes height of bounding box given a full list of points -/// \param[in] begin Iterator pointing to the start of the range of points -/// \param[in] end Iterator pointing the the end of the range of points -/// \param[out] shape A shape in which vertices z values and height field will be set -/// \tparam IT An iterator type, must dereference into a point type with float member z, or -/// appropriate point adapter defined -template -void compute_height(const IT begin, const IT end, autoware_auto_perception_msgs::msg::Shape & shape) -{ - float32_t max_z = -std::numeric_limits::max(); - float32_t min_z = std::numeric_limits::max(); - for (auto it = begin; it != end; ++it) { - const float32_t z = point_adapter::z_(*it); - if (z <= min_z) { - min_z = z; - } - if (z >= max_z) { - max_z = z; - } - } - for (auto & corner : shape.footprint.points) { - corner.z = min_z; - } - shape.dimensions.z = max_z - min_z; -} - -namespace details -{ -/// Templated alias for getting a type without CV or reference qualification -template -using base_type = typename std::remove_cv::type>::type; - -/// Templated alias for an array of 4 objects of type PointT -template -using Point4 = std::array; - -/// \brief Helper struct for compile time introspection of array size from -/// Ref: https://stackoverflow.com/questions/16866033 -template -struct array_size; -template -struct array_size> -{ - static std::size_t const size = N; -}; -static_assert( - array_size>::size == 4U, - "Bounding box does not have the right number of corners"); - -/// \brief Compute length, width, area of bounding box -/// \param[in] corners Corners of the current bounding box -/// \param[out] ret A point struct used to hold size of box defined by corners -void GEOMETRY_PUBLIC -size_2d(const decltype(BoundingBox::corners) & corners, geometry_msgs::msg::Point32 & ret); -/// \brief Computes centroid and orientation of a box given corners -/// \param[in] corners Array of final corners of bounding box -/// \param[out] box Message to have corners, orientation, and centroid updated -void GEOMETRY_PUBLIC -finalize_box(const decltype(BoundingBox::corners) & corners, BoundingBox & box); - -/// \brief given support points and two orthogonal directions, compute corners of bounding box -/// \tparam PointT Type of a point, must have float members x and y` -/// \tparam IT An iterator dereferenceable into PointT -/// \param[out] corners Gets filled with corner points of bounding box -/// \param[in] supports Iterators referring to support points of current bounding box -/// e.g. bounding box is touching these points -/// \param[in] directions Directions of each edge of the bounding box from each support point -template -void compute_corners( - decltype(BoundingBox::corners) & corners, const Point4 & supports, - const Point4 & directions) -{ - for (uint32_t idx = 0U; idx < corners.size(); ++idx) { - const uint32_t jdx = (idx != 3U) ? (idx + 1U) : 0U; - const auto pt = - intersection_2d(*supports[idx], directions[idx], *supports[jdx], directions[jdx]); - // TODO(c.ho) handle error - point_adapter::xr_(corners[idx]) = point_adapter::x_(pt); - point_adapter::yr_(corners[idx]) = point_adapter::y_(pt); - } -} -// TODO(c.ho) type trait enum base - -/// \brief Copy vertices of the given box into a Shape type -/// \param box Box to be converted -/// \return Shape type filled with box vertices -autoware_auto_perception_msgs::msg::Shape GEOMETRY_PUBLIC make_shape(const BoundingBox & box); - -/// \brief Copy centroid and orientation info of the box into Pose type -/// \param box BoundingBox to be converted -/// \return Pose type filled with centroid and orientation from box -geometry_msgs::msg::Pose GEOMETRY_PUBLIC make_pose(const BoundingBox & box); - -/// \brief Fill DetectedObject type with contents from a BoundingBox type -/// \param box BoundingBox to be converted -/// \return Filled DetectedObject type -autoware_auto_perception_msgs::msg::DetectedObject GEOMETRY_PUBLIC -make_detected_object(const BoundingBox & box); - -/// \brief Transform corners from object-local coordinates using the given centroid and orientation -/// \param shape_msg Msg containing corners in object-local coordinates -/// \param centroid Centroid of the polygon whose corners are defined in shape_msg -/// \param orientation Orientation of the polygon -/// \return corners transformed such that their centroid and orientation correspond to the -/// given inputs -std::vector GEOMETRY_PUBLIC get_transformed_corners( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation); - -} // namespace details -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp deleted file mode 100644 index e0f2e66e87ee5..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp +++ /dev/null @@ -1,247 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements 2D pca on a linked list of points to estimate an oriented -/// bounding box - -// cspell: ignore eigenbox, EIGENBOX -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ - -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" - -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ - -/// \brief Simplified 2d covariance matrix -struct Covariance2d -{ - /// \brief Variance in the x direction - float32_t xx; - /// \brief Variance in the y direction - float32_t yy; - /// \brief x-y covariance - float32_t xy; - /// \brief Number of points - std::size_t num_points; -}; // struct Covariance2d - -// cspell: ignore Welford -/// \brief Compute 2d covariance matrix of a list of points using Welford's online algorithm -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \return A 2d covariance matrix for all points in the list -template -Covariance2d covariance_2d(const IT begin, const IT end) -{ - Covariance2d ret{0.0F, 0.0F, 0.0F, 0U}; - float32_t ux = 0.0F; - float32_t uy = 0.0F; - float32_t num_points = 0.0F; - using point_adapter::x_; - using point_adapter::y_; - for (auto it = begin; it != end; ++it) { - ++ret.num_points; - num_points = static_cast(ret.num_points); - const auto & pt = *it; - // update mean x - const float32_t dx = x_(pt) - ux; - ux = ux + (dx / num_points); - // update cov - const float32_t dy = y_(pt) - uy; - ret.xy += (x_(pt) - ux) * (dy); - // update mean y - uy = uy + (dy / num_points); - // update M2 - ret.xx += dx * (x_(pt) - ux); - ret.yy += dy * (y_(pt) - uy); - } - // finalize sample (co-)variance - if (ret.num_points > 1U) { - num_points = num_points - 1.0F; - } - ret.xx /= num_points; - ret.yy /= num_points; - ret.xy /= num_points; - - return ret; -} - -/// \brief Compute eigenvectors and eigenvalues -/// \param[in] cov 2d Covariance matrix -/// \param[out] eig_vec1 First eigenvector -/// \param[out] eig_vec2 Second eigenvector -/// \tparam PointT Point type that has at least float members x and y -/// \return A pair of eigenvalues: The first is the larger eigenvalue -/// \throw std::runtime error if you would get degenerate covariance -template -std::pair eig_2d( - const Covariance2d & cov, PointT & eig_vec1, PointT & eig_vec2) -{ - const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F; - const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy); - // Add a small fudge to alleviate floating point errors - float32_t disc = ((tr_2 * tr_2) - det) + std::numeric_limits::epsilon(); - if (disc < 0.0F) { - throw std::runtime_error( - "pca_2d: negative discriminant! Should never happen for well formed " - "covariance matrix"); - } - disc = sqrtf(disc); - // compute eigenvalues - const auto ret = std::make_pair(tr_2 + disc, tr_2 - disc); - // compute eigenvectors - using point_adapter::xr_; - using point_adapter::yr_; - // We compare squared value against floating epsilon to make sure that eigen vectors - // are persistent against further calculations. - // (e.g. taking cross product of two eigen vectors) - if (fabsf(cov.xy * cov.xy) > std::numeric_limits::epsilon()) { - xr_(eig_vec1) = cov.xy; - yr_(eig_vec1) = ret.first - cov.xx; - xr_(eig_vec2) = cov.xy; - yr_(eig_vec2) = ret.second - cov.xx; - } else { - if (cov.xx > cov.yy) { - xr_(eig_vec1) = 1.0F; - yr_(eig_vec1) = 0.0F; - xr_(eig_vec2) = 0.0F; - yr_(eig_vec2) = 1.0F; - } else { - xr_(eig_vec1) = 0.0F; - yr_(eig_vec1) = 1.0F; - xr_(eig_vec2) = 1.0F; - yr_(eig_vec2) = 0.0F; - } - } - return ret; -} - -/// \brief Given eigenvectors, compute support (furthest) point in each direction -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \tparam PointT type of a point with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] eig1 First principal component of cluster -/// \param[in] eig2 Second principal component of cluster -/// \param[out] support Array to get filled with extreme points in each of the principal -/// components -/// \return whether or not eig2 is ccw wrt eig1 -template -bool8_t compute_supports( - const IT begin, const IT end, const PointT & eig1, const PointT & eig2, Point4 & support) -{ - const bool8_t ret = cross_2d(eig1, eig2) >= 0.0F; - std::array metrics{ - {-std::numeric_limits::max(), -std::numeric_limits::max(), - std::numeric_limits::max(), std::numeric_limits::max()}}; - for (auto it = begin; it != end; ++it) { - const PointT & pt = *it; - float32_t val = dot_2d(ret ? eig1 : eig2, pt); - if (val > metrics[0U]) { - metrics[0U] = val; - support[0U] = it; - } - if (val < metrics[2U]) { - metrics[2U] = val; - support[2U] = it; - } - val = dot_2d(ret ? eig2 : eig1, pt); - if (val > metrics[1U]) { - metrics[1U] = val; - support[1U] = it; - } - if (val < metrics[3U]) { - metrics[3U] = val; - support[3U] = it; - } - } - return ret; -} - -/// \brief Compute bounding box given a pair of basis directions -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \tparam PointT Point type of the lists, must have float members x and y -/// \param[in] ax1 First basis direction, assumed to be normal to ax2 -/// \param[in] ax2 Second basis direction, assumed to be normal to ax1, assumed to be ccw wrt ax1 -/// \param[in] supports Array of iterators referring to points that are most extreme in each basis -/// direction. Should be result of compute_supports function -/// \return A bounding box based on the basis axes and the support points -template -BoundingBox compute_bounding_box( - const PointT & ax1, const PointT & ax2, const Point4 & supports) -{ - decltype(BoundingBox::corners) corners; - const Point4 directions{{ax1, ax2, minus_2d(ax1), minus_2d(ax2)}}; - compute_corners(corners, supports, directions); - - // build box - BoundingBox bbox; - finalize_box(corners, bbox); - size_2d(corners, bbox.size); - return bbox; -} -} // namespace details - -/// \brief Compute oriented bounding box using PCA. This uses all points in a list, and does not -/// modify the list. The resulting bounding box is not necessarily minimum in any way -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \return An oriented bounding box in x-y. This bounding box has no height information -template -BoundingBox eigenbox_2d(const IT begin, const IT end) -{ - // compute covariance - const details::Covariance2d cov = details::covariance_2d(begin, end); - - // compute eigenvectors - using PointT = details::base_type; - PointT eig1; - PointT eig2; - const auto eig_v = details::eig_2d(cov, eig1, eig2); - - // find extreme points - details::Point4 supports; - const bool8_t is_ccw = details::compute_supports(begin, end, eig1, eig2, supports); - // build box - if (is_ccw) { - std::swap(eig1, eig2); - } - BoundingBox bbox = details::compute_bounding_box(eig1, eig2, supports); - bbox.value = eig_v.first; - - return bbox; -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp deleted file mode 100644 index 07fd6c989cedc..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp +++ /dev/null @@ -1,281 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements 2D pca on a linked list of points to estimate an oriented -/// bounding box - -// cspell: ignore LFIT, lfit -// LFIT means "L-Shape Fitting" -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ - -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" - -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ - -/// \brief A representation of the M matrix for the L-fit algorithm -struct LFitWs -{ - /// \brief Number of points in the first partition - std::size_t p; - /// \brief Number of points in the second partition - std::size_t q; - // assume matrix of form: [a b; c d] - /// \brief Sum of x values in first partition - float32_t m12a; - /// \brief Sum of y values in first partition - float32_t m12b; - /// \brief Sum of y values in second partition - float32_t m12c; - /// \brief Negative sum of x values in second partition - float32_t m12d; - // m22 is a symmetric matrix - /// \brief Sum_p x_2 + sum_q y_2 - float32_t m22a; - /// \brief Sum_p x*y - sum_q x*y - float32_t m22b; - /// \brief Sum_p y_2 + sum_x y_2 - float32_t m22d; -}; // struct LFitWs - -/// \brief Initialize M matrix for L-fit algorithm -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \param[out] ws A representation of the M matrix to get initialized -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -template -void init_lfit_ws(const IT begin, const IT end, const std::size_t size, LFitWs & ws) -{ - ws.p = 1UL; - ws.q = size - 1UL; - // init P terms (first partition) - using point_adapter::x_; - using point_adapter::y_; - const auto & pt = *begin; - const float32_t px = x_(pt); - const float32_t py = y_(pt); - // assume matrix of form: [a b; c d] - ws.m12a = px; - ws.m12b = py; - ws.m12c = 0.0F; - ws.m12d = 0.0F; - // m22 is a symmetric matrix - ws.m22a = px * px; - ws.m22b = px * py; - ws.m22d = py * py; - auto it = begin; - ++it; - for (; it != end; ++it) { - const auto & qt = *it; - const float32_t qx = x_(qt); - const float32_t qy = y_(qt); - ws.m12c += qy; - ws.m12d -= qx; - ws.m22a += qy * qy; - ws.m22b -= qx * qy; - ws.m22d += qx * qx; - } -} - -/// \brief Solves the L fit problem for a given M matrix -/// \tparam PointT The point type of the cluster being L-fitted -/// \param[in] ws A representation of the M Matrix -/// \param[out] dir The best fit direction for this partition/M matrix -/// \return The L2 residual for this fit (the score, lower is better) -template -float32_t solve_lfit(const LFitWs & ws, PointT & dir) -{ - const float32_t pi = 1.0F / static_cast(ws.p); - const float32_t qi = 1.0F / static_cast(ws.q); - const Covariance2d M{// matrix of form [x, z; z y] - ws.m22a - (((ws.m12a * ws.m12a) * pi) + ((ws.m12c * ws.m12c) * qi)), - ws.m22d - (((ws.m12b * ws.m12b) * pi) + ((ws.m12d * ws.m12d) * qi)), - ws.m22b - (((ws.m12a * ws.m12b) * pi) + ((ws.m12c * ws.m12d) * qi)), 0UL}; - PointT eig1; - const auto eig_v = eig_2d(M, eig1, dir); - return eig_v.second; -} - -/// \brief Increments L fit M matrix with information in the point -/// \tparam PointT The point type -/// \param[in] pt The point to increment the M matrix with -/// \param[inout] ws A representation of the M matrix -template -void increment_lfit_ws(const PointT & pt, LFitWs & ws) -{ - const float32_t px = point_adapter::x_(pt); - const float32_t py = point_adapter::y_(pt); - ws.m12a += px; - ws.m12b += py; - ws.m12c -= py; - ws.m12d += px; - ws.m22b += 2.0F * px * py; - const float32_t px2y2 = (px - py) * (px + py); - ws.m22a += px2y2; - ws.m22d -= px2y2; -} - -/// \tparam IT An iterator type that should dereference into a point type with float members x and y -template -class LFitCompare -{ -public: - /// \brief Constructor, initializes normal direction - /// \param[in] hint A 2d vector with the direction that will be used to order the - /// point list - explicit LFitCompare(const PointT & hint) - : m_nx(point_adapter::x_(hint)), m_ny(point_adapter::y_(hint)) - { - } - - /// \brief Comparator operation, returns true if the projection of a the tangent line - /// is smaller than the projection of b - /// \param[in] p The first point for comparison - /// \param[in] q The second point for comparison - /// \return True if a has a smaller projection than b on the tangent line - bool8_t operator()(const PointT & p, const PointT & q) const - { - using point_adapter::x_; - using point_adapter::y_; - return ((x_(p) * m_nx) + (y_(p) * m_ny)) < ((x_(q) * m_nx) + (y_(q) * m_ny)); - } - -private: - const float32_t m_nx; - const float32_t m_ny; -}; // class LFitCompare - -/// \brief The main implementation of L-fitting a bounding box to a list of points. -/// Assumes sufficiently valid, large enough, and appropriately ordered point list -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \return A bounding box that minimizes the LFit residual -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -template -BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::size_t size) -{ - // initialize M - LFitWs ws{}; - init_lfit_ws(begin, end, size, ws); - // solve initial problem - details::base_type best_normal; - float32_t min_eig = solve_lfit(ws, best_normal); - // solve subsequent problems - auto it = begin; - ++it; - for (; it != end; ++it) { - // update M - ws.p += 1U; - ws.q -= 1U; - if (ws.q == 0U) { // checks for q = 0 case - break; - } - increment_lfit_ws(*it, ws); - // solve incremented problem - decltype(best_normal) dir; - const float32_t score = solve_lfit(ws, dir); - // update optima - if (score < min_eig) { - min_eig = score; - best_normal = dir; - } - } - // can recover best corner point, but don't care, need to cover all points - const auto inv_norm = 1.0F / norm_2d(best_normal); - if (!std::isnormal(inv_norm)) { - throw std::runtime_error{"LFit: Abnormal norm"}; - } - best_normal = times_2d(best_normal, inv_norm); - auto best_tangent = get_normal(best_normal); - // find extreme points - Point4 supports; - const bool8_t is_ccw = details::compute_supports(begin, end, best_normal, best_tangent, supports); - if (is_ccw) { - std::swap(best_normal, best_tangent); - } - BoundingBox bbox = details::compute_bounding_box(best_normal, best_tangent, supports); - bbox.value = min_eig; - - return bbox; -} -} // namespace details - -/// \brief Compute bounding box which best fits an L-shaped cluster. Uses the method proposed -/// in "Efficient L-shape fitting of laser scanner data for vehicle pose estimation" -/// \return An oriented bounding box in x-y. This bounding box has no height information -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \param[in] hint An iterator pointing to the point whose normal will be used to sort the list -/// \return A pair where the first element is an iterator pointing to the nearest point, and the -/// second element is the size of the list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \throw std::domain_error If the number of points is too few -template -BoundingBox lfit_bounding_box_2d( - const IT begin, const IT end, const PointT hint, const std::size_t size) -{ - if (size <= 1U) { - throw std::domain_error("LFit requires >= 2 points!"); - } - // NOTE: assumes points are in base_link/sensor frame! - // sort along tangent wrt sensor origin - // lint -e522 NOLINT Not a pure function: data structure iterators are pointing to is modified - std::partial_sort(begin, end, end, details::LFitCompare{hint}); - - return details::lfit_bounding_box_2d_impl(begin, end, size); -} - -/// \brief Compute bounding box which best fits an L-shaped cluster. Uses the method proposed -/// in "Efficient L-shape fitting of laser scanner data for vehicle pose estimation". -/// This implementation sorts the list using std::sort -/// \return An oriented bounding box in x-y. This bounding box has no height information -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -template -BoundingBox lfit_bounding_box_2d(const IT begin, const IT end) -{ - // use the principal component as the sorting direction - const auto cov = details::covariance_2d(begin, end); - using PointT = details::base_type; - PointT eig1; - PointT eig2; - (void)details::eig_2d(cov, eig1, eig2); - (void)eig2; - return lfit_bounding_box_2d(begin, end, eig1, cov.num_points); -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp deleted file mode 100644 index fb75384eb07cb..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp +++ /dev/null @@ -1,280 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements the rotating calipers algorithm for minimum oriented bounding boxes - -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/convex_hull.hpp" - -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ -/// \brief Find which (next) edge has smallest angle delta to directions, rotate directions -/// \param[inout] edges Array of edges on polygon after each anchor point (in ccw order). -/// E.g. if anchor point = p_i, edge = P[i+1] - P[i] -/// \param[inout] directions Array of directions of current bounding box (in ccw order) -/// \tparam PointT Point type of the lists, must have float members x and y -/// \return index of array to advance, e.g. the one with the smallest angle between edge and dir -template -uint32_t update_angles(const Point4 & edges, Point4 & directions) -{ - // find smallest angle to next - float32_t best_cos_th = -std::numeric_limits::max(); - float32_t best_edge_dir_mag = 0.0F; - uint32_t advance_idx = 0U; - for (uint32_t idx = 0U; idx < edges.size(); ++idx) { - const float32_t edge_dir_mag = std::max( - norm_2d(edges[idx]) * norm_2d(directions[idx]), std::numeric_limits::epsilon()); - const float32_t cos_th = dot_2d(edges[idx], directions[idx]) / edge_dir_mag; - if (cos_th > best_cos_th) { - best_cos_th = cos_th; - best_edge_dir_mag = edge_dir_mag; - advance_idx = idx; - } - } - - // update directions by smallest angle - const float32_t sin_th = - cross_2d(directions[advance_idx], edges[advance_idx]) / best_edge_dir_mag; - for (uint32_t idx = 0U; idx < edges.size(); ++idx) { - rotate_2d(directions[idx], best_cos_th, sin_th); - } - - return advance_idx; -} - -/// \brief Given support points i, find direction of edge: e = P[i+1] - P[i] -/// \tparam PointT Point type of the lists, must have float members x and y -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] support Array of points that are most extreme in 4 directions for current OBB -/// \param[out] edges An array to be filled with the polygon edges next from support points -template -void init_edges(const IT begin, const IT end, const Point4 & support, Point4 & edges) -{ - for (uint32_t idx = 0U; idx < support.size(); ++idx) { - auto tmp_it = support[idx]; - ++tmp_it; - const PointT & q = (tmp_it == end) ? *begin : *tmp_it; - edges[idx] = minus_2d(q, *support[idx]); - } -} - -/// \brief Scan through list to find support points for bounding box oriented in direction of -/// u = P[1] - P[0] -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[out] support Array that gets filled with pointers to points that are most extreme in -/// each direction aligned with and orthogonal to the first polygon edge. -/// Most extreme = max/min wrt u = P[1]-P[0] (in the dot/cross product sense) -template -void init_bbox(const IT begin, const IT end, Point4 & support) -{ - // compute initial orientation using first two points - auto pt_it = begin; - ++pt_it; - const auto u = minus_2d(*pt_it, *begin); - support[0U] = begin; - std::array metric{ - {-std::numeric_limits::max(), -std::numeric_limits::max(), - std::numeric_limits::max()}}; - // track u * p, fabsf(u x p), and -u * p - for (pt_it = begin; pt_it != end; ++pt_it) { - // check points against orientation for run_ptr - const auto & pt = *pt_it; - // u * p - const float32_t up = dot_2d(u, pt); - if (up > metric[0U]) { - metric[0U] = up; - support[1U] = pt_it; - } - // -u * p - if (up < metric[2U]) { - metric[2U] = up; - support[3U] = pt_it; - } - // u x p - const float32_t uxp = cross_2d(u, pt); - if (uxp > metric[1U]) { - metric[1U] = uxp; - support[2U] = pt_it; - } - } -} -/// \brief Compute the minimum bounding box for a convex hull using the rotating calipers method. -/// This function may possibly also be used for computing the width of a convex hull, as it uses the -/// external supports of a single convex hull. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \param[in] metric_fn A functor determining what measure the bounding box is minimum with respect -/// to -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -/// \tparam MetricF A functor that computes a float measure from the x and y size (width and length) -/// of a bounding box, assumed to be packed in a Point32 message. -/// \throw std::domain_error if the list of points is too small to reasonable generate a bounding -/// box -template -BoundingBox rotating_calipers_impl(const IT begin, const IT end, const MetricF metric_fn) -{ - using PointT = base_type; - // sanity check TODO(c.ho) more checks (up to n = 2?) - if (begin == end) { - throw std::domain_error("Malformed list, size = " + std::to_string(std::distance(begin, end))); - } - // initial scan to get anchor points - Point4 support; - init_bbox(begin, end, support); - // initialize directions accordingly - Point4 directions; - { // I just don't want the temporary variable floating around - auto tmp = support[0U]; - ++tmp; - directions[0U] = minus_2d(*tmp, *support[0U]); - directions[1U] = get_normal(directions[0U]); - directions[2U] = minus_2d(directions[0U]); - directions[3U] = minus_2d(directions[1U]); - } - // initialize edges - Point4 edges; - init_edges(begin, end, support, edges); - // size of initial guess - BoundingBox bbox; - decltype(BoundingBox::corners) best_corners; - compute_corners(best_corners, support, directions); - size_2d(best_corners, bbox.size); - bbox.value = metric_fn(bbox.size); - // rotating calipers step: incrementally advance, update angles, points, compute area - for (auto it = begin; it != end; ++it) { - // find smallest angle to next, update directions - const uint32_t advance_idx = update_angles(edges, directions); - // recompute area - decltype(BoundingBox::corners) corners; - compute_corners(corners, support, directions); - decltype(BoundingBox::size) tmp_size; - size_2d(corners, tmp_size); - const float32_t tmp_value = metric_fn(tmp_size); - // update best if necessary - if (tmp_value < bbox.value) { - // corners: memcpy is fine since I know corners and best_corners are distinct - (void)std::memcpy(&best_corners[0U], &corners[0U], sizeof(corners)); - // size - bbox.size = tmp_size; - bbox.value = tmp_value; - } - // Step to next iteration of calipers - { - // update smallest support - auto next_it = support[advance_idx]; - ++next_it; - const auto run_it = (end == next_it) ? begin : next_it; - support[advance_idx] = run_it; - // update edges - next_it = run_it; - ++next_it; - const PointT & next = (end == next_it) ? (*begin) : (*next_it); - edges[advance_idx] = minus_2d(next, *run_it); - } - } - - finalize_box(best_corners, bbox); - - // TODO(christopher.ho) check if one of the sizes is too small, fuzz corner 1 and 2 - // This doesn't cause any issues now, it shouldn't happen in practice, and even if it did, - // it would probably get smoothed out by prediction. But, this could cause issues with the - // collision detection algorithms (i.e GJK), but probably not. - - return bbox; -} -} // namespace details - -/// \brief Compute the minimum area bounding box given a convex hull of points. -/// This function is exposed in case a user might already have a convex hull via other methods. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \return A minimum area bounding box, value field is the area -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -template -BoundingBox minimum_area_bounding_box(const IT begin, const IT end) -{ - const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t { - return pt.x * pt.y; - }; - return details::rotating_calipers_impl(begin, end, metric_fn); -} - -/// \brief Compute the minimum perimeter bounding box given a convex hull of points -/// This function is exposed in case a user might already have a convex hull via other methods. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \return A minimum perimeter bounding box, value field is half the perimeter -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -template -BoundingBox minimum_perimeter_bounding_box(const IT begin, const IT end) -{ - const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t { - return pt.x + pt.y; - }; - return details::rotating_calipers_impl(begin, end, metric_fn); -} - -/// \brief Compute the minimum area bounding box given an unstructured list of points. -/// Only a list is supported as it enables the convex hull to be formed in O(n log n) time and -/// without memory allocation. -/// \param[inout] list A list of points to form a hull around, gets reordered -/// \return A minimum area bounding box, value field is the area -/// \tparam PointT Point type of the lists, must have float members x and y -template -BoundingBox minimum_area_bounding_box(std::list & list) -{ - const auto last = convex_hull(list); - return minimum_area_bounding_box(list.cbegin(), last); -} - -/// \brief Compute the minimum perimeter bounding box given an unstructured list of points -/// Only a list is supported as it enables the convex hull to be formed in O(n log n) time and -/// without memory allocation. -/// \param[inout] list A list of points to form a hull around, gets reordered -/// \return A minimum perimeter bounding box, value field is half the perimeter -/// \tparam PointT Point type of the lists, must have float members x and y -template -BoundingBox minimum_perimeter_bounding_box(std::list & list) -{ - const auto last = convex_hull(list); - return minimum_perimeter_bounding_box(list.cbegin(), last); -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp deleted file mode 100644 index c4c52928ac19a..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2017-2020 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief Main header for user-facing bounding box algorithms: functions and types -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ - -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -} // namespace geometry -} // namespace common -} // namespace autoware -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp deleted file mode 100644 index e3c2e58c9f80e..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp +++ /dev/null @@ -1,587 +0,0 @@ -// Copyright 2017-2021 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file includes common functionality for 2D geometry, such as dot products - -#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" - -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; - -namespace comparison = autoware::common::helper_functions::comparisons; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \brief Temporary namespace for point adapter methods, for use with nonstandard point types -namespace point_adapter -{ -/// \brief Gets the x value for a point -/// \return The x value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto x_(const PointT & pt) -{ - return pt.x; -} -/// \brief Gets the x value for a TrajectoryPoint message -/// \return The x value of the point -/// \param[in] pt The point -inline auto x_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.x; -} -/// \brief Gets the y value for a point -/// \return The y value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto y_(const PointT & pt) -{ - return pt.y; -} -/// \brief Gets the y value for a TrajectoryPoint message -/// \return The y value of the point -/// \param[in] pt The point -inline auto y_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.y; -} -/// \brief Gets the z value for a point -/// \return The z value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto z_(const PointT & pt) -{ - return pt.z; -} -/// \brief Gets the z value for a TrajectoryPoint message -/// \return The z value of the point -/// \param[in] pt The point -inline auto z_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.z; -} -/// \brief Gets a reference to the x value for a point -/// \return A reference to the x value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & xr_(PointT & pt) -{ - return pt.x; -} -/// \brief Gets a reference to the x value for a TrajectoryPoint -/// \return A reference to the x value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & xr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.x; -} -/// \brief Gets a reference to the y value for a point -/// \return A reference to The y value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & yr_(PointT & pt) -{ - return pt.y; -} -/// \brief Gets a reference to the y value for a TrajectoryPoint -/// \return A reference to the y value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & yr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.y; -} -/// \brief Gets a reference to the z value for a point -/// \return A reference to the z value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & zr_(PointT & pt) -{ - return pt.z; -} -/// \brief Gets a reference to the z value for a TrajectoryPoint -/// \return A reference to the z value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & zr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.z; -} -} // namespace point_adapter - -namespace details -{ -// Return the next iterator, cycling back to the beginning of the list if you hit the end -template -IT circular_next(const IT begin, const IT end, const IT current) noexcept -{ - auto next = std::next(current); - if (end == next) { - next = begin; - } - return next; -} - -} // namespace details - -/// \tparam T1, T2, T3 point type. Must have point adapters defined or have float members x and y -/// \brief compute whether line segment rp is counter clockwise relative to line segment qp -/// \param[in] pt shared point for both line segments -/// \param[in] r point to check if it forms a ccw angle -/// \param[in] q reference point -/// \return whether angle formed is ccw. Three collinear points is considered ccw -template -inline auto ccw(const T1 & pt, const T2 & q, const T3 & r) -{ - using point_adapter::x_; - using point_adapter::y_; - return (((x_(q) - x_(pt)) * (y_(r) - y_(pt))) - ((y_(q) - y_(pt)) * (x_(r) - x_(pt)))) <= 0.0F; -} - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x and y -/// \brief compute p x q = p1 * q2 - p2 * q1 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 2d cross product -template -inline auto cross_2d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - return (x_(pt) * y_(q)) - (y_(pt) * x_(q)); -} - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x and y -/// \brief compute p * q = p1 * q1 + p2 * q2 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 2d scalar dot product -template -inline auto dot_2d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - return (x_(pt) * x_(q)) + (y_(pt) * y_(q)); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the 2d difference between two points, p - q -/// \param[in] p The left hand side -/// \param[in] q The right hand side -/// \return A point with the difference in the x and y fields, all other fields are default -/// initialized -template -T minus_2d(const T & p, const T & q) -{ - T r; - using point_adapter::x_; - using point_adapter::y_; - point_adapter::xr_(r) = x_(p) - x_(q); - point_adapter::yr_(r) = y_(p) - y_(q); - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The unary minus or negation operator applied to a single point's 2d fields -/// \param[in] p The left hand side -/// \return A point with the negation in the x and y fields, all other fields are default -/// initialized -template -T minus_2d(const T & p) -{ - T r; - point_adapter::xr_(r) = -point_adapter::x_(p); - point_adapter::yr_(r) = -point_adapter::y_(p); - return r; -} -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The 2d addition operation, p + q -/// \param[in] p The left hand side -/// \param[in] q The right hand side -/// \return A point with the sum in the x and y fields, all other fields are default -/// initialized -template -T plus_2d(const T & p, const T & q) -{ - T r; - using point_adapter::x_; - using point_adapter::y_; - point_adapter::xr_(r) = x_(p) + x_(q); - point_adapter::yr_(r) = y_(p) + y_(q); - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The scalar multiplication operation, p * a -/// \param[in] p The point value -/// \param[in] a The scalar value -/// \return A point with the scaled x and y fields, all other fields are default -/// initialized -template -T times_2d(const T & p, const float32_t a) -{ - T r; - point_adapter::xr_(r) = static_cast(point_adapter::x_(p)) * a; - point_adapter::yr_(r) = static_cast(point_adapter::y_(p)) * a; - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief solve p + t * u = q + s * v -/// Ref: https://stackoverflow.com/questions/563198/ -/// \param[in] pt anchor point of first line -/// \param[in] u direction of first line -/// \param[in] q anchor point of second line -/// \param[in] v direction of second line -/// \return intersection point -/// \throw std::runtime_error if lines are (nearly) collinear or parallel -template -inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v) -{ - const float32_t num = cross_2d(minus_2d(pt, q), u); - float32_t den = cross_2d(v, u); - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - if (fabsf(den) < FEPS) { - if (fabsf(num) < FEPS) { - // collinear case, anything is ok - den = 1.0F; - } else { - // parallel case, no valid output - throw std::runtime_error( - "intersection_2d: no unique solution (either collinear or parallel)"); - } - } - return plus_2d(q, times_2d(v, num / den)); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief rotate point given precomputed sin and cos -/// \param[inout] pt point to rotate -/// \param[in] cos_th precomputed cosine value -/// \param[in] sin_th precomputed sine value -template -inline void rotate_2d(T & pt, const float32_t cos_th, const float32_t sin_th) -{ - const float32_t x = point_adapter::x_(pt); - const float32_t y = point_adapter::y_(pt); - point_adapter::xr_(pt) = (cos_th * x) - (sin_th * y); - point_adapter::yr_(pt) = (sin_th * x) + (cos_th * y); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief rotate by radian angle th in z direction with ccw positive -/// \param[in] pt reference point to rotate -/// \param[in] th_rad angle by which to rotate point -/// \return rotated point -template -inline T rotate_2d(const T & pt, const float32_t th_rad) -{ - T q(pt); // It's reasonable to expect a copy constructor - const float32_t s = sinf(th_rad); - const float32_t c = cosf(th_rad); - rotate_2d(q, c, s); - return q; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief compute q s.t. p T q, or p * q = 0 -/// This is the equivalent of a 90 degree ccw rotation -/// \param[in] pt point to get normal point of -/// \return point normal to p (un-normalized) -template -inline T get_normal(const T & pt) -{ - T q(pt); - point_adapter::xr_(q) = -point_adapter::y_(pt); - point_adapter::yr_(q) = point_adapter::x_(pt); - return q; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief get magnitude of x and y components: -/// \param[in] pt point to get magnitude of -/// \return magnitude of x and y components together -template -inline auto norm_2d(const T & pt) -{ - return sqrtf(static_cast(dot_2d(pt, pt))); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the closest point on line segment p-q to point r -/// Based on equations from https://stackoverflow.com/a/1501725 and -/// http://paulbourke.net/geometry/pointlineplane/ -/// \param[in] p First point defining the line segment -/// \param[in] q Second point defining the line segment -/// \param[in] r Reference point to find the closest point to -/// \return Closest point on line segment p-q to point r -template -inline T closest_segment_point_2d(const T & p, const T & q, const T & r) -{ - const T qp = minus_2d(q, p); - const float32_t len2 = static_cast(dot_2d(qp, qp)); - T ret = p; - if (len2 > std::numeric_limits::epsilon()) { - const Interval_f unit_interval(0.0f, 1.0f); - const float32_t val = static_cast(dot_2d(minus_2d(r, p), qp)) / len2; - const float32_t t = Interval_f::clamp_to(unit_interval, val); - ret = plus_2d(p, times_2d(qp, t)); - } - return ret; -} -// -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the closest point on the line going through p-q to point r -// Obtained by simplifying closest_segment_point_2d. -/// \param[in] p First point defining the line -/// \param[in] q Second point defining the line -/// \param[in] r Reference point to find the closest point to -/// \return Closest point on line p-q to point r -/// \throw std::runtime_error if the two points coincide and hence don't uniquely -// define a line -template -inline T closest_line_point_2d(const T & p, const T & q, const T & r) -{ - const T qp = minus_2d(q, p); - const float32_t len2 = dot_2d(qp, qp); - T ret = p; - if (len2 > std::numeric_limits::epsilon()) { - const float32_t t = dot_2d(minus_2d(r, p), qp) / len2; - ret = plus_2d(p, times_2d(qp, t)); - } else { - throw std::runtime_error( - "closet_line_point_2d: line ill-defined because given points coincide"); - } - return ret; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the distance from line segment p-q to point r -/// \param[in] p First point defining the line segment -/// \param[in] q Second point defining the line segment -/// \param[in] r Reference point to find the distance from the line segment to -/// \return Distance from point r to line segment p-q -template -inline auto point_line_segment_distance_2d(const T & p, const T & q, const T & r) -{ - const T pq_r = minus_2d(closest_segment_point_2d(p, q, r), r); - return norm_2d(pq_r); -} - -/// \brief Make a 2D unit vector given an angle. -/// \tparam T Point type. Must have point adapters defined or have float members x and y -/// \param th Angle in radians -/// \return Unit vector in the direction of the given angle. -template -inline T make_unit_vector2d(float th) -{ - T r; - point_adapter::xr_(r) = std::cos(th); - point_adapter::yr_(r) = std::sin(th); - return r; -} - -/// \brief Compute squared euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x and y -/// \tparam T2 point type. Must have point adapters defined or have float members x and y -/// \param a point 1 -/// \param b point 2 -/// \return squared euclidean distance -template -inline OUT squared_distance_2d(const T1 & a, const T2 & b) -{ - const auto x = static_cast(point_adapter::x_(a)) - static_cast(point_adapter::x_(b)); - const auto y = static_cast(point_adapter::y_(a)) - static_cast(point_adapter::y_(b)); - return (x * x) + (y * y); -} - -/// \brief Compute euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x and y -/// \tparam T2 point type. Must have point adapters defined or have float members x and y -/// \param a point 1 -/// \param b point 2 -/// \return euclidean distance -template -inline OUT distance_2d(const T1 & a, const T2 & b) -{ - return std::sqrt(squared_distance_2d(a, b)); -} - -/// \brief Check the given point's position relative the infinite line passing -/// from p1 to p2. Logic based on http://geomalgorithms.com/a01-_area.html#isLeft() -/// \tparam T T point type. Must have point adapters defined or have float members x and y -/// \param p1 point 1 laying on the infinite line -/// \param p2 point 2 laying on the infinite line -/// \param q point to be checked against the line -/// \return > 0 for point q left of the line through p1 to p2 -/// = 0 for point q on the line through p1 to p2 -/// < 0 for point q right of the line through p1 to p2 -template -inline auto check_point_position_to_line_2d(const T & p1, const T & p2, const T & q) -{ - return cross_2d(minus_2d(p2, p1), minus_2d(q, p1)); -} - -/// Check if all points are ordered in x-y plane (in either clockwise or counter-clockwise -/// direction): This function does not check for convexity -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Beginning of point sequence -/// \param[in] end One past the last of the point sequence -/// \return Whether or not all point triples p_i, p_{i+1}, p_{i+2} are in a particular order. -/// Returns true for collinear points as well -template -bool all_ordered(const IT begin, const IT end) noexcept -{ - // Short circuit: a line or point is always CCW or otherwise ill-defined - if (std::distance(begin, end) <= 2U) { - return true; - } - bool is_first_point_cw = false; - // Can't use std::all_of because that works directly on the values - for (auto line_start = begin; line_start != end; ++line_start) { - const auto line_end = details::circular_next(begin, end, line_start); - const auto query_point = details::circular_next(begin, end, line_end); - // Check if 3 points starting from current point are in clockwise direction - const bool is_cw = comparison::abs_lte( - check_point_position_to_line_2d(*line_start, *line_end, *query_point), 0.0F, 1e-3F); - if (is_cw) { - if (line_start == begin) { - is_first_point_cw = true; - } else { - if (!is_first_point_cw) { - return false; - } - } - } else if (is_first_point_cw) { - return false; - } - } - return true; -} - -/// Compute the area of a convex hull, points are assumed to be ordered (in either CW or CCW) -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Iterator pointing to the beginning of the polygon points -/// \param[in] end Iterator pointing to one past the last of the polygon points -/// \return The area of the polygon, in squared of whatever units your points are in -template -auto area_2d(const IT begin, const IT end) noexcept -{ - using point_adapter::x_; - using point_adapter::y_; - using T = decltype(x_(*begin)); - auto area = T{}; // zero initialization - // use the approach of https://www.mathwords.com/a/area_convex_polygon.htm - for (auto it = begin; it != end; ++it) { - const auto next = details::circular_next(begin, end, it); - area += x_(*it) * y_(*next); - area -= x_(*next) * y_(*it); - } - return std::abs(T{0.5} * area); -} - -/// Compute area of convex hull, throw if points are not ordered (convexity check is not -/// implemented) -/// \throw std::domain_error if points are not ordered either CW or CCW -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Iterator pointing to the beginning of the polygon points -/// \param[in] end Iterator pointing to one past the last of the polygon points -/// \return The area of the polygon, in squared of whatever units your points are in -template -auto area_checked_2d(const IT begin, const IT end) -{ - if (!all_ordered(begin, end)) { - throw std::domain_error{"Cannot compute area: points are not ordered"}; - } - return area_2d(begin, end); -} - -/// \brief Check if the given point is inside or on the edge of the given polygon -/// \tparam IteratorType iterator type. The value pointed to by this must have point adapters -/// defined or have float members x and y -/// \tparam PointType point type. Must have point adapters defined or have float members x and y -/// \param start_it iterator pointing to the first vertex of the polygon -/// \param end_it iterator pointing to the last vertex of the polygon. The vertices should be in -/// order. -/// \param p point to be searched -/// \return True if the point is inside or on the edge of the polygon. False otherwise -template -bool is_point_inside_polygon_2d( - const IteratorType & start_it, const IteratorType & end_it, const PointType & p) -{ - std::int32_t winding_num = 0; - - for (IteratorType it = start_it; it != end_it; ++it) { - auto next_it = std::next(it); - if (next_it == end_it) { - next_it = start_it; - } - if (point_adapter::y_(*it) <= point_adapter::y_(p)) { - // Upward crossing edge - if (point_adapter::y_(*next_it) >= point_adapter::y_(p)) { - if (comparison::abs_gt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) { - ++winding_num; - } else { - if (comparison::abs_eq_zero(check_point_position_to_line_2d(*it, *next_it, p), 1e-3F)) { - return true; - } - } - } - } else { - // Downward crossing edge - if (point_adapter::y_(*next_it) <= point_adapter::y_(p)) { - if (comparison::abs_lt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) { - --winding_num; - } else { - if (comparison::abs_eq_zero(check_point_position_to_line_2d(*it, *next_it, p), 1e-3F)) { - return true; - } - } - } - } - } - return winding_num != 0; -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp deleted file mode 100644 index 74cd210dec586..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file includes common functionality for 3D geometry, such as dot products - -#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x, y and z -/// \brief compute p * q = p1 * q1 + p2 * q2 + p3 * 13 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 3d scalar dot product -template -inline auto dot_3d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - using point_adapter::z_; - return (x_(pt) * x_(q)) + (y_(pt) * y_(q) + z_(pt) * z_(q)); -} - -/// \brief Compute 3D squared euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x y and z -/// \tparam T2 point type. Must have point adapters defined or have float members x y and z -/// \param a point 1 -/// \param b point 2 -/// \return squared 3D euclidean distance -template -inline OUT squared_distance_3d(const T1 & a, const T2 & b) -{ - const auto x = static_cast(point_adapter::x_(a)) - static_cast(point_adapter::x_(b)); - const auto y = static_cast(point_adapter::y_(a)) - static_cast(point_adapter::y_(b)); - const auto z = static_cast(point_adapter::z_(a)) - static_cast(point_adapter::z_(b)); - return (x * x) + (y * y) + (z * z); -} - -/// \brief Compute 3D euclidean distance between two points -/// \tparam T1 point type. Must have point adapters defined or have float members x y and z -/// \tparam T2 point type. Must have point adapters defined or have float members x y and z -/// \param a point 1 -/// \param b point 2 -/// \return 3D euclidean distance -template -inline OUT distance_3d(const T1 & a, const T2 & b) -{ - return std::sqrt(squared_distance_3d(a, b)); -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp deleted file mode 100644 index ae81c55ad6b55..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp +++ /dev/null @@ -1,195 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements the monotone chain algorithm to compute 2D convex hulls on linked -/// lists of points - -#ifndef AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -// lint -e537 NOLINT pclint vs cpplint -#include -// lint -e537 NOLINT pclint vs cpplint -#include -#include -#include - -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief Contains computation geometry functions not intended for the end user to directly use -namespace details -{ - -/// \brief Moves points comprising the lower convex hull from points to hull. -/// \param[inout] points A list of points, assumed to be sorted in lexical order -/// \param[inout] hull An empty list of points, assumed to have same allocator as points -/// (for splice) -/// \tparam PointT The point type for the points list -/// \tparam HullT the point type for the hull list -template -void form_lower_hull(std::list & points, std::list & hull) -{ - auto hull_it = hull.cbegin(); - auto point_it = points.cbegin(); - // This ensures that the points we splice to back to the end of list are not processed - const auto iters = points.size(); - for (auto idx = decltype(iters){0}; idx < iters; ++idx) { - // splice points from tail of hull to tail of list until point from head of list satisfies ccw - bool8_t is_ccw = true; - while ((hull.cbegin() != hull_it) && is_ccw) { - const auto current_hull_it = hull_it; - --hull_it; - is_ccw = ccw(*hull_it, *current_hull_it, *point_it); - if (!is_ccw) { - hull_it = current_hull_it; - break; - } - // return this node to list for consideration in upper hull - points.splice(points.end(), hull, current_hull_it); - } - const auto last_point_it = point_it; - ++point_it; - // Splice head of list to tail of (lower) hull - hull.splice(hull.end(), points, last_point_it); - // point_it has been advanced, hull_it has been advanced (to where point_it was previously) - hull_it = last_point_it; - } - // loop is guaranteed not to underflow because a node can be removed from points AT MOST once - // per loop iteration. The loop is upper bounded to the prior size of the point list -} -/// \brief Moves points comprising the lower convex hull from points to hull. -/// \param[inout] points A list of points, assumed to be sorted in lexical order, and to contain -/// the leftmost point -/// \param[inout] hull A list of points, assumed to have same allocator as points (for splice), -/// and to contain the lower hull (minus the left-most point) -/// \tparam PointT The point type for the points list -/// \tparam HullT the point type for the hull list -template -void form_upper_hull(std::list & points, std::list & hull) -{ - // TODO(c.ho) consider reverse iterators, not sure if they work with splice() - auto hull_it = hull.cend(); - --hull_it; - const auto lower_hull_end = hull_it; - auto point_it = points.cend(); - --point_it; - // This ensures that the points we splice to back to the head of list are not processed - const auto iters = points.size(); - for (auto idx = decltype(iters){0}; idx < iters; ++idx) { - // splice points from tail of hull to head of list until ccw is satisfied with tail of list - bool8_t is_ccw = true; - while ((lower_hull_end != hull_it) && is_ccw) { - const auto current_hull_it = hull_it; - --hull_it; - is_ccw = ccw(*hull_it, *current_hull_it, *point_it); - if (!is_ccw) { - hull_it = current_hull_it; - break; - } - points.splice(points.begin(), hull, current_hull_it); - } - const auto last_point_it = point_it; - --point_it; - // Splice points from tail of lexically ordered point list to tail of hull - hull.splice(hull.end(), points, last_point_it); - hull_it = last_point_it; - } - // loop is guaranteed not to underflow because a node can be removed from points AT MOST once - // per loop iteration. The loop is upper bounded to the prior size of the point list -} - -/// \brief A static memory implementation of convex hull computation. Shuffles points around the -/// deque such that the points of the convex hull of the deque of points are first in the -/// deque, with the internal points following in an unspecified order. -/// The head of the deque will be the point with the smallest x value, with the other -/// points following in a counter-clockwise manner (from a top down view/facing -z direction) -/// \param[inout] list A list of nodes that will be pruned down and reordered into a ccw convex hull -/// \return An iterator pointing to one after the last point contained in the hull -/// \tparam PointT Type of a point, must have x and y float members -template -typename std::list::const_iterator convex_hull_impl(std::list & list) -{ - // Functor that return whether a <= b in the lexical sense (a.x < b.x), sort by y if tied - const auto lexical_comparator = [](const PointT & a, const PointT & b) -> bool8_t { - using point_adapter::x_; - using point_adapter::y_; - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - return (fabsf(x_(a) - x_(b)) > FEPS) ? (x_(a) < x_(b)) : (y_(a) < y_(b)); - }; - list.sort(lexical_comparator); - - // Temporary list to store points - std::list tmp_hull_list{list.get_allocator()}; - - // Shuffle lower hull points over to tmp_hull_list - form_lower_hull(list, tmp_hull_list); - - // Resort list since we can't guarantee the order TODO(c.ho) is this true? - list.sort(lexical_comparator); - // splice first hull point to beginning of list to ensure upper hull is properly closed - // This is done after sorting because we know exactly where it should go, and keeping it out of - // sorting reduces complexity somewhat - list.splice(list.begin(), tmp_hull_list, tmp_hull_list.begin()); - - // build upper hull - form_upper_hull(list, tmp_hull_list); - // Move hull to beginning of list, return iterator pointing to one after the convex hull - const auto ret = list.begin(); - // first move left-most point to ensure it is first - auto tmp_it = tmp_hull_list.end(); - --tmp_it; - list.splice(list.begin(), tmp_hull_list, tmp_it); - // Then move remainder of hull points - list.splice(ret, tmp_hull_list); - return ret; -} -} // namespace details - -/// \brief A static memory implementation of convex hull computation. Shuffles points around the -/// deque such that the points of the convex hull of the deque of points are first in the -/// deque, with the internal points following in an unspecified order. -/// -/// The head of the deque will be the point with the smallest x value, with the other -/// points following in a counter-clockwise manner (from a top down view/facing -z -/// direction). If the point list is 3 or smaller, nothing is done (e.g. the ordering result -/// as previously stated does not hold). -/// \param[inout] list A list of nodes that will be reordered into a ccw convex hull -/// \return An iterator pointing to one after the last point contained in the hull -/// \tparam PointT Type of a point, must have x and y float members -template -typename std::list::const_iterator convex_hull(std::list & list) -{ - return (list.size() <= 3U) ? list.end() : details::convex_hull_impl(list); -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp deleted file mode 100644 index cd9fd49f71635..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp +++ /dev/null @@ -1,111 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland. 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements an algorithm for getting a list of "pockets" in the convex -/// hull of a non-convex simple polygon. - -#ifndef AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -#include -#include -#include -#include -#include - -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \brief Compute a list of "pockets" of a simple polygon -/// (https://en.wikipedia.org/wiki/Simple_polygon), that is, the areas that make -/// up the difference between the polygon and its convex hull. -/// This currently has a bug: -// * "Rollover" is not properly handled - if a pocket contains the segment from -// the last point in the list to the first point (which is still part of the -// polygon), that does not get added -/// -/// \param[in] polygon_start Start iterator for the simple polygon (has to be on convex hull) -/// \param[in] polygon_end End iterator for the simple polygon -/// \param[in] convex_hull_start Start iterator for the convex hull of the simple polygon -/// \param[in] convex_hull_end End iterator for the convex hull of the simple polygon -/// \return A vector of vectors of the iterator value type. Each inner vector contains the points -/// for one pocket. We return by value instead of as iterator pairs, because it is possible -/// that the edge connecting the final point in the list and the first point in the list is -/// part of a pocket as well, and this is awkward to represent using iterators into the -/// original collection. -/// -/// \tparam Iter1 Iterator to a point type -/// \tparam Iter2 Iterator to a point type (can be the same as Iter1, but does not need to be) -template -typename std::vector::value_type>> hull_pockets( - const Iter1 polygon_start, const Iter1 polygon_end, const Iter2 convex_hull_start, - const Iter2 convex_hull_end) -{ - auto pockets = std::vector::value_type>>{}; - if (std::distance(polygon_start, polygon_end) <= 3) { - return pockets; - } - - // Function to check whether a point is in the convex hull. - const auto in_convex_hull = [convex_hull_start, convex_hull_end](Iter1 p) { - return std::any_of(convex_hull_start, convex_hull_end, [p](auto hull_entry) { - return norm_2d(minus_2d(hull_entry, *p)) < std::numeric_limits::epsilon(); - }); - }; - - // We go through the points of the polygon only once, adding pockets to the list of pockets - // as we detect them. - std::vector::value_type> current_pocket{}; - for (auto it = polygon_start; it != polygon_end; it = std::next(it)) { - if (in_convex_hull(it)) { - if (current_pocket.size() > 1) { - current_pocket.emplace_back(*it); - pockets.push_back(current_pocket); - } - current_pocket.clear(); - current_pocket.emplace_back(*it); - } else { - current_pocket.emplace_back(*it); - } - } - - // At this point, we have reached the end of the polygon, but have not considered the connection - // of the final point back to the first. In case the first point is part of a pocket as well, we - // have some points left in current_pocket, and we add one final pocket that is made up by those - // points plus the first point in the collection. - if (current_pocket.size() > 1) { - current_pocket.push_back(*polygon_start); - pockets.push_back(current_pocket); - } - - return pockets; -} -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp deleted file mode 100644 index 08c70c3a5a6be..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp +++ /dev/null @@ -1,312 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/convex_hull.hpp" - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -using autoware::common::geometry::closest_line_point_2d; -using autoware::common::geometry::convex_hull; -using autoware::common::geometry::dot_2d; -using autoware::common::geometry::get_normal; -using autoware::common::geometry::minus_2d; -using autoware::common::geometry::norm_2d; -using autoware::common::geometry::times_2d; -using autoware_auto_perception_msgs::msg::BoundingBox; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; - -using Point = geometry_msgs::msg::Point32; - -namespace details -{ - -/// Alias for a std::pair of two points -using Line = std::pair; - -/// \tparam Iter1 Iterator over point-types that must have point adapters -// defined or have float members x and y -/// \brief Compute a sorted list of faces of a polyhedron given a list of points -/// \param[in] start Start iterator of the list of points -/// \param[in] end End iterator of the list of points -/// \return The list of faces -template -std::vector get_sorted_face_list(const Iter start, const Iter end) -{ - // First get a sorted list of points - convex_hull does that by modifying its argument - auto corner_list = std::list(start, end); - const auto first_interior_point = convex_hull(corner_list); - - std::vector face_list{}; - auto itLast = corner_list.begin(); - auto itNext = std::next(itLast, 1); - do { - face_list.emplace_back(Line{*itLast, *itNext}); - itLast = itNext; - itNext = std::next(itNext, 1); - } while ((itNext != first_interior_point) && (itNext != corner_list.end())); - - face_list.emplace_back(Line{*itLast, corner_list.front()}); - - return face_list; -} - -/// \brief Append points of the polygon `internal` that are contained in the polygon `external`. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -void append_contained_points( - const Iterable1T & external, const Iterable2T & internal, - std::list & result) -{ - std::copy_if( - internal.begin(), internal.end(), std::back_inserter(result), [&external](const auto & pt) { - return common::geometry::is_point_inside_polygon_2d(external.begin(), external.end(), pt); - }); -} - -/// \brief Append the intersecting points between two polygons into the output list. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -void append_intersection_points( - const Iterable1T & polygon1, const Iterable2T & polygon2, - std::list & result) -{ - using FloatT = decltype(point_adapter::x_(std::declval())); - using Interval = common::geometry::Interval; - - auto get_edge = [](const auto & list, const auto & iterator) { - const auto next_it = std::next(iterator); - const auto & next_pt = (next_it != list.end()) ? *next_it : list.front(); - return std::make_pair(*iterator, next_pt); - }; - - // Get the max absolute value out of two intervals and a scalar. - auto compute_eps_scale = [](const auto & i1, const auto & i2, const auto val) { - auto get_abs_max = [](const auto & interval) { - return std::max(std::fabs(Interval::min(interval)), std::fabs(Interval::max(interval))); - }; - return std::max(std::fabs(val), std::max(get_abs_max(i1), get_abs_max(i2))); - }; - - // Compare each edge from polygon1 to each edge from polygon2 - for (auto corner1_it = polygon1.begin(); corner1_it != polygon1.end(); ++corner1_it) { - const auto & edge1 = get_edge(polygon1, corner1_it); - - Interval edge1_x_interval{ - std::min(point_adapter::x_(edge1.first), point_adapter::x_(edge1.second)), - std::max(point_adapter::x_(edge1.first), point_adapter::x_(edge1.second))}; - - Interval edge1_y_interval{ - std::min(point_adapter::y_(edge1.first), point_adapter::y_(edge1.second)), - std::max(point_adapter::y_(edge1.first), point_adapter::y_(edge1.second))}; - - for (auto corner2_it = polygon2.begin(); corner2_it != polygon2.end(); ++corner2_it) { - try { - const auto & edge2 = get_edge(polygon2, corner2_it); - const auto & intersection = common::geometry::intersection_2d( - edge1.first, minus_2d(edge1.second, edge1.first), edge2.first, - minus_2d(edge2.second, edge2.first)); - - Interval edge2_x_interval{ - std::min(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second)), - std::max(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second))}; - - Interval edge2_y_interval{ - std::min(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second)), - std::max(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second))}; - - // cspell: ignore feps - // feps means "Float EPSilon" - - // The accumulated floating point error depends on the magnitudes of each end of the - // intervals. Hence the upper bound of the absolute magnitude should be taken into account - // while computing the epsilon. - const auto max_feps_scale = std::max( - compute_eps_scale(edge1_x_interval, edge2_x_interval, point_adapter::x_(intersection)), - compute_eps_scale(edge1_y_interval, edge2_y_interval, point_adapter::y_(intersection))); - const auto feps = max_feps_scale * std::numeric_limits::epsilon(); - // Only accept intersections that lie on both of the line segments (edges) - if ( - Interval::contains(edge1_x_interval, point_adapter::x_(intersection), feps) && - Interval::contains(edge2_x_interval, point_adapter::x_(intersection), feps) && - Interval::contains(edge1_y_interval, point_adapter::y_(intersection), feps) && - Interval::contains(edge2_y_interval, point_adapter::y_(intersection), feps)) { - result.push_back(intersection); - } - } catch (const std::runtime_error &) { - // Parallel lines. TODO(yunus.caliskan): #1229 - continue; - } - } - } -} - -} // namespace details - -// TODO(s.me) implement GJK(+EPA) algorithm as well as per Chris Ho's suggestion -/// \tparam Iter Iterator over point-types that must have point adapters -// defined or have float members x and y -/// \brief Check if polyhedra defined by two given sets of points intersect -// This uses SAT for doing the actual checking -// (https://en.wikipedia.org/wiki/Hyperplane_separation_theorem#Use_in_collision_detection) -/// \param[in] begin1 Start iterator to first list of point types -/// \param[in] end1 End iterator to first list of point types -/// \param[in] begin2 Start iterator to first list of point types -/// \param[in] end2 End iterator to first list of point types -/// \return true if the boxes collide, false otherwise. -template -bool intersect(const Iter begin1, const Iter end1, const Iter begin2, const Iter end2) -{ - // Obtain sorted lists of faces of both boxes, merge them into one big list of faces - auto faces = details::get_sorted_face_list(begin1, end1); - const auto faces_2 = details::get_sorted_face_list(begin2, end2); - faces.reserve(faces.size() + faces_2.size()); - faces.insert(faces.end(), faces_2.begin(), faces_2.end()); - - // Also look at last line - for (const auto & face : faces) { - // Compute normal vector to the face and define a closure to get progress along it - const auto normal = get_normal(minus_2d(face.second, face.first)); - auto get_position_along_line = [&normal](auto point) { - return dot_2d(normal, minus_2d(point, Point{})); - }; - - // Define a function to get the minimum and maximum projected position of the corners - // of a given bounding box along the normal line of the face - auto get_projected_min_max = [&get_position_along_line, &normal](Iter begin, Iter end) { - const auto zero_point = Point{}; - auto min_corners = get_position_along_line(closest_line_point_2d(normal, zero_point, *begin)); - auto max_corners = min_corners; - - for (auto & point = begin; point != end; ++point) { - const auto point_projected = closest_line_point_2d(normal, zero_point, *point); - const auto position_along_line = get_position_along_line(point_projected); - min_corners = std::min(min_corners, position_along_line); - max_corners = std::max(max_corners, position_along_line); - } - return std::pair{min_corners, max_corners}; - }; - - // Perform the actual computations for the extent computation - auto minmax_1 = get_projected_min_max(begin1, end1); - auto minmax_2 = get_projected_min_max(begin2, end2); - - // Check for any intersections - const auto eps = std::numeric_limits::epsilon(); - if (minmax_1.first > minmax_2.second + eps || minmax_2.first > minmax_1.second + eps) { - // Found separating hyperplane, stop - return false; - } - } - - // No separating hyperplane found, boxes collide - return true; -} - -/// \brief Get the intersection between two polygons. The polygons should be provided in an -/// identical format to the output of `convex_hull` function as in the corners should be ordered -/// in a CCW fashion. -/// The computation is done by: -/// * Find the corners of each polygon that are contained by the other polygon. -/// * Find the intersection points between two polygons -/// * Combine these points and order CCW to get the final polygon. -/// The criteria for intersection is better explained in: -/// "Area of intersection of arbitrary polygons" (Livermore, Calif, 1977) -/// See https://www.osti.gov/servlets/purl/7309916/, chapter II - B -/// TODO(yunus.caliskan): This is a naive implementation. We should scan for a more efficient -/// algorithm: #1230 -/// \tparam Iterable1T A container class that has stl style iterators defined. -/// \tparam Iterable2T A container class that has stl style iterators defined. -/// \tparam PointT Point type that have the adapters for the x and y fields. -/// set to `Point1T` -/// \param polygon1 A convex polygon -/// \param polygon2 A convex polygon -/// \return The resulting conv -template < - template class Iterable1T, template class Iterable2T, typename PointT> -std::list convex_polygon_intersection2d( - const Iterable1T & polygon1, const Iterable2T & polygon2) -{ - std::list result; - details::append_contained_points(polygon1, polygon2, result); - details::append_contained_points(polygon2, polygon1, result); - details::append_intersection_points(polygon1, polygon2, result); - const auto end_it = common::geometry::convex_hull(result); - result.resize(static_cast(std::distance(result.cbegin(), end_it))); - return result; -} - -/// \brief Compute the intersection over union of two 2d convex polygons. If any of the polygons -/// span a zero area, the result is 0.0. -/// \tparam Iterable1T A container class that has stl style iterators defined. -/// \tparam Iterable2T A container class that has stl style iterators defined. -/// \tparam Point1T Point type that have the adapters for the x and y fields. -/// \tparam Point2T Point type that have the adapters for the x and y fields. -/// \param polygon1 A convex polygon -/// \param polygon2 A convex polygon -/// \return (Intersection / Union) between two given polygons. -/// \throws std::domain_error If there is any inconsistency on the underlying geometrical -/// computation. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -common::types::float32_t convex_intersection_over_union_2d( - const Iterable1T & polygon1, const Iterable2T & polygon2) -{ - constexpr auto eps = std::numeric_limits::epsilon(); - const auto intersection = convex_polygon_intersection2d(polygon1, polygon2); - - const auto intersection_area = - common::geometry::area_2d(intersection.begin(), intersection.end()); - - if (intersection_area < eps) { - return 0.0F; // There's either no intersection or the points are collinear - } - - const auto polygon1_area = common::geometry::area_2d(polygon1.begin(), polygon1.end()); - const auto polygon2_area = common::geometry::area_2d(polygon2.begin(), polygon2.end()); - - const auto union_area = polygon1_area + polygon2_area - intersection_area; - if (union_area < eps) { - throw std::domain_error("IoU is undefined for polygons with a zero union area"); - } - - return intersection_area / union_area; -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp deleted file mode 100644 index 54be2c32b1d05..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp +++ /dev/null @@ -1,358 +0,0 @@ -// Copyright 2020 Mapless AI, Inc. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#ifndef AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/** - * @brief Data structure to contain scalar interval bounds. - * - * @post The interval is guaranteed to be valid upon successful construction. An - * interval [min, max] is "valid" if it is empty (min/max = NaN) or its bounds - * are ordered (min <= max). - */ -template -class Interval -{ - static_assert(std::is_floating_point::value, "Intervals only support floating point types."); - -public: - /** - * @brief Compute equality. - * - * Two intervals compare equal iff they are both valid and they are both - * either empty or have equal bounds. - * - * @note This is defined inline because the class is templated. - * - * @return True iff the intervals compare equal. - */ - friend bool operator==(const Interval & i1, const Interval & i2) - { - const auto min_eq = (Interval::min(i1) == Interval::min(i2)); - const auto max_eq = (Interval::max(i1) == Interval::max(i2)); - const auto bounds_equal = (min_eq && max_eq); - const auto both_empty = (Interval::empty(i1) && Interval::empty(i2)); - return both_empty || bounds_equal; - } - - /** - * @brief Compute inequality and the logical negation of equality. - * @note This is defined inline because the class is templated. - */ - friend bool operator!=(const Interval & i1, const Interval & i2) { return !(i1 == i2); } - - /** - * @brief Stream overload for Interval types. - * - * @note Output precision is fixed inside the function definition, and the - * serialization is JSON compatible. - * - * @note The serialization is lossy. It is used for debugging and for - * generating exception strings. - */ - friend std::ostream & operator<<(std::ostream & os, const Interval & i) - { - constexpr auto PRECISION = 5; - - // Internal helper to format the output. - const auto readable_extremum = [](const T & val) { - if (val == std::numeric_limits::lowest()) { - return std::string("REAL_MIN"); - } - if (val == std::numeric_limits::max()) { - return std::string("REAL_MAX"); - } - - std::stringstream ss; - ss.setf(std::ios::fixed, std::ios::floatfield); - ss.precision(PRECISION); - ss << val; - return ss.str(); - }; - - // Do stream output. - if (Interval::empty(i)) { - return os << "{}"; - } - return os << "{\"min\": " << readable_extremum(Interval::min(i)) - << ", \"max\": " << readable_extremum(Interval::max(i)) << "}"; - } - - /** - * @brief Test whether the two intervals have bounds within epsilon of each - * other. - * - * @note If both intervals are empty, this returns true. If only one is empty, - * this returns false. - */ - static bool abs_eq(const Interval & i1, const Interval & i2, const T & eps); - - /** @brief The minimum bound of the interval. */ - static T min(const Interval & i) { return i.min_; } - - /** @brief The maximum bound of the interval. */ - static T max(const Interval & i) { return i.max_; } - - /** - * @brief Return the measure (length) of the interval. - * - * @note For empty or invalid intervals, NaN is returned. See Interval::empty - * for note on distinction between measure zero and the emptiness property. - */ - static T measure(const Interval & i); - - /** - * @brief Utility for checking whether an interval has zero measure. - * - * @note For empty or invalid intervals, false is returned. See - * Interval::empty for note on distinction between measure zero and the - * emptiness property. - * - * @return True iff the interval has zero measure. - */ - static bool zero_measure(const Interval & i); - - /** - * @brief Whether the interval is empty or not. - * - * @note Emptiness refers to whether the interval contains any points and is - * thus a distinct property from measure: an interval is non-empty if contains - * only a single point even though its measure in that case is zero. - * - * @return True iff the interval is empty. - */ - static bool empty(const Interval & i); - - /** - * @brief Test for whether a given interval contains a given value within the given epsilon - * @return True iff 'interval' contains 'value'. - */ - static bool contains( - const Interval & i, const T & value, const T & eps = std::numeric_limits::epsilon()); - - /** - * @brief Test for whether 'i1' subseteq 'i2' - * @return True iff i1 subseteq i2. - */ - static bool is_subset_eq(const Interval & i1, const Interval & i2); - - /** - * @brief Compute the intersection of two intervals as a new interval. - */ - static Interval intersect(const Interval & i1, const Interval & i2); - - /** - * @brief Clamp a scalar 'val' onto 'interval'. - * @return If 'val' in 'interval', return 'val'; otherwise return the nearer - * interval bound. - */ - static T clamp_to(const Interval & i, T val); - - /** - * @brief Constructor: initialize an empty interval with members set to NaN. - */ - Interval(); - - /** - * @brief Constructor: specify exact interval bounds. - * - * @note An empty interval is specified by setting both bounds to NaN. - * @note An exception is thrown if the specified bounds are invalid. - * - * @post Construction is successful iff the interval is valid. - */ - Interval(const T & min, const T & max); - -private: - static constexpr T NaN = std::numeric_limits::quiet_NaN(); - - T min_; - T max_; - - /** - * @brief Verify that the bounds are valid in an interval. - * @note This method is private because it can only be used in the - * constructor. Once an interval has been constructed, its bounds are - * guaranteed to be valid. - */ - static bool bounds_valid(const Interval & i); -}; // class Interval - -//------------------------------------------------------------------------------ - -typedef Interval Interval_d; -typedef Interval Interval_f; - -//------------------------------------------------------------------------------ - -template -constexpr T Interval::NaN; - -//------------------------------------------------------------------------------ - -template -Interval::Interval() : min_(Interval::NaN), max_(Interval::NaN) -{ -} - -//------------------------------------------------------------------------------ - -template -Interval::Interval(const T & min, const T & max) : min_(min), max_(max) -{ - if (!Interval::bounds_valid(*this)) { - std::stringstream ss; - ss << "Attempted to construct an invalid interval: " << *this; - throw std::runtime_error(ss.str()); - } -} - -//------------------------------------------------------------------------------ - -template -bool Interval::abs_eq(const Interval & i1, const Interval & i2, const T & eps) -{ - namespace comp = helper_functions::comparisons; - - const auto both_empty = Interval::empty(i1) && Interval::empty(i2); - const auto both_non_empty = !Interval::empty(i1) && !Interval::empty(i2); - - const auto mins_equal = comp::abs_eq(Interval::min(i1), Interval::min(i2), eps); - const auto maxes_equal = comp::abs_eq(Interval::max(i1), Interval::max(i2), eps); - const auto both_non_empty_equal = both_non_empty && mins_equal && maxes_equal; - - return both_empty || both_non_empty_equal; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::zero_measure(const Interval & i) -{ - // An empty interval will return false because (NaN == NaN) is false. - return Interval::min(i) == Interval::max(i); -} - -//------------------------------------------------------------------------------ - -template -bool Interval::empty(const Interval & i) -{ - return std::isnan(Interval::min(i)) && std::isnan(Interval::max(i)); -} - -//------------------------------------------------------------------------------ - -template -bool Interval::bounds_valid(const Interval & i) -{ - const auto is_ordered = (Interval::min(i) <= Interval::max(i)); - - // Check for emptiness explicitly because it requires both bounds to be NaN - return Interval::empty(i) || is_ordered; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::is_subset_eq(const Interval & i1, const Interval & i2) -{ - const auto lower_contained = (Interval::min(i1) >= Interval::min(i2)); - const auto upper_contained = (Interval::max(i1) <= Interval::max(i2)); - return lower_contained && upper_contained; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::contains(const Interval & i, const T & value, const T & eps) -{ - return common::helper_functions::comparisons::abs_gte(value, Interval::min(i), eps) && - common::helper_functions::comparisons::abs_lte(value, Interval::max(i), eps); -} - -//------------------------------------------------------------------------------ - -template -T Interval::measure(const Interval & i) -{ - return Interval::max(i) - Interval::min(i); -} - -//------------------------------------------------------------------------------ - -template -Interval Interval::intersect(const Interval & i1, const Interval & i2) -{ - // Construct intersection assuming an intersection exists. - try { - const auto either_empty = Interval::empty(i1) || Interval::empty(i2); - const auto min = std::max(Interval::min(i1), Interval::min(i2)); - const auto max = std::min(Interval::max(i1), Interval::max(i2)); - return either_empty ? Interval() : Interval(min, max); - } catch (...) { - } - - // Otherwise, the intersection is empty. - return Interval(); -} - -//------------------------------------------------------------------------------ - -template -T Interval::clamp_to(const Interval & i, T val) -{ - // clamp val to min - val = std::max(val, Interval::min(i)); - - // clamp val to max - val = std::min(val, Interval::max(i)); - - return Interval::empty(i) ? Interval::NaN : val; -} - -//------------------------------------------------------------------------------ - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp deleted file mode 100644 index 7b8867ca096ae..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright 2017-2020 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file contains a 1D linear lookup table implementation - -#ifndef AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace helper_functions -{ - -namespace -{ - -/** - * @brief Compute a scaling between 'a' and 'b' - * @note scaling is clamped to be in the interval [0, 1] - */ -template -T interpolate(const T & a, const T & b, U scaling) -{ - static const geometry::Interval UNIT_INTERVAL(static_cast(0), static_cast(1)); - scaling = geometry::Interval::clamp_to(UNIT_INTERVAL, scaling); - - const auto m = (b - a); - const auto offset = static_cast(m) * scaling; - return a + static_cast(offset); -} - -// TODO(c.ho) support more forms of interpolation as template functor -// Actual lookup logic, assuming all invariants hold: -// Throw if value is not finite -template -T lookup_impl_1d(const std::vector & domain, const std::vector & range, const T value) -{ - if (!std::isfinite(value)) { - throw std::domain_error{"Query value is not finite (NAN or INF)"}; - } - if (value <= domain.front()) { - return range.front(); - } else if (value >= domain.back()) { - return range.back(); - } else { - // Fall through to normal case - } - - auto second_idx{0U}; - for (auto idx = 1U; idx < domain.size(); ++idx) { - if (value < domain[idx]) { - second_idx = idx; - break; - } - } - // T must be a floating point between 0 and 1 - const auto num = static_cast(value - domain[second_idx - 1U]); - const auto den = static_cast(domain[second_idx] - domain[second_idx - 1U]); - const auto t = num / den; - const auto val = interpolate(range[second_idx - 1U], range[second_idx], t); - return static_cast(val); -} - -// Check invariants for table lookup: -template -void check_table_lookup_invariants(const std::vector & domain, const std::vector & range) -{ - if (domain.size() != range.size()) { - throw std::domain_error{"Domain's size does not match range's"}; - } - if (domain.empty()) { - throw std::domain_error{"Empty domain or range"}; - } - // Check if sorted: Can start at 1 because not empty - for (auto idx = 1U; idx < domain.size(); ++idx) { - if (domain[idx] <= domain[idx - 1U]) { // This is safe because indexing starts at 1 - throw std::domain_error{"Domain is not sorted"}; - } - } -} -} // namespace - -/// Do a 1D table lookup: Does some semi-expensive O(N) error checking first. -/// If query value fall out of the domain, then the value at the corresponding edge of the domain is -/// returned. -/// \param[in] domain The domain, or set of x values -/// \param[in] range The range, or set of y values -/// \param[in] value The point in the domain to query, x -/// \return A linearly interpolated value y, corresponding to the query, x -/// \throw std::domain_error If domain or range is empty -/// \throw std::domain_error If range is not the same size as domain -/// \throw std::domain_error If domain is not sorted -/// \throw std::domain_error If value is not finite (NAN or INF) -/// \tparam T The type of the function, must be interpolatable -template -T lookup_1d(const std::vector & domain, const std::vector & range, const T value) -{ - check_table_lookup_invariants(domain, range); - - return lookup_impl_1d(domain, range, value); -} - -/// A class wrapping a 1D lookup table. Intended for more frequent lookups. Error checking is pushed -/// into the constructor and not done in the lookup function call -/// \tparam T The type of the function, must be interpolatable -template -class LookupTable1D -{ -public: - /// Constructor - /// \param[in] domain The domain, or set of x values - /// \param[in] range The range, or set of y values - /// \throw std::domain_error If domain or range is empty - /// \throw std::domain_error If range is not the same size as domain - /// \throw std::domain_error If domain is not sorted - LookupTable1D(const std::vector & domain, const std::vector & range) - : m_domain{domain}, m_range{range} - { - check_table_lookup_invariants(m_domain, m_range); - } - - /// Move constructor - /// \param[in] domain The domain, or set of x values - /// \param[in] range The range, or set of y values - /// \throw std::domain_error If domain or range is empty - /// \throw std::domain_error If range is not the same size as domain - /// \throw std::domain_error If domain is not sorted - LookupTable1D(std::vector && domain, std::vector && range) - : m_domain{domain}, m_range{range} - { - check_table_lookup_invariants(m_domain, m_range); - } - - /// Do a 1D table lookup - /// If query value fall out of the domain, then the value at the corresponding edge of the domain - /// is returned. - /// \param[in] value The point in the domain to query, x - /// \return A linearly interpolated value y, corresponding to the query, x - /// \throw std::domain_error If value is not finite - T lookup(const T value) const { return lookup_impl_1d(m_domain, m_range, value); } - /// Get the domain table - const std::vector & domain() const noexcept { return m_domain; } - /// Get the range table - const std::vector & range() const noexcept { return m_range; } - -private: - std::vector m_domain; - std::vector m_range; -}; // class LookupTable1D - -} // namespace helper_functions -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp deleted file mode 100644 index 8936e2c17d779..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp +++ /dev/null @@ -1,332 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in -/// 2D - -#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ - -#include "autoware_auto_geometry/spatial_hash_config.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include - -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup -namespace spatial_hash -{ - -/// \brief An implementation of the spatial hash or integer lattice data structure for efficient -/// (O(1)) near neighbor queries. -/// \tparam PointT The point type stored in this data structure. Must have float members x, y, and z -/// -/// This implementation can support both 2D and 3D queries -/// (though only one type per data structure), and can support queries of varying radius. This data -/// structure cannot do near neighbor lookups for euclidean distance in arbitrary dimensions. -template -class GEOMETRY_PUBLIC SpatialHashBase -{ - using Index3 = details::Index3; - // lint -e{9131} NOLINT There's no other way to make this work in a static assert - static_assert( - std::is_same::value || std::is_same::value, - "SpatialHash only works with Config2d or Config3d"); - -public: - using Hash = std::unordered_multimap; - using IT = typename Hash::const_iterator; - /// \brief Wrapper around an iterator and a distance (from some query point) - class Output - { - public: - /// \brief Constructor - /// \param[in] iterator An iterator pointing to some point - /// \param[in] distance The euclidean distance (2d or 3d) to a reference point - Output(const IT iterator, const float32_t distance) : m_iterator(iterator), m_distance(distance) - { - } - /// \brief Get stored point - /// \return A const reference to the stored point - const PointT & get_point() const { return m_iterator->second; } - /// \brief Get underlying iterator - /// \return A copy of the underlying iterator - IT get_iterator() const { return m_iterator; } - /// \brief Convert to underlying point - /// \return A reference to the underlying point - operator const PointT &() const { return get_point(); } - /// \brief Convert to underlying iterator - /// \return A copy of the iterator - operator IT() const { return get_iterator(); } - /// \brief Get distance to reference point - /// \return The distance - float32_t get_distance() const { return m_distance; } - - private: - IT m_iterator; - float32_t m_distance; - }; // class Output - using OutputVector = typename std::vector; - - /// \brief Constructor - /// \param[in] cfg The configuration object for this class - explicit SpatialHashBase(const ConfigT & cfg) - : m_config{cfg}, - m_hash(), - m_neighbors{}, // TODO(c.ho) reserve, but there's no default constructor for output - m_bins_hit{}, // zero initialization (and below) - m_neighbors_found{} - { - } - - /// \brief Inserts point - /// \param[in] pt The Point to insert - /// \return Iterator pointing to the inserted point - /// \throw std::length_error If the data structure is at capacity - IT insert(const PointT & pt) - { - if (size() >= capacity()) { - throw std::length_error{"SpatialHash: Cannot insert past capacity"}; - } - return insert_impl(pt); - } - - /// \brief Inserts a range of points - /// \param[in] begin The start of the range of points to insert - /// \param[in] end The end of the range of points to insert - /// \tparam IteratorT The iterator type - /// \throw std::length_error If the range of points to insert exceeds the data structure's - /// capacity - template - void insert(IteratorT begin, IteratorT end) - { - // This check is here for strong exception safety - if ((size() + std::distance(begin, end)) > capacity()) { - throw std::length_error{"SpatialHash: Cannot multi-insert past capacity"}; - } - for (IteratorT it = begin; it != end; ++it) { - (void)insert_impl(*it); - } - } - - /// \brief Removes the specified element from the data structure - /// \param[in] point An iterator pointing to a point to be removed - /// \return An iterator pointing to the element after the erased element - /// \throw std::domain_error If pt is invalid or does not belong to this data structure - /// - /// \note There is no reliable way to check if an iterator is invalid. The checks here are - /// based on a heuristic and is not guaranteed to find all invalid iterators. This method - /// should be used with care and only on valid iterators - IT erase(const IT point) - { - if (end() == m_hash.find(point->first)) { - throw std::domain_error{"SpatialHash: Attempting to erase invalid iterator"}; - } - return m_hash.erase(point); - } - - /// \brief Reset the state of the data structure - void clear() { m_hash.clear(); } - /// \brief Get current number of element stored in this data structure - /// \return Number of stored elements - Index size() const { return m_hash.size(); } - /// \brief Get the maximum capacity of the data structure - /// \return The capacity of the data structure - Index capacity() const { return m_config.get_capacity(); } - /// \brief Whether the hash is empty - /// \return True if data structure is empty - bool8_t empty() const { return m_hash.empty(); } - /// \brief Get iterator to beginning of data structure - /// \return Iterator - IT begin() const { return m_hash.begin(); } - /// \brief Get iterator to end of data structure - /// \return Iterator - IT end() const { return m_hash.end(); } - /// \brief Get iterator to beginning of data structure - /// \return Iterator - IT cbegin() const { return begin(); } - /// \brief Get iterator to end of data structure - /// \return Iterator - IT cend() const { return end(); } - - /// \brief Get the number of bins touched during the lifetime of this object, for debugging and - /// size tuning - /// \return The total number of bins touched during near() queries - Index bins_hit() const { return m_bins_hit; } - - /// \brief Get number of near neighbors found during the lifetime of this object, for debugging - /// and size tuning - /// \return The total number of neighbors found during near() queries - Index neighbors_found() const { return m_neighbors_found; } - -protected: - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] z The z component of the reference point, respected only if the spatial hash is not - /// 2D. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near_impl( - const float32_t x, const float32_t y, const float32_t z, const float32_t radius) - { - // reset output - m_neighbors.clear(); - // Compute bin, bin range - const Index3 ref_idx = m_config.index3(x, y, z); - const float32_t radius2 = radius * radius; - const details::BinRange idx_range = m_config.bin_range(ref_idx, radius); - Index3 idx = idx_range.first; - // For bins in radius - do { // guaranteed to have at least the bin ref_idx is in - // update book-keeping - ++m_bins_hit; - // Iterating in a square/cube pattern is easier than constructing sphere pattern - if (m_config.is_candidate_bin(ref_idx, idx, radius2)) { - // For point in bin - const Index jdx = m_config.index(idx); - const auto range = m_hash.equal_range(jdx); - for (auto it = range.first; it != range.second; ++it) { - const auto & pt = it->second; - const float32_t dist2 = m_config.distance_squared(x, y, z, pt); - if (dist2 <= radius2) { - // Only compute true distance if necessary - m_neighbors.emplace_back(it, sqrtf(dist2)); - } - } - } - } while (m_config.next_bin(idx_range, idx)); - // update book-keeping - m_neighbors_found += m_neighbors.size(); - return m_neighbors; - } - -private: - /// \brief Internal insert method with no error checking - /// \param[in] pt The Point to insert - GEOMETRY_LOCAL IT insert_impl(const PointT & pt) - { - // Compute bin - const Index idx = - m_config.bin(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt)); - // Insert into bin - return m_hash.insert(std::make_pair(idx, pt)); - } - - const ConfigT m_config; - Hash m_hash; - OutputVector m_neighbors; - Index m_bins_hit; - Index m_neighbors_found; -}; // class SpatialHashBase - -/// \brief The class to be used for specializing on -/// apex_app::common::geometry::spatial_hash::SpatialHashBase to provide different function -/// signatures on 2D and 3D configurations -/// \tparam PointT The point type stored in this data structure. Must have float members x, y and z -template -class GEOMETRY_PUBLIC SpatialHash; - -/// \brief Explicit specialization of SpatialHash for 2D configuration -/// \tparam PointT The point type stored in this data structure. -template -class GEOMETRY_PUBLIC SpatialHash : public SpatialHashBase -{ -public: - using OutputVector = typename SpatialHashBase::OutputVector; - - explicit SpatialHash(const Config2d & cfg) : SpatialHashBase(cfg) {} - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const float32_t x, const float32_t y, const float32_t radius) - { - return this->near_impl(x, y, 0.0F, radius); - } - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] pt The reference point. Only the x and y members are respected. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const PointT & pt, const float32_t radius) - { - return near(point_adapter::x_(pt), point_adapter::y_(pt), radius); - } -}; - -/// \brief Explicit specialization of SpatialHash for 3D configuration -/// \tparam PointT The point type stored in this data structure. Must have float members x, y and z -template -class GEOMETRY_PUBLIC SpatialHash : public SpatialHashBase -{ -public: - using OutputVector = typename SpatialHashBase::OutputVector; - - explicit SpatialHash(const Config3d & cfg) : SpatialHashBase(cfg) {} - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] z The z component of the reference point, respected only if the spatial hash is not - /// 2D. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near( - const float32_t x, const float32_t y, const float32_t z, const float32_t radius) - { - return this->near_impl(x, y, z, radius); - } - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] pt The reference point. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const PointT & pt, const float32_t radius) - { - return near(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt), radius); - } -}; - -template -using SpatialHash2d = SpatialHash; -template -using SpatialHash3d = SpatialHash; -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp deleted file mode 100644 index 24c4d6e879d4a..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp +++ /dev/null @@ -1,450 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in -/// 2D - -#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include -#include - -#include -#include -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup -namespace spatial_hash -{ -/// \brief Index type for identifying bins of the hash/lattice -using Index = std::size_t; -/// \brief Spatial hash functionality not intended to be used by an external user -namespace details -{ -/// \brief Internal struct for packing three indices together -/// -/// The use of this struct publicly is a violation of our coding standards, but I claim it's -/// fine because (a) it's details, (b) it is literally three unrelated members packaged together. -/// This type is needed for conceptual convenience so I don't have massive function parameter -/// lists -struct GEOMETRY_PUBLIC Index3 -{ - Index x; - Index y; - Index z; -}; // struct Index3 - -using BinRange = std::pair; -} // namespace details - -/// \brief The base class for the configuration object for the SpatialHash class -/// \tparam Derived The type of the derived class to support static polymorphism/CRTP -template -class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp -{ -public: - /// \brief Constructor for spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] min_z The minimum z value for the spatial hash - /// \param[in] max_z The maximum z value for the spatial hash - /// \param[in] radius The look up radius - /// \param[in] capacity The maximum number of points the spatial hash can store - Config( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity) - : m_min_x{min_x}, - m_min_y{min_y}, - m_min_z{min_z}, - m_max_x{max_x}, - m_max_y{max_y}, - m_max_z{max_z}, - m_side_length{radius}, - m_side_length2{radius * radius}, - m_side_length_inv{1.0F / radius}, - m_capacity{capacity}, - m_max_x_idx{check_basis_direction(min_x, max_x)}, - m_max_y_idx{check_basis_direction(min_y, max_y)}, - m_max_z_idx{check_basis_direction(min_z, max_z)}, - m_y_stride{m_max_x_idx + 1U}, - m_z_stride{m_y_stride * (m_max_y_idx + 1U)} - { - if (radius <= 0.0F) { - throw std::domain_error("Error constructing SpatialHash: must have positive side length"); - } - - if ((m_max_y_idx + m_y_stride) > std::numeric_limits::max() / 2U) { - // TODO(c.ho) properly check for multiplication overflow - throw std::domain_error("SpatialHash::Config: voxel index may overflow!"); - } - // small fudging to prevent weird boundary effects - // (e.g (x=x_max, y) rolls index over to (x=0, y+1) - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - // lint -e{1938} read only access is fine NOLINT - m_max_x -= FEPS; - m_max_y -= FEPS; - m_max_z -= FEPS; - if ((m_z_stride + m_max_z_idx) > std::numeric_limits::max() / 2U) { - // TODO(c.ho) properly check for multiplication overflow - throw std::domain_error("SpatialHash::Config: voxel index may overflow!"); - } - // I don't know if this is even possible given other checks - if (std::numeric_limits::max() == m_max_z_idx) { - throw std::domain_error("SpatialHash::Config: max z index exceeds reasonable value"); - } - } - - /// \brief Given a reference index triple, compute the first and last bin - /// \param[in] ref The reference index triple - /// \param[in] radius The allowable radius for any point in the reference bin to any point in the - /// range - /// \return A pair where the first element is an index triple of the first bin, and the second - /// element is an index triple for the last bin - details::BinRange bin_range(const details::Index3 & ref, const float radius) const - { - // Compute distance in units of voxels - const Index i_radius = static_cast(std::ceil(radius / m_side_length)); - // Dumb ternary because potentially unsigned Index type - const Index x_min = (ref.x > i_radius) ? (ref.x - i_radius) : 0U; - const Index y_min = (ref.y > i_radius) ? (ref.y - i_radius) : 0U; - const Index z_min = (ref.z > i_radius) ? (ref.z - i_radius) : 0U; - // In 2D mode, m_max_z should be 0, same with ref.z - const Index x_max = std::min(ref.x + i_radius, m_max_x_idx); - const Index y_max = std::min(ref.y + i_radius, m_max_y_idx); - const Index z_max = std::min(ref.z + i_radius, m_max_z_idx); - // return bottom-left portion of cube and top-right portion of cube - return {{x_min, y_min, z_min}, {x_max, y_max, z_max}}; - } - - /// \brief Get next index within a given range - /// \return True if idx is valid and still in range - /// \param[in] range The max and min bin indices - /// \param[inout] idx The index to be incremented, updated even if a negative result occurs - bool8_t next_bin(const details::BinRange & range, details::Index3 & idx) const - { - // TODO(c.ho) is there any way to make this neater without triple nested if? - bool8_t ret = true; - ++idx.x; - if (idx.x > range.second.x) { - idx.x = range.first.x; - ++idx.y; - if (idx.y > range.second.y) { - idx.y = range.first.y; - ++idx.z; - if (idx.z > range.second.z) { - ret = false; - } - } - } - return ret; - } - /// \brief Get the maximum capacity of the spatial hash - /// \return The capacity - Index get_capacity() const { return m_capacity; } - - /// \brief Getter for the side length, equivalently the lookup radius - float32_t radius2() const { return m_side_length2; } - - //////////////////////////////////////////////////////////////////////////////////////////// - // "Polymorphic" API - /// \brief Compute the single index given a point - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The combined index of the bin for a given point - Index bin(const float32_t x, const float32_t y, const float32_t z) const - { - return this->impl().bin_(x, y, z); - } - /// \brief Compute whether the query bin and reference bin could possibly contain a pair of points - /// such that their distance is within a certain threshold - /// \param[in] ref The index triple of the bin containing the reference point - /// \param[in] query The index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if query and ref could possibly hold points within reference distance to one - /// another - bool is_candidate_bin( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const - { - return this->impl().valid(ref, query, ref_distance2); - } - /// \brief Compute the decomposed index given a point - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3(const float32_t x, const float32_t y, const float32_t z) const - { - return this->impl().index3_(x, y, z); - } - /// \brief Compute the composed single index given a decomposed index - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index(const details::Index3 & idx) const { return this->impl().index_(idx); } - /// \brief Compute the squared distance between the two points - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (2d or 3d) - template - float32_t distance_squared( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - return this->impl().distance_squared_(x, y, z, pt); - } - -protected: - /// \brief Computes the index in the x basis direction - /// \param[in] x The x value of a point - /// \return The x offset of the index - Index x_index(const float32_t x) const - { - return static_cast( - std::floor((std::min(std::max(x, m_min_x), m_max_x) - m_min_x) * m_side_length_inv)); - } - /// \brief Computes the index in the y basis direction - /// \param[in] y The y value of a point - /// \return The x offset of the index - Index y_index(const float32_t y) const - { - return static_cast( - std::floor((std::min(std::max(y, m_min_y), m_max_y) - m_min_y) * m_side_length_inv)); - } - /// \brief Computes the index in the z basis direction - /// \param[in] z The z value of a point - /// \return The x offset of the index - Index z_index(const float32_t z) const - { - return static_cast( - std::floor((std::min(std::max(z, m_min_z), m_max_z) - m_min_z) * m_side_length_inv)); - } - /// \brief Compose the provided index offsets - Index bin_impl(const Index xdx, const Index ydx) const { return xdx + (ydx * m_y_stride); } - /// \brief Compose the provided index offsets - Index bin_impl(const Index xdx, const Index ydx, const Index zdx) const - { - return bin_impl(xdx, ydx) + (zdx * m_z_stride); - } - - /// \brief The index offset of a point given it's x and y values - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \return The index of the point in the 2D case, or the offset within a z-slice of the hash - Index bin_impl(const float32_t x, const float32_t y) const - { - return bin_impl(x_index(x), y_index(y)); - } - /// \brief The index of a point given it's x, y and z values - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_impl(const float32_t x, const float32_t y, const float32_t z) const - { - return bin_impl(x, y) + (z_index(z) * m_z_stride); - } - /// \brief The distance between two indices as a float, where adjacent indices have zero - /// distance (e.g. dist(0, 1) = 0) - float32_t idx_distance(const Index ref_idx, const Index query_idx) const - { - /// Not using fabs because Index is (possibly) unsigned - const Index i_dist = (ref_idx >= query_idx) ? (ref_idx - query_idx) : (query_idx - ref_idx); - float32_t dist = static_cast(i_dist) - 1.0F; - return std::max(dist, 0.0F); - } - - /// \brief Get side length squared - float side_length2() const { return m_side_length2; } - -private: - /// \brief Sanity check a range in a basis direction - /// \return The number of voxels in the given basis direction - Index check_basis_direction(const float32_t min, const float32_t max) const - { - if (min >= max) { - throw std::domain_error("SpatialHash::Config: must have min < max"); - } - // This family of checks is to ensure that you don't get weird casting effects due to huge - // floating point values - const float64_t dmax = static_cast(max); - const float64_t dmin = static_cast(min); - const float64_t width = (dmax - dmin) * static_cast(m_side_length_inv); - constexpr float64_t flt_max = static_cast(std::numeric_limits::max()); - if (flt_max <= width) { - throw std::domain_error("SpatialHash::Config: voxel size approaching floating point limit"); - } - return static_cast(width); - } - float32_t m_min_x; - float32_t m_min_y; - float32_t m_min_z; - float32_t m_max_x; - float32_t m_max_y; - float32_t m_max_z; - float32_t m_side_length; - float32_t m_side_length2; - float32_t m_side_length_inv; - Index m_capacity; - Index m_max_x_idx; - Index m_max_y_idx; - Index m_max_z_idx; - Index m_y_stride; - Index m_z_stride; -}; // class Config - -/// \brief Configuration class for a 2d spatial hash -class GEOMETRY_PUBLIC Config2d : public Config -{ -public: - /// \brief Config constructor for 2D spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] radius The lookup distance - /// \param[in] capacity The maximum number of points the spatial hash can store - Config2d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t radius, const Index capacity); - /// \brief The index of a point given it's x, y and z values, 2d implementation - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Determine if a bin could possibly hold a point within a distance to any point in a - /// reference bin for the 2D case - /// \param[in] ref The decomposed index triple of the reference bin - /// \param[in] query The decomposed index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if the reference bin and query bin could possibly hold a point within the - /// reference distance - bool valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const; - /// \brief Compute the decomposed index given a point, 2d implementation - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Compute the composed single index given a decomposed index, 2d implementation - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index_(const details::Index3 & idx) const; - /// \brief Compute the squared distance between the two points, 2d implementation - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (2d) - template - float32_t distance_squared_( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - (void)z; - const float32_t dx = x - point_adapter::x_(pt); - const float32_t dy = y - point_adapter::y_(pt); - return (dx * dx) + (dy * dy); - } -}; // class Config2d - -/// \brief Configuration class for a 3d spatial hash -class GEOMETRY_PUBLIC Config3d : public Config -{ -public: - /// \brief Config constructor for a 3d spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] min_z The minimum z value for the spatial hash - /// \param[in] max_z The maximum z value for the spatial hash - /// \param[in] radius The lookup distance - /// \param[in] capacity The maximum number of points the spatial hash can store - Config3d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity); - /// \brief The index of a point given it's x, y and z values, 3d implementation - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Determine if a bin could possibly hold a point within a distance to any point in a - /// reference bin for the 3D case - /// \param[in] ref The decomposed index triple of the reference bin - /// \param[in] query The decomposed index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if the reference bin and query bin could possibly hold a point within the - /// reference distance - bool valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const; - /// \brief Compute the decomposed index given a point, 3d implementation - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Compute the composed single index given a decomposed index, 3d implementation - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index_(const details::Index3 & idx) const; - /// \brief Compute the squared distance between the two points, 3d implementation - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (3d) - template - float32_t distance_squared_( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - const float32_t dx = x - point_adapter::x_(pt); - const float32_t dy = y - point_adapter::y_(pt); - const float32_t dz = z - point_adapter::z_(pt); - return (dx * dx) + (dy * dy) + (dz * dz); - } -}; // class Config3d -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp deleted file mode 100644 index 8972246997997..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#define GEOMETRY_PUBLIC __declspec(dllexport) -#define GEOMETRY_LOCAL -#else // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#define GEOMETRY_PUBLIC __declspec(dllimport) -#define GEOMETRY_LOCAL -#endif // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#elif defined(__linux__) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#elif defined(QNX) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#else // defined(__linux__) -#error "Unsupported Build Configuration" -#endif // defined(__WIN32) -#endif // AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_geometry/package.xml b/common/autoware_auto_geometry/package.xml deleted file mode 100644 index f53412298a485..0000000000000 --- a/common/autoware_auto_geometry/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - autoware_auto_geometry - 1.0.0 - Geometry related algorithms - Satoshi Ota - Apache License 2.0 - - Apex.AI, Inc. - - ament_cmake_auto - autoware_cmake - - autoware_auto_common - autoware_auto_geometry_msgs - autoware_auto_planning_msgs - autoware_auto_tf2 - autoware_auto_vehicle_msgs - geometry_msgs - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - osrf_testing_tools_cpp - - - ament_cmake - - diff --git a/common/autoware_auto_geometry/src/bounding_box.cpp b/common/autoware_auto_geometry/src/bounding_box.cpp deleted file mode 100644 index 3a4ea96a151a2..0000000000000 --- a/common/autoware_auto_geometry/src/bounding_box.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" - -#include -// cspell: ignore eigenbox -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -// cspell: ignore lfit -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -#include - -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ -//////////////////////////////////////////////////////////////////////////////// -void size_2d(const decltype(BoundingBox::corners) & corners, geometry_msgs::msg::Point32 & ret) -{ - ret.x = std::max( - norm_2d(minus_2d(corners[1U], corners[0U])), std::numeric_limits::epsilon()); - ret.y = std::max( - norm_2d(minus_2d(corners[2U], corners[1U])), std::numeric_limits::epsilon()); -} -//////////////////////////////////////////////////////////////////////////////// -void finalize_box(const decltype(BoundingBox::corners) & corners, BoundingBox & box) -{ - (void)std::copy(&corners[0U], &corners[corners.size()], &box.corners[0U]); - // orientation: this is robust to lines - const float32_t yaw_rad_2 = - atan2f(box.corners[2U].y - box.corners[1U].y, box.corners[2U].x - box.corners[1U].x) * 0.5F; - box.orientation.w = cosf(yaw_rad_2); - box.orientation.z = sinf(yaw_rad_2); - // centroid - box.centroid = times_2d(plus_2d(corners[0U], corners[2U]), 0.5F); -} - -autoware_auto_perception_msgs::msg::Shape make_shape(const BoundingBox & box) -{ - autoware_auto_perception_msgs::msg::Shape retval; - // Polygon is 2D rectangle - geometry_msgs::msg::Polygon polygon; - auto & points = polygon.points; - points.resize(4); - - // Note that the x and y of size field in BoundingBox should be swapped when being used to - // compute corners. - // origin of reference system: centroid of bbox - points[0].x = -0.5F * box.size.y; - points[0].y = -0.5F * box.size.x; - points[0].z = -box.size.z; - - points[1] = points[0]; - points[1].y += box.size.x; - - points[2] = points[1]; - points[2].x += box.size.y; - - points[3] = points[2]; - points[3].y -= box.size.x; - - retval.footprint = polygon; - retval.dimensions.z = 2 * box.size.z; - - return retval; -} - -autoware_auto_perception_msgs::msg::DetectedObject make_detected_object(const BoundingBox & box) -{ - autoware_auto_perception_msgs::msg::DetectedObject ret; - - ret.kinematics.pose_with_covariance.pose.position.x = static_cast(box.centroid.x); - ret.kinematics.pose_with_covariance.pose.position.y = static_cast(box.centroid.y); - ret.kinematics.pose_with_covariance.pose.position.z = static_cast(box.centroid.z); - ret.kinematics.pose_with_covariance.pose.orientation.x = static_cast(box.orientation.x); - ret.kinematics.pose_with_covariance.pose.orientation.y = static_cast(box.orientation.y); - ret.kinematics.pose_with_covariance.pose.orientation.z = static_cast(box.orientation.z); - ret.kinematics.pose_with_covariance.pose.orientation.w = static_cast(box.orientation.w); - ret.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; - - ret.shape = make_shape(box); - - ret.existence_probability = 1.0F; - - autoware_auto_perception_msgs::msg::ObjectClassification label; - label.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; - label.probability = 1.0F; - ret.classification.emplace_back(std::move(label)); - - return ret; -} - -std::vector GEOMETRY_PUBLIC get_transformed_corners( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation) -{ - std::vector retval(shape_msg.footprint.points.size()); - geometry_msgs::msg::TransformStamped tf; - tf.transform.rotation = orientation; - tf.transform.translation.x = centroid.x; - tf.transform.translation.y = centroid.y; - tf.transform.translation.z = centroid.z; - - for (size_t i = 0U; i < shape_msg.footprint.points.size(); ++i) { - tf2::doTransform(shape_msg.footprint.points[i], retval[i], tf); - } - return retval; -} - -} // namespace details -/////////////////////////////////////////////////////////////////////////////// -// pre-compilation -using autoware::common::types::PointXYZIF; -template BoundingBox minimum_area_bounding_box(std::list & list); -template BoundingBox minimum_perimeter_bounding_box(std::list & list); -using PointXYZIFVIT = std::vector::iterator; -template BoundingBox eigenbox_2d(const PointXYZIFVIT begin, const PointXYZIFVIT end); -template BoundingBox lfit_bounding_box_2d( - const PointXYZIFVIT begin, const PointXYZIFVIT end); -using geometry_msgs::msg::Point32; -template BoundingBox minimum_area_bounding_box(std::list & list); -template BoundingBox minimum_perimeter_bounding_box(std::list & list); -using Point32VIT = std::vector::iterator; -template BoundingBox eigenbox_2d(const Point32VIT begin, const Point32VIT end); -template BoundingBox lfit_bounding_box_2d(const Point32VIT begin, const Point32VIT end); -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware diff --git a/common/autoware_auto_geometry/src/spatial_hash.cpp b/common/autoware_auto_geometry/src/spatial_hash.cpp deleted file mode 100644 index ffd91aaa08b3a..0000000000000 --- a/common/autoware_auto_geometry/src/spatial_hash.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/spatial_hash.hpp" - -#include -// lint -e537 NOLINT repeated include file due to cpplint rule -#include -// lint -e537 NOLINT repeated include file due to cpplint rule -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace spatial_hash -{ -//////////////////////////////////////////////////////////////////////////////// -Config2d::Config2d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t radius, const Index capacity) -: Config(min_x, max_x, min_y, max_y, {}, std::numeric_limits::min(), radius, capacity) -{ -} -//////////////////////////////////////////////////////////////////////////////// -Index Config2d::bin_(const float32_t x, const float32_t y, const float32_t z) const -{ - (void)z; - return bin_impl(x, y); -} -//////////////////////////////////////////////////////////////////////////////// -bool Config2d::valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const -{ - const float dx = idx_distance(ref.x, query.x); - const float dy = idx_distance(ref.y, query.y); - // minor algebraic manipulation happening below - return (((dx * dx) + (dy * dy)) * side_length2()) <= ref_distance2; -} -//////////////////////////////////////////////////////////////////////////////// -details::Index3 Config2d::index3_(const float32_t x, const float32_t y, const float32_t z) const -{ - (void)z; - return {x_index(x), y_index(y), Index{}}; // zero initialization -} -//////////////////////////////////////////////////////////////////////////////// -Index Config2d::index_(const details::Index3 & idx) const -{ - return bin_impl(idx.x, idx.y); -} -//////////////////////////////////////////////////////////////////////////////// -Config3d::Config3d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity) -: Config(min_x, max_x, min_y, max_y, min_z, max_z, radius, capacity) -{ -} -//////////////////////////////////////////////////////////////////////////////// -Index Config3d::bin_(const float32_t x, const float32_t y, const float32_t z) const -{ - return bin_impl(x, y, z); -} -//////////////////////////////////////////////////////////////////////////////// -bool Config3d::valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const -{ - const float dx = idx_distance(ref.x, query.x); - const float dy = idx_distance(ref.y, query.y); - const float dz = idx_distance(ref.z, query.z); - // minor algebraic manipulation happening below - return (((dx * dx) + (dy * dy) + (dz * dz)) * side_length2()) <= ref_distance2; -} -//////////////////////////////////////////////////////////////////////////////// -details::Index3 Config3d::index3_(const float32_t x, const float32_t y, const float32_t z) const -{ - return {x_index(x), y_index(y), z_index(z)}; // zero initialization -} -//////////////////////////////////////////////////////////////////////////////// -Index Config3d::index_(const details::Index3 & idx) const -{ - return bin_impl(idx.x, idx.y, idx.z); -} -//////////////////////////////////////////////////////////////////////////////// -template class SpatialHash; -template class SpatialHash; -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware diff --git a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp deleted file mode 100644 index a179fbde5f151..0000000000000 --- a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp +++ /dev/null @@ -1,550 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef TEST_BOUNDING_BOX_HPP_ -#define TEST_BOUNDING_BOX_HPP_ - -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -// cspell: ignore lfit -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -#include - -#include -#include -#include - -using autoware::common::geometry::point_adapter::x_; -using autoware::common::geometry::point_adapter::xr_; -using autoware::common::geometry::point_adapter::y_; -using autoware::common::geometry::point_adapter::yr_; -using autoware_auto_perception_msgs::msg::BoundingBox; - -template -class BoxTest : public ::testing::Test -{ -protected: - std::list points; - BoundingBox box; - - void minimum_area_bounding_box() - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::minimum_area_bounding_box(points); - // apex_test_tools::memory_test::stop(); - } - - void minimum_perimeter_bounding_box() - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::minimum_perimeter_bounding_box(points); - // apex_test_tools::memory_test::stop(); - } - - // cspell: ignore eigenbox - template - void eigenbox(const IT begin, const IT end) - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::eigenbox_2d(begin, end); - // apex_test_tools::memory_test::stop(); - } - template - void lfit_bounding_box_2d(const IT begin, const IT end) - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::lfit_bounding_box_2d(begin, end); - // apex_test_tools::memory_test::stop(); - } - - PointT make(const float x, const float y) - { - PointT ret; - xr_(ret) = x; - yr_(ret) = y; - return ret; - } - - void check(const float cx, const float cy, const float sx, const float sy, const float val) const - { - constexpr float32_t TOL = 1.0E-4F; - ASSERT_LT(fabsf(box.size.x - sx), TOL); - ASSERT_LT(fabsf(box.size.y - sy), TOL); - ASSERT_LT(fabsf(box.value - val), TOL) << box.value; - - ASSERT_LT(fabsf(box.centroid.x - cx), TOL); - ASSERT_LT(fabsf(box.centroid.y - cy), TOL); - } - - void test_corners(const std::vector & expect, const float TOL = 1.0E-6F) const - { - for (uint32_t idx = 0U; idx < 4U; ++idx) { - bool found = false; - for (auto & p : expect) { - if (fabsf(x_(p) - box.corners[idx].x) < TOL && fabsf(y_(p) - box.corners[idx].y) < TOL) { - found = true; - break; - } - } - ASSERT_TRUE(found) << idx << ": " << box.corners[idx].x << ", " << box.corners[idx].y; - } - } - - /// \brief th_deg - phi_deg, normalized to +/- 180 deg - float32_t angle_distance_deg(const float th_deg, const float phi_deg) const - { - return fmodf((th_deg - phi_deg) + 540.0F, 360.0F) - 180.0F; - } - - /// \brief converts a radian value to a degree value - float32_t rad2deg(const float rad_val) const { return rad_val * 57.2958F; } - - void test_orientation(const float ref_angle_deg, const float TOL = 1.0E-4F) const - { - bool found = false; - const float angle_deg = rad2deg(2.0F * asinf(box.orientation.z)); - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 90.0F)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 180.0F)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 270.0F)) < TOL; - ASSERT_TRUE(found) << angle_deg; - } -}; // BoxTest - -// Instantiate tests for given types, add more types here as they are used -using PointTypesBoundingBox = - ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(BoxTest, PointTypesBoundingBox, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -// TODO(c.ho) consider typed and parameterized tests: -// https://stackoverflow.com/questions/3258230/passing-a-typename-and-string-to-parameterized-test-using-google-test - -/////////////////////////////////////////// -TYPED_TEST(BoxTest, PointSegmentDistance) -{ - using autoware::common::geometry::closest_segment_point_2d; - using autoware::common::geometry::point_line_segment_distance_2d; - // normal case - TypeParam p = this->make(-1.0F, 0.0F); - TypeParam q = this->make(-1.0F, 2.0F); - TypeParam r = this->make(1.0F, 1.0F); - TypeParam t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -1.0F); - ASSERT_FLOAT_EQ(y_(t), 1.0F); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 2.0F); - // boundary case - p = this->make(1.0F, 0.0F); - q = this->make(-2.0F, 0.0F); - r = this->make(-5.0F, 0.0F); - t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -2.0F); - ASSERT_NEAR(y_(t), 0.0F, std::numeric_limits::epsilon()); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 3.0F); - // singular case - p = this->make(1.0F, 5.0F); - q = this->make(1.0F, 5.0F); - r = this->make(1.0F, 1.0F); - t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), 1.0F); - ASSERT_FLOAT_EQ(y_(t), 5.0F); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 4.0F); -} - -TYPED_TEST(BoxTest, ClosestPointOnLine) -{ - using autoware::common::geometry::closest_line_point_2d; - // normal case - TypeParam p = this->make(-1.0F, 0.0F); - TypeParam q = this->make(-1.0F, 2.0F); - TypeParam r = this->make(1.0F, 1.0F); - TypeParam t = closest_line_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -1.0F); - ASSERT_FLOAT_EQ(y_(t), 1.0F); - // out-of-boundary case - p = this->make(1.0F, 0.0F); - q = this->make(-2.0F, 0.0F); - r = this->make(-5.0F, 0.0F); - t = closest_line_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -5.0F); - ASSERT_NEAR(y_(t), 0.0F, std::numeric_limits::epsilon()); - // singular case - p = this->make(1.0F, 5.0F); - q = this->make(1.0F, 5.0F); - r = this->make(1.0F, 1.0F); - EXPECT_THROW(t = closest_line_point_2d(p, q, r), std::runtime_error); -} - -TYPED_TEST(BoxTest, Basic) -{ - const std::vector data{ - {this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1)}}; - this->points.insert(this->points.begin(), data.begin(), data.end()); - - this->minimum_area_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 1.0F); - this->test_corners(data); - this->test_orientation(0.0F); - - this->minimum_perimeter_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 2.0F); - this->test_corners(data); - this->test_orientation(0.0F); -} - -// -TYPED_TEST(BoxTest, OrientedTriangle) -{ - this->points.insert(this->points.begin(), {this->make(5, 5), this->make(7, 7), this->make(5, 7)}); - - this->minimum_area_bounding_box(); - - this->check(5.5F, 6.5F, sqrtf(2.0F), 2.0F * sqrtf(2.0F), 4.0F); - this->test_corners({this->make(5, 5), this->make(7, 7), this->make(4, 6), this->make(6, 8)}); - this->test_orientation(45.0F); - - this->minimum_perimeter_bounding_box(); - - this->check(6.0F, 6.0F, 2.0F, 2.0F, 4.0F); - this->test_corners({this->make(5, 5), this->make(7, 7), this->make(5, 7), this->make(7, 5)}); - this->test_orientation(0.0F); -} -// -TYPED_TEST(BoxTest, Hull) -{ - const uint32_t FUZZ_SIZE = 1024U; - const float dx = 9.0F; - const float dy = 15.0F; - const float rx = 10.0F; - const float ry = 5.0F; - const float dth = 0.0F; - - ASSERT_EQ(FUZZ_SIZE % 4U, 0U); - - // fuzz part 1 - for (uint32_t idx = 0U; idx < FUZZ_SIZE; ++idx) { - const float th = ((idx * autoware::common::types::TAU) / FUZZ_SIZE) + dth; - this->points.push_back(this->make(rx * cosf(th) + dx, ry * sinf(th) + dy)); - } - - this->minimum_area_bounding_box(); - - // allow 10cm = 2% of size for tolerance - const float TOL_M = 0.1F; - ASSERT_LT(fabsf(this->box.centroid.x - dx), TOL_M); - ASSERT_LT(fabsf(this->box.centroid.y - dy), TOL_M); - - this->test_corners( - {this->make(dx + rx, dy + ry), this->make(dx - rx, dy + ry), this->make(dx - rx, dy - ry), - this->make(dx + rx, dy - ry)}, - TOL_M); - - this->test_orientation(this->rad2deg(dth), 1.0F); - // allow 1 degree of tolerance - - ASSERT_LT(fabsf(this->box.size.y - 2.0F * rx), TOL_M); - ASSERT_LT(fabsf(this->box.size.x - 2.0F * ry), TOL_M); - ASSERT_FLOAT_EQ(this->box.value, this->box.size.x * this->box.size.y); -} - -// -TYPED_TEST(BoxTest, Collinear) -{ - this->points.insert( - this->points.begin(), {this->make(-2, -2), this->make(-3, -2), this->make(-4, -2), - this->make(-2, -4), this->make(-3, -4), this->make(-4, -4), - this->make(-2, -3), this->make(-2, -3), this->make(-2, -4)}); - - this->minimum_area_bounding_box(); - - this->check(-3.0F, -3.0F, 2.0F, 2.0F, 4.0F); - this->test_corners( - {this->make(-2, -2), this->make(-2, -4), this->make(-4, -4), this->make(-4, -2)}); - this->test_orientation(0.0F); -} - -// -TYPED_TEST(BoxTest, Line1) -{ - this->points.insert( - this->points.begin(), {this->make(-4, 3), this->make(-8, 6), this->make(-12, 9), - this->make(-16, 12), this->make(-20, 15), this->make(-24, 18), - this->make(-28, 21), this->make(-32, 24), this->make(-36, 27)}); - - this->minimum_area_bounding_box(); - - this->check(-20.0F, 15.0F, 1.0E-6F, 40.0F, 4.0E-5F); - this->test_orientation(this->rad2deg(atan2f(3, -4))); - this->test_corners( - {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make(-36, 27)}); - - this->minimum_perimeter_bounding_box(); - - this->check(-20.0F, 15.0F, 1.0E-6F, 40.0F, 40.00001F); - this->test_orientation(this->rad2deg(atan2f(3, -4))); - this->test_corners( - {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make(-36, 27)}); -} - -// -TYPED_TEST(BoxTest, Line2) -{ - this->points.insert( - this->points.begin(), - {this->make(4, 0), this->make(8, 0), this->make(12, 0), this->make(16, 0), this->make(20, 0), - this->make(24, 0), this->make(28, 0), this->make(32, 0), this->make(36, 0)}); - - this->minimum_area_bounding_box(); - - this->check(20.0F, 0.0F, 1.0E-6F, 32.0F, 3.2E-5F); - this->test_orientation(0.0F); - this->test_corners({this->make(4, 0), this->make(36, 0), this->make(4, 0), this->make(36, 0)}); -} - -// -TYPED_TEST(BoxTest, Line3) -{ - this->points.insert( - this->points.begin(), - {this->make(4, 3), this->make(8, 6), this->make(12, 9), this->make(16, 12), this->make(20, 15), - this->make(24, 18), this->make(28, 21), this->make(32, 24), this->make(36, 27)}); - - this->minimum_area_bounding_box(); - - this->check(20.0F, 15.0F, 1.0E-6F, 40.0F, 4.0E-5F); - this->test_orientation(this->rad2deg(atan2f(3, 4))); - this->test_corners({this->make(4, 3), this->make(36, 27), this->make(4, 3), this->make(36, 27)}); -} - -/* _ - /_/ <-- first guess is suboptimal - -*/ -TYPED_TEST(BoxTest, SuboptimalInit) -{ - this->points.insert( - this->points.begin(), - {this->make(8, 15), this->make(17, 0), this->make(0, 0), this->make(25, 15)}); - - this->minimum_area_bounding_box(); - - this->check(12.5F, 7.5F, 15.0F, 25.0F, 375.0F); - this->test_orientation(this->rad2deg(atan2f(15, 8))); - // these are approximate. - this->test_corners( - {this->make(0, 0), this->make(25, 15), this->make(11.7647F, 22.0588F), - this->make(13.2353F, -7.05882F)}, - 0.1F); -} - -// -TYPED_TEST(BoxTest, Centered) -{ - this->points.insert( - this->points.begin(), - {this->make(-1, 0), this->make(1, 0), this->make(0, -1), this->make(0, 1)}); - - this->minimum_area_bounding_box(); - - this->check(0.0F, 0.0F, sqrtf(2.0F), sqrtf(2.0F), 2.0F); - this->test_orientation(45.0F); - this->test_corners({this->make(-1, 0), this->make(1, 0), this->make(0, 1), this->make(0, -1)}); -} - -// convex_hull is imperfect in this case, check if this can save the day -TYPED_TEST(BoxTest, OverlappingPoints) -{ - this->points.insert( - this->points.begin(), {this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1), - this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1)}); - - this->minimum_area_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 1.0F); - this->test_orientation(0.0F); - this->test_corners({this->make(0, 0), this->make(1, 0), this->make(0, 1), this->make(1, 1)}); -} - -// check that minimum perimeter box is different from minimum area box -TYPED_TEST(BoxTest, Perimeter) -{ - this->points.insert( - this->points.begin(), {this->make(0, 0), this->make(0, 1), this->make(0, 2), this->make(0, 3), - this->make(0, 4), this->make(1, -0.1), // small fudge to force diagonal - this->make(2, 0), this->make(3, 0)}); - - this->minimum_area_bounding_box(); - - this->check(0.54F, 1.28F, 5.0F, 12.0F / 5.0F, 12.0F); - this->test_orientation(-53.13F, 0.001F); - this->test_corners( - {this->make(3, 0), this->make(0, 4), this->make(-1.92, 2.56), this->make(1.08, -1.44)}); - - // eigenbox should produce AABB TODO(c.ho) - // compute minimum perimeter box - this->minimum_perimeter_bounding_box(); - this->check(1.5F, 1.95F, 4.1F, 3.0F, 7.1F); - // perimeter for first box would be 14.8 - this->test_orientation(0.0F, 0.001F); - this->test_corners( - {this->make(3, -0.1), this->make(0, 4), this->make(3, 4), this->make(0, -0.1)}); -} - -// bounding box is diagonal on an L -TYPED_TEST(BoxTest, Eigenbox1) -{ - std::vector v{this->make(0, 0), this->make(0, 1), - this->make(-1, 2), // small fudge to force diagonal - this->make(0, 3), this->make(0, 4), - this->make(1, 0), this->make(2, -1), // small fudge to force diagonal - this->make(3, 0), this->make(4, 0)}; - this->points.insert(this->points.begin(), v.begin(), v.end()); - - // rotating calipers should produce a diagonal box - this->minimum_area_bounding_box(); - const float r = 4.0F; - - this->check(1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r, r * r); - const std::vector diag_corners{ - this->make(4, 0), this->make(0, 4), this->make(-2, 2), this->make(2, -2)}; - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - - //// perimeter should also produce diagonal //// - this->minimum_perimeter_bounding_box(); - this->check( - 1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r, r * (sqrtf(2.0F) + (1.0F / sqrtf(2.0F)))); - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - //// eigenbox should also produce aabb //// - this->eigenbox(v.begin(), v.end()); - // TODO(c.ho) don't care about value.. - this->check(1.0F, 1.0F, r * sqrtf(2.0F), r / sqrtf(2.0F), 4.375F); - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - //// Lfit should produce aabb //// - this->lfit_bounding_box_2d(v.begin(), v.end()); - - this->test_corners( - {this->make(4, -1), this->make(-1, 4), this->make(4, 4), this->make(-1, -1)}, 0.25F); - this->test_orientation(0.0F, 3.0F); -} - -// same as above test, just rotated -TYPED_TEST(BoxTest, Eigenbox2) -{ - std::vector v{this->make(0, 0), this->make(1, 1), - this->make(1, 2), // small fudge to force diagonal - this->make(3, 3), this->make(4, 4), - this->make(1, -1), this->make(1, -2), // small fudge to force diagonal - this->make(3, -3), this->make(4, -4)}; - this->points.insert(this->points.begin(), v.begin(), v.end()); - - const std::vector diag_corners{ - this->make(4, 4), this->make(0, 4), this->make(0, -4), this->make(4, -4)}; - // rotating calipers should produce a aabb - this->minimum_area_bounding_box(); - this->check(2.0F, 0.0F, 8, 4, 32); - this->test_corners(diag_corners); - this->test_orientation(0.0F, 0.001F); - - //// perimeter should also produce aabb //// - this->minimum_perimeter_bounding_box(); - this->check(2.0F, 0.0F, 8, 4, 12); - this->test_corners(diag_corners); - - //// eigenbox should also produce obb //// - this->eigenbox(v.begin(), v.end()); - this->test_orientation(0.0F, 0.001F); - this->test_corners(diag_corners); - //// Lfit should produce obb //// - this->lfit_bounding_box_2d(v.begin(), v.end()); - this->test_corners( - {this->make(3.5, 4.5), this->make(-1, 0), this->make(3.5, -4.5), this->make(8, 0)}, 0.2F); - this->test_orientation(45.0F, 2.0F); -} -// line case for eigenbox -TYPED_TEST(BoxTest, Eigenbox3) -{ - // horizontal line with some noise - std::vector v{this->make(0, 0.00), this->make(1, -0.01), this->make(3, 0.02), - this->make(3, 0.03), this->make(4, -0.04), this->make(-1, 0.01), - this->make(-2, -0.02), this->make(-3, -0.03), this->make(-4, 0.04)}; - this->lfit_bounding_box_2d(v.begin(), v.end()); - this->test_corners( - {this->make(-4, -0.04), this->make(-4, 0.04), this->make(4, -0.04), this->make(4, 0.04)}, 0.2F); - this->test_orientation(0.0F, 1.0F); -} - -// bad case: causes intersection2d to fail -// See https://gitlab.apex.ai/ApexAI/grand_central/issues/2862#note_156875 for more failure cases -TYPED_TEST(BoxTest, IntersectFail) -{ - std::vector vals{ - this->make(-13.9, 0.100006), this->make(-14.1, 0.100006), this->make(-13.9, 0.300003), - this->make(-14.1, 0.300003), this->make(-13.9, 0.5), this->make(-14.1, 0.5), - this->make(-13.9, 0.699997), this->make(-14.1, 0.699997), this->make(-14.3, 0.699997)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); - vals = {this->make(-2.1, 10.1), this->make(-1.89999, 10.1), this->make(-1.89999, 10.3), - this->make(-1.7, 10.3), this->make(-1.5, 10.3), this->make(-1.3, 10.3), - this->make(-0.5, 10.3), this->make(-0.300003, 10.3), this->make(-0.0999908, 10.3), - this->make(0.699997, 10.3), this->make(0.900009, 10.3), this->make(1.3, 10.3), - this->make(1.7, 10.3)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); - vals = {this->make(2.7, -5.5), this->make(2.7, -5.3), this->make(2.7, -5.1), - this->make(2.7, -4.9), this->make(2.7, -4.7), this->make(2.7, -4.5), - this->make(2.7, -4.3), this->make(2.7, -4.1), this->make(2.7, -3.9)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); -} -/// Handle slight floating point underflow case -// Note: raw discriminant checks are disabled because they don't work. I suspect this is due to -// tiny differences in floating point math when using our compile flags -TYPED_TEST(BoxTest, EigUnderflow) -{ - using autoware::common::geometry::bounding_box::details::Covariance2d; - // auto discriminant = [](const Covariance2d cov) -> float32_t { - // // duplicated raw math - // const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F; - // const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy); - // return (tr_2 * tr_2) - det; - // }; - TypeParam u, v; - const Covariance2d c1{0.0300002, 0.0300002, 5.46677e-08, 0U}; - // EXPECT_LT(discriminant(c1), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c1, u, v)); - const Covariance2d c2{0.034286, 0.0342847, 2.12874e-09, 0U}; - // EXPECT_LT(discriminant(c2), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c2, u, v)); - const Covariance2d c3{0.0300002, 0.0300002, -2.84987e-08, 0U}; - // EXPECT_LT(discriminant(c3), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c3, u, v)); - const Covariance2d c4{0.0300004, 0.0300002, 3.84156e-08, 0U}; - // EXPECT_LT(discriminant(c4), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c4, u, v)); - const Covariance2d c5{0.0300014, 0.0300014, -7.45058e-09, 0U}; - // EXPECT_LT(discriminant(c5), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c5, u, v)); - const Covariance2d c6{0.0400004, 0.0400002, 3.28503e-08, 0U}; - // EXPECT_LT(discriminant(c6), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c6, u, v)); -} - -//////////////////////////////////////////////// - -#endif // TEST_BOUNDING_BOX_HPP_ diff --git a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp deleted file mode 100644 index fc2d97c257b95..0000000000000 --- a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp +++ /dev/null @@ -1,260 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef TEST_SPATIAL_HASH_HPP_ -#define TEST_SPATIAL_HASH_HPP_ - -#include "autoware_auto_geometry/spatial_hash.hpp" - -#include - -#include -#include - -using autoware::common::geometry::spatial_hash::Config2d; -using autoware::common::geometry::spatial_hash::Config3d; -using autoware::common::geometry::spatial_hash::SpatialHash; -using autoware::common::geometry::spatial_hash::SpatialHash2d; -using autoware::common::geometry::spatial_hash::SpatialHash3d; -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedSpatialHashTest : public ::testing::Test -{ -public: - TypedSpatialHashTest() : ref(), EPS(0.001F) - { - ref.x = 0.0F; - ref.y = 0.0F; - ref.z = 0.0F; - } - -protected: - template - void add_points( - SpatialHash & hash, const uint32_t points_per_ring, const uint32_t num_rings, - const float32_t dr, const float32_t dx = 0.0F, const float32_t dy = 0.0F) - { - const float32_t dth = 2.0F * 3.14159F / points_per_ring; - - // insert - float32_t r = dr; - for (uint32_t rdx = 0U; rdx < num_rings; ++rdx) { - float32_t th = 0.0F; - for (uint32_t pdx = 0U; pdx < points_per_ring; ++pdx) { - PointT pt; - pt.x = r * cosf(th) + dx; - pt.y = r * sinf(th) + dy; - pt.z = 0.0F; - hash.insert(pt); - th += dth; - } - r += dr; - } - } - PointT ref; - const float32_t EPS; -}; // SpatialHash -// test struct - -// Instantiate tests for given types, add more types here as they are used -using PointTypesSpatialHash = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedSpatialHashTest, PointTypesSpatialHash, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -/////////////////////////////////////////////////////////////// -// TODO(christopher.ho) helper functions to simplify this stuff -/// all points in one bin -TYPED_TEST(TypedSpatialHashTest, OneBin) -{ - using PointT = TypeParam; - const float32_t dr = 1.0F; - Config2d cfg{-10.0F, 10.0F, -10.0F, 10.0F, 3.0F, 1024U}; - SpatialHash2d hash{cfg}; - - // build concentric rings around origin - const uint32_t PTS_PER_RING = 10U; - const uint32_t NUM_RINGS = 1U; - this->add_points(hash, PTS_PER_RING, NUM_RINGS, dr); - - // loop through all points - float r = dr - this->EPS; - for (uint32_t rdx = 0U; rdx < NUM_RINGS + 1U; ++rdx) { - const uint32_t n_pts = rdx * PTS_PER_RING; - const auto & neighbors = hash.near(this->ref, r); - uint32_t points_seen = 0U; - for (const auto & itd : neighbors) { - const PointT & pt = itd; - const float dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y)); - ASSERT_LT(dist, r); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - ASSERT_EQ(points_seen, n_pts); - r += dr; - // Make sure statistics are consistent - EXPECT_EQ(hash.bins_hit(), 9U * (1U + rdx)); - EXPECT_EQ(hash.neighbors_found(), rdx * PTS_PER_RING); - } - // check iterators etc. - uint32_t count = 0U; - for (auto iter = hash.cbegin(); iter != hash.cend(); ++iter) { - // TODO(c.ho) check uniqueness of stuff - ++count; - } - EXPECT_EQ(PTS_PER_RING, count); - hash.clear(); - EXPECT_EQ(hash.size(), 0U); - EXPECT_TRUE(hash.empty()); - count = 0U; - for (auto it : hash) { - // TODO(c.ho) check uniqueness of stuff - (void)it; - ++count; - } - EXPECT_EQ(count, 0U); -} -/// test out of bounds points -TYPED_TEST(TypedSpatialHashTest, Oob) -{ - using PointT = TypeParam; - const float32_t dr = 20.0F; - Config2d cfg{-2.0F, 2.0F, -2.0F, 2.0F, dr + this->EPS, 1024U}; - SpatialHash2d hash{cfg}; - - // build concentric rings around origin - const uint32_t PTS_PER_RING = 12U; - this->add_points(hash, PTS_PER_RING, 1U, dr); - - // loop through all points - float32_t r = dr + this->EPS; - const uint32_t n_pts = PTS_PER_RING; - const auto & neighbors = hash.near(this->ref, r); - uint32_t points_seen = 0U; - for (const auto itd : neighbors) { - const PointT & pt = itd; - const float32_t dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y)); - ASSERT_LT(dist, r); - ASSERT_GT(dist, 10.0F * sqrtf(2.0F)); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - - ASSERT_EQ(points_seen, n_pts); -} -// 3d test case -TYPED_TEST(TypedSpatialHashTest, 3d) -{ - using PointT = TypeParam; - Config3d cfg{-30.0F, 30.0F, -30.0F, 30.0F, -30.0F, 30.0F, 30.0F, 1024U}; - SpatialHash3d hash{cfg}; - EXPECT_TRUE(hash.empty()); - - // build concentric rings around origin - const uint32_t points_per_ring = 32U; - const uint32_t num_rings = 5U; - const float32_t dth = 2.0F * 3.14159F / points_per_ring; - std::vector pts{}; - - // insert - const float32_t r = 10.0F; - float32_t phi = 0.0f; - for (uint32_t rdx = 0U; rdx < num_rings; ++rdx) { - float32_t th = 0.0F; - for (uint32_t pdx = 0U; pdx < points_per_ring; ++pdx) { - PointT pt; - pt.x = r * cosf(th) * cosf(phi); - pt.y = r * sinf(th) * cosf(phi); - pt.z = r * sinf(phi); - pts.push_back(pt); - th += dth; - } - hash.insert(pts.begin(), pts.end()); - pts.clear(); - phi += 1.0f; - } - EXPECT_FALSE(hash.empty()); - EXPECT_EQ(hash.size(), num_rings * points_per_ring); - - // loop through all points - const uint32_t n_pts = num_rings * points_per_ring; - const auto & neighbors = hash.near(this->ref, r + this->EPS); - uint32_t points_seen = 0U; - for (const auto & itd : neighbors) { - const PointT & pt = itd; - const float32_t dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y) + (pt.z * pt.z)); - ASSERT_LT(dist, r + this->EPS); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - - ASSERT_EQ(points_seen, n_pts); - - // check iterators etc. - uint32_t count = 0U; - for (auto iter = hash.cbegin(); iter != hash.cend(); ++iter) { - // TODO(c.ho) check uniqueness of stuff - ++count; - } - EXPECT_EQ(n_pts, count); - hash.clear(); - EXPECT_EQ(hash.size(), 0U); - EXPECT_TRUE(hash.empty()); - count = 0U; - for (auto it : hash) { - // TODO(c.ho) check uniqueness of stuff - (void)it; - ++count; - } - EXPECT_EQ(count, 0U); -} - -/// edge cases -TEST(SpatialHashConfig, BadCases) -{ - // negative side length - EXPECT_THROW(Config2d({-30.0F, 30.0F, -30.0F, 30.0F, -1.0F, 1024U}), std::domain_error); - // min_x >= max_x - EXPECT_THROW(Config2d({31.0F, 30.0F, -30.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - // min_y >= max_y - EXPECT_THROW(Config2d({-30.0F, 30.0F, 30.1F, 30.0F, 1.0F, 1024U}), std::domain_error); - // min_z >= max_z - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -30.0F, 30.0F, 31.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - // floating point limit - constexpr float32_t max_float = std::numeric_limits::max(); - EXPECT_THROW(Config2d({-max_float, max_float, -30.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -max_float, max_float, -30.0F, 30.0F, 1.0F, 1024U}), - std::domain_error); - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -30.0F, 30.0F, -max_float, max_float, 1.0F, 1024U}), - std::domain_error); - // y would overflow - // constexpr float32_t big_float = - // static_cast(std::numeric_limits::max() / 4UL); - // EXPECT_THROW(Config({-big_float, big_float, -big_float, big_float, 0.001F, 1024U}), - // std::domain_error); - // z would overflow - // EXPECT_THROW( - // Config3d({-30.0F, 30.0F, -99999.0F, 99999.0F, -99999.0F, 99999.0F, 0.001F, 1024U}), - // std::domain_error); - // TODO(c.ho) re-enable test when we can actually check unsigned integer multiplication overflow -} -#endif // TEST_SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/test/src/lookup_table.cpp b/common/autoware_auto_geometry/test/src/lookup_table.cpp deleted file mode 100644 index 81865656c55b5..0000000000000 --- a/common/autoware_auto_geometry/test/src/lookup_table.cpp +++ /dev/null @@ -1,166 +0,0 @@ -// Copyright 2017-2020 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/lookup_table.hpp" - -#include - -#include - -#include -#include - -using autoware::common::helper_functions::interpolate; -using autoware::common::helper_functions::lookup_1d; -using autoware::common::helper_functions::LookupTable1D; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class BadCase : public ::testing::Test -{ -}; - -using BadTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(BadCase, BadTypes, ); - -// Empty domain/range -TYPED_TEST(BadCase, Empty) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({}, {}, T{}), std::domain_error); -} - -// Unequal domain/range -TYPED_TEST(BadCase, UnequalDomain) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({T{1}, T{2}}, {T{1}}, T{}), std::domain_error); -} - -// Not sorted domain -TYPED_TEST(BadCase, DomainNotSorted) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({T{2}, T{1}}, {T{1}, T{2}}, T{}), std::domain_error); -} - -//////////////////////////////////////////////////////////////////////////////// - -template -class SanityCheck : public ::testing::Test -{ -public: - using T = Type; - - void SetUp() override - { - domain_ = std::vector{T{1}, T{3}, T{5}}; - range_ = std::vector{T{2}, T{4}, T{0}}; - ASSERT_NO_THROW(table_ = std::make_unique>(domain_, range_)); - } - - bool not_in_domain(const T bad_value) const noexcept - { - return std::find(domain_.begin(), domain_.end(), bad_value) == domain_.end(); - } - - void check(const T expected, const T actual) const noexcept - { - if (std::is_floating_point::value) { - EXPECT_FLOAT_EQ(actual, expected); - } else { - EXPECT_EQ(actual, expected); - } - } - -protected: - std::vector domain_{}; - std::vector range_{}; - std::unique_ptr> table_{}; -}; - -using NormalTypes = ::testing::Types; -TYPED_TEST_SUITE(SanityCheck, NormalTypes, ); - -TYPED_TEST(SanityCheck, Exact) -{ - const auto x = this->domain_[1U]; - const auto result = this->table_->lookup(x); - ASSERT_FALSE(this->not_in_domain(x)); - this->check(result, this->range_[1U]); -} - -TYPED_TEST(SanityCheck, Interpolation) -{ - const auto x = TypeParam{2}; - // Assert x is not identically in domain_ - ASSERT_TRUE(this->not_in_domain(x)); - const auto result = this->table_->lookup(x); - this->check(result, TypeParam{3}); -} - -TYPED_TEST(SanityCheck, AboveRange) -{ - const auto x = TypeParam{999999}; - ASSERT_GT(x, this->domain_.back()); // domain is known to be sorted - const auto result = this->table_->lookup(x); - this->check(result, this->range_.back()); -} - -TYPED_TEST(SanityCheck, BelowRange) -{ - const auto x = TypeParam{0}; - ASSERT_LT(x, this->domain_.front()); // domain is known to be sorted - const auto result = this->table_->lookup(x); - this->check(result, this->range_.front()); -} - -TEST(LookupTableHelpers, Interpolate) -{ - { - const auto scaling = 0.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 2.0f); - } - - { - const auto scaling = 1.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 1.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.5f); - } - - { - const auto scaling = -1.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 2.0f); - } - - { - const auto scaling = 2.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 1.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.5f); - } - - { - const auto scaling = 0.75f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.75f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.125f); - } -} - -// TODO(c.ho) check with more interesting functions diff --git a/common/autoware_auto_geometry/test/src/test_area.cpp b/common/autoware_auto_geometry/test/src/test_area.cpp deleted file mode 100644 index bc9c28682ed44..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_area.cpp +++ /dev/null @@ -1,132 +0,0 @@ -// Copyright 2021 Apex.AI, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -#include - -#include -#include -#include - -template -class AreaTest : public ::testing::Test -{ -protected: - DataStructure data_{}; - using Point = typename DataStructure::value_type; - using Real = decltype(autoware::common::geometry::point_adapter::x_(std::declval())); - - auto area() { return autoware::common::geometry::area_checked_2d(data_.begin(), data_.end()); } - - void add_point(Real x, Real y) - { - namespace pa = autoware::common::geometry::point_adapter; - Point p{}; - pa::xr_(p) = x; - pa::yr_(p) = y; - (void)data_.insert(data_.end(), p); - } -}; - -// Data structures to test... -template -using TestTypes_ = ::testing::Types..., std::list...>; -// ... and point types to test -using TestTypes = TestTypes_; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(AreaTest, TestTypes, ); - -// The empty set has zero area -TYPED_TEST(AreaTest, DegenerateZero) -{ - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// An individual point has zero area -TYPED_TEST(AreaTest, DegenerateOne) -{ - this->add_point(0.0, 0.0); - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// An line segment has zero area -TYPED_TEST(AreaTest, DegenerateTwo) -{ - this->add_point(1.0, -1.0); - this->add_point(-3.0, 2.0); - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// Simple triangle -TYPED_TEST(AreaTest, Triangle) -{ - this->add_point(1.0, 0.0); - this->add_point(3.0, 0.0); // 2.0 wide - this->add_point(2.0, 2.0); // 2.0 tall - EXPECT_FLOAT_EQ(2.0, this->area()); // A = (1/2) * b * h -} - -// Rectangle is easy to do computational -TYPED_TEST(AreaTest, Rectangle) -{ - this->add_point(-5.0, -5.0); - this->add_point(-2.0, -5.0); // L = 3 - this->add_point(-2.0, -1.0); // H = 4 - this->add_point(-5.0, -1.0); - EXPECT_FLOAT_EQ(12.0, this->area()); // A = b * h -} - -// Parallelogram is slightly less trivial than a rectangle -TYPED_TEST(AreaTest, Parallelogram) -{ - this->add_point(-5.0, 1.0); - this->add_point(-2.0, 1.0); // L = 3 - this->add_point(-1.0, 3.0); // H = 2 - this->add_point(-4.0, 3.0); - EXPECT_FLOAT_EQ(6.0, this->area()); // A = b * h -} - -// Octagon is analytical and reasonably easy to build -TYPED_TEST(AreaTest, Octagon) -{ - const auto sq2 = std::sqrt(2.0); - const auto a = 1.0; - const auto a2 = a / 2.0; - const auto b = (a + sq2) / 2.0; - this->add_point(-a2, -b); - this->add_point(a2, -b); - this->add_point(b, -a2); - this->add_point(b, a2); - this->add_point(a2, b); - this->add_point(-a2, b); - this->add_point(-b, a2); - this->add_point(-b, -a2); - const auto expect = (2.0 * (1.0 + sq2)) * (a * a); - EXPECT_FLOAT_EQ(expect, this->area()); // A = b * h -} - -// Bad case -TYPED_TEST(AreaTest, NotCcw) -{ - this->add_point(0.0, 0.0); - this->add_point(1.0, 1.0); - this->add_point(1.0, 0.0); - this->add_point(2.0, 1.0); - EXPECT_THROW(this->area(), std::domain_error); -} diff --git a/common/autoware_auto_geometry/test/src/test_common_2d.cpp b/common/autoware_auto_geometry/test/src/test_common_2d.cpp deleted file mode 100644 index baf6967edd47e..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_common_2d.cpp +++ /dev/null @@ -1,197 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/common_2d.hpp" - -#include -#include - -#include - -#include -#include - -using autoware::common::geometry::point_adapter::xr_; -using autoware::common::geometry::point_adapter::yr_; - -// Helper function for adding new points -template -T make_points(const float x, const float y) -{ - T ret; - xr_(ret) = x; - yr_(ret) = y; - return ret; -} - -// PointTypes to be tested -using PointTypes = - ::testing::Types; - -// Wrapper function for stubbing output of -// autoware::common::geometry::check_point_position_to_line_2d -template -int point_position_checker(const T & p1, const T & p2, const T & q) -{ - auto result = autoware::common::geometry::check_point_position_to_line_2d(p1, p2, q); - if (result > 0.0F) { - return 1; - } else if (result < 0.0F) { - return -1; - } - return result; -} - -// Templated struct defining the function parameters for -// autoware::common::geometry::check_point_position_to_line_2d and input_output vector for -// assertion of return values from the function -template -struct PointPositionToLine : public ::testing::Test -{ - struct Parameters - { - T p1; - T p2; - T q; - }; - static std::vector> input_output; -}; - -TYPED_TEST_SUITE_P(PointPositionToLine); - -template -std::vector::Parameters, int>> - PointPositionToLine::input_output{ - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(1.0F, 5.0F)}, -1}, - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(-1.0F, 0.5F)}, 1}, - // Check point on the line - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(-2.0F, 2.0F)}, 0}, - }; - -TYPED_TEST_P(PointPositionToLine, PointPositionToLineTest) -{ - for (size_t i = 0; i < PointPositionToLine::input_output.size(); ++i) { - const auto & input = PointPositionToLine::input_output[i].first; - EXPECT_EQ( - point_position_checker(input.p1, input.p2, input.q), - PointPositionToLine::input_output[i].second) - << "Index " << i; - } -} - -REGISTER_TYPED_TEST_SUITE_P(PointPositionToLine, PointPositionToLineTest); -// cppcheck-suppress syntaxError -INSTANTIATE_TYPED_TEST_SUITE_P(Test, PointPositionToLine, PointTypes, ); - -///////////////////////////////////////////////////////////////////////////////////// - -// Templated struct defining the function parameters for -// autoware::common::geometry::is_point_inside_polygon_2d and input_output vector for -// assertion of return values from the function -template -struct InsidePolygon : public ::testing::Test -{ - struct Parameters - { - std::vector polygon; - T q; - }; - static std::vector> input_output; -}; - -TYPED_TEST_SUITE_P(InsidePolygon); - -template -std::vector::Parameters, bool>> InsidePolygon::input_output{ - // point inside the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.F, 0.5F)}, - true}, - // point below the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 0.F)}, - false}, - // point above the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 1.75F)}, - false}, - // point on the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 0.5F)}, - true}, -}; - -TYPED_TEST_P(InsidePolygon, InsidePolygonTest) -{ - for (size_t i = 0; i < InsidePolygon::input_output.size(); ++i) { - const auto & input = InsidePolygon::input_output[i].first; - EXPECT_EQ( - autoware::common::geometry::is_point_inside_polygon_2d( - input.polygon.begin(), input.polygon.end(), input.q), - InsidePolygon::input_output[i].second) - << "Index " << i; - } -} - -REGISTER_TYPED_TEST_SUITE_P(InsidePolygon, InsidePolygonTest); -// cppcheck-suppress syntaxError -INSTANTIATE_TYPED_TEST_SUITE_P(Test, InsidePolygon, PointTypes, ); - -TEST(ordered_check, basic) -{ - // CW - std::vector points_list = { - make_points(8.0, 4.0), - make_points(9.0, 1.0), - make_points(3.0, 2.0), - make_points(2.0, 5.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // CCW - points_list = { - make_points(2.0, 5.0), - make_points(3.0, 2.0), - make_points(9.0, 1.0), - make_points(8.0, 4.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Unordered - points_list = { - make_points(2.0, 5.0), - make_points(3.0, 2.0), - make_points(8.0, 4.0), - make_points(9.0, 1.0)}; - EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Unordered - points_list = { - make_points(0.0, 0.0), - make_points(1.0, 1.0), - make_points(1.0, 0.0), - make_points(2.0, 1.0)}; - EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Collinearity - points_list = { - make_points(2.0, 2.0), - make_points(4.0, 4.0), - make_points(6.0, 6.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); -} diff --git a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp deleted file mode 100644 index 8b9bbce36c428..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp +++ /dev/null @@ -1,372 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" - -#include - -#include - -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedConvexHullTest : public ::testing::Test -{ -protected: - std::list list; - - typename std::list::const_iterator convex_hull() - { - const auto ret = autoware::common::geometry::convex_hull(list); - return ret; - } - - void check_hull( - const typename std::list::const_iterator last, const std::vector & expect, - const bool8_t strict = true) - { - uint32_t items = 0U; - for (auto & pt : expect) { - bool8_t found = false; - auto it = list.begin(); - while (it != last) { - constexpr float32_t TOL = 1.0E-6F; - if ( - fabsf(pt.x - it->x) <= TOL && fabsf(pt.y - it->y) <= TOL && - (fabsf(pt.z - it->z) <= TOL || !strict)) // TODO(@estive): z if only strict - { - found = true; - break; - } - ++it; - } - ASSERT_TRUE(found) << items; - ++items; - } - if (strict) { - ASSERT_EQ(items, expect.size()); - } - } - - PointT make(const float32_t x, const float32_t y, const float32_t z) - { - PointT ret; - ret.x = x; - ret.y = y; - ret.z = z; - return ret; - } -}; // class convex_hull_test - -// Instantiate tests for given types, add more types here as they are used -using PointTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedConvexHullTest, PointTypes, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -////////////////////////////////////////// - -/* - 3 - 2 -1 -*/ -TYPED_TEST(TypedConvexHullTest, Triangle) -{ - std::vector expect({this->make(1, 0, 0), this->make(3, 1, 0), this->make(2, 2, 0)}); - this->list.insert(this->list.begin(), expect.begin(), expect.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(this->list.size(), 3U); - // check order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, 3); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->x, 2); - ++it; // node 3 - ASSERT_EQ(it, last); -} -/* - 2 1 - -4 - 3 -*/ -// test that things get reordered to ccw -TYPED_TEST(TypedConvexHullTest, Quadrilateral) -{ - std::vector expect( - {this->make(-1, -1, 1), this->make(-5, -1, 2), this->make(-2, -6, 3), this->make(-6, -5, 4)}); - this->list.insert(this->list.begin(), expect.begin(), expect.end()); - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(this->list.size(), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, -6); - ++it; // node 4 - ASSERT_FLOAT_EQ(it->x, -2); - ++it; // node 3 - ASSERT_FLOAT_EQ(it->x, -1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, -5); - ++it; // node 2 - ASSERT_EQ(it, last); -} - -// test that things get reordered to ccw -TYPED_TEST(TypedConvexHullTest, QuadHull) -{ - std::vector data( - {this->make(1, 1, 1), this->make(5, 1, 2), this->make(2, 6, 3), this->make(3, 3, 4), - this->make(6, 5, 5)}); - std::vector expect{{data[0], data[1], data[2], data[4]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, 5); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->x, 6); - ++it; // node 4 - ASSERT_FLOAT_EQ(it->x, 2); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -// a ring plus a bunch of random stuff in the middle -TYPED_TEST(TypedConvexHullTest, Hull) -{ - const uint32_t HULL_SIZE = 13U; - const uint32_t FUZZ_SIZE = 50U; - const float32_t dth = 1.133729384F; // some weird irrational(ish) number - const float32_t r_hull = 20.0F; - const float32_t r_fuzz = 10.0F; - ASSERT_LT(r_fuzz, r_hull); - - std::vector hull; - - uint32_t hull_pts = 0U; - float32_t th = 0.0F; - // hull part 1 - for (uint32_t idx = 0U; idx < HULL_SIZE / 3U; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++hull_pts; - } - - // fuzz part 1 - uint32_t fuzz_pts = 0U; - for (uint32_t idx = 0U; idx < FUZZ_SIZE / 2U; ++idx) { - const auto pt = this->make(r_fuzz * cosf(th), r_fuzz * sinf(th), th); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++fuzz_pts; - } - - // hull part 2 - for (uint32_t idx = 0U; idx < HULL_SIZE / 3U; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++hull_pts; - } - - // fuzz part 2 - for (uint32_t idx = 0U; idx < FUZZ_SIZE - fuzz_pts; ++idx) { - const auto pt = this->make(r_fuzz * cosf(th), r_fuzz * sinf(th), th); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - } - - // hull part 3 - for (uint32_t idx = 0U; idx < HULL_SIZE - hull_pts; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - } - - const auto last = this->convex_hull(); - - this->check_hull(last, hull); - ASSERT_EQ(std::distance(this->list.cbegin(), last), HULL_SIZE); -} - -TYPED_TEST(TypedConvexHullTest, Collinear) -{ - std::vector data( - {this->make(0, 0, 1), this->make(1, 0, 2), this->make(2, 0, 3), this->make(0, 2, 4), - this->make(1, 2, 8), this->make(2, 2, 7), this->make(1, 0, 6), this->make(1, 2, 5), - this->make(1, 1, 0)}); - const std::vector expect{{data[0], data[2], data[3], data[5]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->z, 3); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->z, 7); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->z, 4); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -// degenerate cases -TYPED_TEST(TypedConvexHullTest, OverlappingPoints) -{ - std::vector data( - {this->make(3, -1, 1), this->make(4, -2, 2), this->make(5, -7, 3), this->make(4, -2, 4), - this->make(5, -7, 8), this->make(3, -1, 7), this->make(5, -7, 6), this->make(4, -2, 5), - this->make(3, -1, 0)}); - const std::vector expect{{data[0], data[1], data[2]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 3U); - this->check_hull(last, expect, false); -} - -TYPED_TEST(TypedConvexHullTest, Line) -{ - std::vector data( - {this->make(-3, 3, 1), this->make(-2, 2, 2), this->make(-1, 1, 3), this->make(-8, 8, 4), - this->make(-6, 6, 8), this->make(-4, 4, 7), this->make(-10, 10, 6), this->make(-12, 12, 5), - this->make(-11, 11, 0)}); - const std::vector expect{{data[2], data[7]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 2U); - this->check_hull(last, expect, false); - - // check for order: this part is a little loose - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 5); - ++it; // node 8 - ASSERT_FLOAT_EQ(it->z, 3); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -/* -1 - 4 - - 3 - 2 -*/ -TYPED_TEST(TypedConvexHullTest, LowerHull) -{ - const std::vector data({ - this->make(1, 3, 1), - this->make(2, -2, 2), - this->make(3, -1, 3), - this->make(4, 1, 4), - }); - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - this->check_hull(last, data); - - // check for order: this part is a little loose - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; - ASSERT_FLOAT_EQ(it->z, 2); - ++it; - ASSERT_FLOAT_EQ(it->z, 3); - ++it; - ASSERT_FLOAT_EQ(it->z, 4); - ++it; - ASSERT_EQ(it, last); -} - -// Ensure the leftmost item is properly shuffled -/* - 5 -1 6 - 2 4 - 3 -*/ -TYPED_TEST(TypedConvexHullTest, Root) -{ - const std::vector data({ - this->make(0, 0, 1), - this->make(1, -1, 2), - this->make(3, -2, 3), - this->make(4, 0, 4), - this->make(3, 1, 5), - this->make(1, 0, 6), - }); - const std::vector expect{{data[0], data[1], data[2], data[3], data[4]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 5); - this->check_hull(last, expect); - - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; - ASSERT_FLOAT_EQ(it->z, 2); - ++it; - ASSERT_FLOAT_EQ(it->z, 3); - ++it; - ASSERT_FLOAT_EQ(it->z, 4); - ++it; - ASSERT_FLOAT_EQ(it->z, 5); - ++it; - ASSERT_EQ(it, last); - EXPECT_NE(last, this->list.cend()); - EXPECT_EQ(last->z, 6); -} - -// TODO(c.ho) random input, fuzzing, stress tests diff --git a/common/autoware_auto_geometry/test/src/test_geometry.cpp b/common/autoware_auto_geometry/test/src/test_geometry.cpp deleted file mode 100644 index 0a7dd288d0d79..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_geometry.cpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2018 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "gtest/gtest.h" -#include "test_bounding_box.hpp" -#include "test_spatial_hash.hpp" - -int32_t main(int32_t argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp deleted file mode 100644 index 9477a83a488ed..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp +++ /dev/null @@ -1,182 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland. 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" -#include "autoware_auto_geometry/hull_pockets.hpp" - -#include - -#include - -#include -#include -#include -#include - -using autoware::common::geometry::point_adapter::x_; -using autoware::common::geometry::point_adapter::y_; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedHullPocketsTest : public ::testing::Test -{ -protected: - PointT make(const float32_t x, const float32_t y, const float32_t z) - { - PointT ret; - ret.x = x; - ret.y = y; - ret.z = z; - return ret; - } -}; // class TypedHullPocketsTest - -using PointTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedHullPocketsTest, PointTypes, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -// Inner test function, shared among all the tests -template -typename std::vector::value_type>> -compute_hull_and_pockets(const Iter polygon_start, const Iter polygon_end) -{ - auto convex_hull_list = - std::list::value_type>{polygon_start, polygon_end}; - const auto cvx_hull_end = autoware::common::geometry::convex_hull(convex_hull_list); - - const typename decltype(convex_hull_list)::const_iterator list_beginning = - convex_hull_list.begin(); - const auto pockets = autoware::common::geometry::hull_pockets( - polygon_start, polygon_end, list_beginning, cvx_hull_end); - - return pockets; -} - -// Test for a triangle - any triangle should really not result in any pockets. -TYPED_TEST(TypedHullPocketsTest, Triangle) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(1, 1, 0)}; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - ASSERT_EQ(pockets.size(), 0u); -} - -// Test for the use case: -// +--------------------+ -// | | -// | | -// | | -// | | -// | | -// | | -// +--------------------+ -// This should not result in any pockets. -TYPED_TEST(TypedHullPocketsTest, Box) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(2, 1, 0), this->make(0, 1, 0)}; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - ASSERT_EQ(pockets.size(), 0u); -} - -// Test for the use case: -// +-----+ +-----+ -// / | | | -// / | | | -// + | | + -// | | | | -// | | | | -// | -------------- | -// | | -// | | -// | | -// | | -// +------------------------------+ -// This should come up with a single box on the top left. -TYPED_TEST(TypedHullPocketsTest, UShape) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(5, 0, 0), this->make(5, 4.5, 0), this->make(4, 5, 0), - this->make(4, 2, 0), this->make(2, 2, 0), this->make(2, 5, 0), this->make(0, 4.5, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 1u); - ASSERT_EQ(pockets[0].size(), 4u); - ASSERT_FLOAT_EQ(x_(pockets[0][0]), 4.0); - ASSERT_FLOAT_EQ(y_(pockets[0][0]), 5.0); - ASSERT_FLOAT_EQ(x_(pockets[0][1]), 4.0); - ASSERT_FLOAT_EQ(y_(pockets[0][1]), 2.0); -} - -// Test for the use case: -// +------+ -// | | -// | | -// | | -// +------------------+ +------+ -// | | -// | | -// | | -// +--------------------------------+ -// -// This should come up with two pockets, a triangle on the top left and one on the -// top right. -TYPED_TEST(TypedHullPocketsTest, TypicalGap) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(10, 0, 0), this->make(10, 2, 0), this->make(8, 2, 0), - this->make(8, 4, 0), this->make(6, 4, 0), this->make(6, 2, 0), this->make(0, 2, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 2u); - ASSERT_EQ(pockets[0].size(), 3u); - ASSERT_EQ(pockets[1].size(), 3u); - // TODO(s.me) check for correct pocket positioning -} - -// Test for the use case: -// -// +-----------------+ -// | | -// | | -// + | -// / | -// / | -// +-----------------+ -// -// This should come up with one pocket, in particular a pocket that contains -// the segment of the final to the first point. -TYPED_TEST(TypedHullPocketsTest, EndsInPocket) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(2, 2, 0), - this->make(0, 2, 0), this->make(0.1, 1, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 1u); - ASSERT_EQ(pockets[0].size(), 3u); - // TODO(s.me) check for correct pocket positioning -} diff --git a/common/autoware_auto_geometry/test/src/test_intersection.cpp b/common/autoware_auto_geometry/test/src/test_intersection.cpp deleted file mode 100644 index 1036c69573c49..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_intersection.cpp +++ /dev/null @@ -1,155 +0,0 @@ -// Copyright 2021 Apex.AI, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" -#include "autoware_auto_geometry/intersection.hpp" - -#include - -#include - -struct TestPoint -{ - autoware::common::types::float32_t x; - autoware::common::types::float32_t y; -}; - -struct IntersectionTestParams -{ - std::list polygon1_pts; - std::list polygon2_pts; - std::list expected_intersection_pts; -}; - -void order_ccw(std::list & points) -{ - const auto end_it = autoware::common::geometry::convex_hull(points); - ASSERT_EQ(end_it, points.end()); // Points should have represent a shape -} - -class IntersectionTest : public ::testing::TestWithParam -{ -}; - -TEST_P(IntersectionTest, Basic) -{ - const auto get_ordered_polygon = [](auto polygon) { - order_ccw(polygon); - return polygon; - }; - - const auto polygon1 = get_ordered_polygon(GetParam().polygon1_pts); - const auto polygon2 = get_ordered_polygon(GetParam().polygon2_pts); - const auto expected_intersection = get_ordered_polygon(GetParam().expected_intersection_pts); - - const auto result = autoware::common::geometry::convex_polygon_intersection2d(polygon1, polygon2); - - ASSERT_EQ(result.size(), expected_intersection.size()); - auto expected_shape_it = expected_intersection.begin(); - for (auto result_it = result.begin(); result_it != result.end(); ++result_it) { - EXPECT_FLOAT_EQ(result_it->x, expected_shape_it->x); - EXPECT_FLOAT_EQ(result_it->y, expected_shape_it->y); - ++expected_shape_it; - } -} - -INSTANTIATE_TEST_SUITE_P( - Basic, IntersectionTest, - ::testing::Values( - IntersectionTestParams{{}, {}, {}}, - IntersectionTestParams{// Partial intersection - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {15.0F, 5.0F}, {5.0F, 15.0F}, {15.0F, 15.0F}}, - {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}}, - // Full intersection with overlapping edges - // TODO(yunus.caliskan): enable after #1231 - // IntersectionTestParams{ - // {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - // {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}, - // {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}, - // }, - IntersectionTestParams{ - // Fully contained - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}}, - }, - IntersectionTestParams{ - // Fully contained triangle - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}}, - }, - IntersectionTestParams{// Triangle rectangle intersection. - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 1.0F}, {5.0F, 9.0F}, {15.0F, 5.0F}}, - {{5.0F, 1.0F}, {5.0F, 9.0F}, {10.0F, 3.0F}, {10.0F, 7.0F}}}, - IntersectionTestParams{// No intersection - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{15.0F, 15.0F}, {20.0F, 15.0F}, {15.0F, 20.0F}, {20.0F, 20.0F}}, - {}} // cppcheck-suppress syntaxError - )); - -TEST(PolygonPointTest, Basic) -{ - GTEST_SKIP(); // TODO(yunus.caliskan): enable after #1231 - std::list polygon{{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}; - order_ccw(polygon); - EXPECT_FALSE(autoware::common::geometry::is_point_inside_polygon_2d( - polygon.begin(), polygon.end(), TestPoint{0.0F, 10.0F})); -} - -// IoU of two intersecting shapes: a pentagon and a square. The test includes pen and paper -// computations for the intermediate steps as assertions. -TEST(IoUTest, PentagonRectangleIntersection) -{ - std::list polygon1{ - {0.0F, 3.0F}, {3.0F, 4.0F}, {6.0F, 3.0F}, {4.0F, 1.0F}, {2.0F, 1.0F}}; - std::list polygon2{{3.0F, 0.0F}, {3.0F, 2.0F}, {5.0F, 2.0F}, {5.0F, 0.0F}}; - - order_ccw(polygon1); - order_ccw(polygon2); - - const auto intersection = - autoware::common::geometry::convex_polygon_intersection2d(polygon1, polygon2); - const auto expected_intersection_area = - autoware::common::geometry::area_2d(intersection.begin(), intersection.end()); - ASSERT_FLOAT_EQ(expected_intersection_area, 1.5F); // Pen & paper proof. - - const auto expected_union_area = - autoware::common::geometry::area_2d(polygon1.begin(), polygon1.end()) + - autoware::common::geometry::area_2d(polygon2.begin(), polygon2.end()) - - expected_intersection_area; - ASSERT_FLOAT_EQ(expected_union_area, (11.0F + 4.0F - 1.5F)); // Pen & paper proof. - - const auto computed_iou = - autoware::common::geometry::convex_intersection_over_union_2d(polygon1, polygon2); - - EXPECT_FLOAT_EQ(computed_iou, expected_intersection_area / expected_union_area); -} - -// IoU of two non-intersecting rectangles. -TEST(IoUTest, NoIntersection) -{ - std::list polygon1{{0.0F, 0.0F}, {0.0F, 1.0F}, {1.0F, 1.0F}, {1.0F, 0.0F}}; - std::list polygon2{{3.0F, 0.0F}, {3.0F, 2.0F}, {5.0F, 2.0F}, {5.0F, 0.0F}}; - - order_ccw(polygon1); - order_ccw(polygon2); - - EXPECT_FLOAT_EQ( - autoware::common::geometry::convex_intersection_over_union_2d(polygon1, polygon2), 0.0F); -} diff --git a/common/autoware_auto_geometry/test/src/test_interval.cpp b/common/autoware_auto_geometry/test/src/test_interval.cpp deleted file mode 100644 index 266ab123f5203..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_interval.cpp +++ /dev/null @@ -1,259 +0,0 @@ -// Copyright 2020 Mapless AI, Inc. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include -#include -#include - -using autoware::common::geometry::Interval; -using autoware::common::geometry::Interval_d; -using autoware::common::geometry::Interval_f; - -namespace -{ -const auto Inf = std::numeric_limits::infinity(); -const auto Min = std::numeric_limits::lowest(); -const auto Max = std::numeric_limits::max(); -const auto NaN = std::numeric_limits::quiet_NaN(); -const auto epsilon = 1e-5; -} // namespace - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, AbsEq) -{ - const auto i1 = Interval_d(-1.0, 1.0); - const auto i2 = Interval_d(-1.0 + 0.5 * epsilon, 1.0 + 0.5 * epsilon); - const auto shift = (2.0 * epsilon); - const auto i3 = Interval_d(-1.0 + shift, 1.0 + shift); - const auto i_empty = Interval_d(); - - EXPECT_TRUE(Interval_d::abs_eq(i1, i1, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i1, i2, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i2, i1, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i1, i3, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i3, i1, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i1, i_empty, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i_empty, i1, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i_empty, i_empty, epsilon)); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, IsSubsetEq) -{ - EXPECT_TRUE(Interval_d::is_subset_eq(Interval_d(-0.5, 0.5), Interval_d(-1.0, 1.0))); - EXPECT_TRUE(Interval_d::is_subset_eq(Interval_d(3.2, 4.2), Interval_d(3.2, 4.2))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(-3.0, -1.0), Interval_d(1.0, 3.0))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(1.0, 3.0), Interval_d(2.0, 4.0))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(), Interval_d())); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ClampTo) -{ - const auto i = Interval_d(-1.0, 1.0); - { - const auto val = 0.0; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, val); - } - - { - const auto val = -3.4; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, Interval_d::min(i)); - } - - { - const auto val = 2.7; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, Interval_d::max(i)); - } - - const auto val = 1.0; - const auto empty_interval = Interval_d(); - const auto projected = Interval_d::clamp_to(empty_interval, val); - EXPECT_TRUE(std::isnan(projected)); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Comparisons) -{ - { - const auto i1 = Interval_d(0.25, 1); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(-0.25, 0.5); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 0.5); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 1); - const auto i2 = Interval_d(0, 1); - EXPECT_TRUE((i1 == i2)); - EXPECT_FALSE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 1); - const auto i2 = Interval_d(); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(); - EXPECT_TRUE((i1 == i2)); - EXPECT_FALSE((i1 != i2)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Contains) -{ - { - const auto i = Interval_d(); - EXPECT_FALSE(Interval_d::contains(i, 0.0)); - } - - { - const auto i = Interval_d(-1.0, 1.0); - EXPECT_TRUE(Interval_d::contains(i, 0.0)); - EXPECT_FALSE(Interval_d::contains(i, 2.0)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Empty) -{ - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(); - const auto i3 = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i3)); - } - - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(0.0, 1.0); - const auto i3 = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i3)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ZeroMeasure) -{ - { - const auto i = Interval_d(0, 1); - EXPECT_FALSE(Interval_d::zero_measure(i)); - } - - { - const auto i = Interval_d(); - EXPECT_FALSE(Interval_d::zero_measure(i)); - } - - { - const auto i = Interval_d(2, 2); - EXPECT_TRUE(Interval_d::zero_measure(i)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, IntersectionMeasure) -{ - { - const auto i1 = Interval_d(-1.0, 1.0); - const auto i2 = Interval_d(-0.5, 1.5); - const auto i = Interval_d::intersect(i1, i2); - EXPECT_EQ(Interval_d::min(i), -0.5); - EXPECT_EQ(Interval_d::max(i), 1.0); - EXPECT_NEAR(Interval_d::measure(i), 1.5, epsilon); - } - - { - const auto i1 = Interval_d(-2.0, -1.0); - const auto i2 = Interval_d(1.0, 2.0); - const auto i = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i)); - EXPECT_TRUE(std::isnan(Interval_d::min(i))); - EXPECT_TRUE(std::isnan(Interval_d::max(i))); - EXPECT_TRUE(std::isnan(Interval_d::measure(i))); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ConstructionMeasure) -{ - { - const auto i = Interval_d(); - EXPECT_TRUE(std::isnan(Interval_d::min(i))); - EXPECT_TRUE(std::isnan(Interval_d::max(i))); - EXPECT_TRUE(std::isnan(Interval_d::measure(i))); - } - - { - const auto i = Interval_d(-1.0, 1.0); - EXPECT_EQ(Interval_d::min(i), -1.0); - EXPECT_EQ(Interval_d::max(i), 1.0); - EXPECT_NEAR(Interval_d::measure(i), 2.0, epsilon); - } - - { - const auto i = Interval_d(0.0, 0.0); - EXPECT_EQ(Interval_d::min(i), 0.0); - EXPECT_EQ(Interval_d::max(i), 0.0); - EXPECT_FALSE(Interval_d::empty(i)); - EXPECT_EQ(Interval_d::measure(i), 0.0); - } - - { - EXPECT_THROW({ Interval_d(1.0, -1.0); }, std::runtime_error); - } -} - -//------------------------------------------------------------------------------ diff --git a/perception/traffic_light_occlusion_predictor/package.xml b/perception/traffic_light_occlusion_predictor/package.xml index 49a67d68057d9..daf14ba4147c2 100644 --- a/perception/traffic_light_occlusion_predictor/package.xml +++ b/perception/traffic_light_occlusion_predictor/package.xml @@ -12,7 +12,6 @@ autoware_cmake - autoware_auto_geometry autoware_auto_mapping_msgs geometry_msgs image_geometry