From 9f921ec2aa757536e01b143fb763a141180b6b95 Mon Sep 17 00:00:00 2001 From: "Schleicher Matthias (XC-AS/EPE5)" Date: Tue, 30 Apr 2024 09:26:40 +0200 Subject: [PATCH] plugins/frustum_culling: Add frustum culling as a cloe plugin. --- conanfile.py | 1 + models/CMakeLists.txt | 1 + .../include/cloe/utility/frustum_culling.hpp | 151 +++++++ .../src/cloe/utility/frustum_culling_test.cpp | 240 ++++++++++ plugins/clothoid_fit/src/clothoid_fit.hpp | 13 + plugins/clothoid_fit/src/clothoid_test.cpp | 44 ++ plugins/frustum_culling/.vscode/launch.json | 27 ++ plugins/frustum_culling/CMakeLists.txt | 50 +++ plugins/frustum_culling/Makefile | 1 + plugins/frustum_culling/README.md | 37 ++ plugins/frustum_culling/conanfile.py | 85 ++++ .../src/frustum_culling_conf.hpp | 103 +++++ .../src/frustum_culling_lanes.cpp | 132 ++++++ .../src/frustum_culling_objects.cpp | 152 +++++++ .../src/frustum_culling_test.cpp | 413 ++++++++++++++++++ 15 files changed, 1450 insertions(+) create mode 100644 models/include/cloe/utility/frustum_culling.hpp create mode 100644 models/src/cloe/utility/frustum_culling_test.cpp create mode 100644 plugins/frustum_culling/.vscode/launch.json create mode 100644 plugins/frustum_culling/CMakeLists.txt create mode 100644 plugins/frustum_culling/Makefile create mode 100644 plugins/frustum_culling/README.md create mode 100644 plugins/frustum_culling/conanfile.py create mode 100644 plugins/frustum_culling/src/frustum_culling_conf.hpp create mode 100644 plugins/frustum_culling/src/frustum_culling_lanes.cpp create mode 100644 plugins/frustum_culling/src/frustum_culling_objects.cpp create mode 100644 plugins/frustum_culling/src/frustum_culling_test.cpp diff --git a/conanfile.py b/conanfile.py index ec266770f..b1b17d121 100644 --- a/conanfile.py +++ b/conanfile.py @@ -46,6 +46,7 @@ def cloe_requires(dep): cloe_requires("cloe-models") cloe_requires("cloe-plugin-basic") cloe_requires("cloe-plugin-clothoid-fit") + cloe_requires("cloe-plugin-frustum-culling") cloe_requires("cloe-plugin-gndtruth-extractor") cloe_requires("cloe-plugin-minimator") cloe_requires("cloe-plugin-mocks") diff --git a/models/CMakeLists.txt b/models/CMakeLists.txt index e05290326..b3d436fce 100644 --- a/models/CMakeLists.txt +++ b/models/CMakeLists.txt @@ -56,6 +56,7 @@ if(BUILD_TESTING) src/cloe/component/latlong_actuator_test.cpp src/cloe/component/utility/steering_utils_test.cpp src/cloe/utility/actuation_level_test.cpp + src/cloe/utility/frustum_culling_test.cpp ) set_target_properties(test-models PROPERTIES CXX_STANDARD 17 diff --git a/models/include/cloe/utility/frustum_culling.hpp b/models/include/cloe/utility/frustum_culling.hpp new file mode 100644 index 000000000..e98331c40 --- /dev/null +++ b/models/include/cloe/utility/frustum_culling.hpp @@ -0,0 +1,151 @@ +/* + * Copyright 2024 Robert Bosch GmbH + * + * 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. + * + * SPDX-License-Identifier: Apache-2.0 + */ +/** + * \file cloe/utility/frustum_culling.hpp + */ + +#pragma once + +#include // for cos, sin + +#include // for format +#include // for Vector3d +#include // for Frustum + +namespace cloe::utility { + +struct Point { + double x{0.0}; + double y{0.0}; +}; + +// Rotates a given point from one coordinate system to another and returns rotated point +Point rotate_point(const Point& point, double angle) { + return Point{std::cos(angle) * point.x - std::sin(angle) * point.y, + std::sin(angle) * point.x + std::cos(angle) * point.y}; +} + +// Calculates the corner points of a field of view. +// p0 to p2 are the points counter clock-wise with the idstance clip_far to the root +// An additional offset of the field of view to the original coordinate coordinate system is considered. +// +// clip_far p2 p1 +// \ / +// x \ / +// ^ \ / +// | \ / +// y <---| \ / +// p0 +// \param fov Angle [radians] between p0-p1 and p0-p2 +// \param offset Angle [radians] to shift the points p1 and p2 by +// \param clip_far Distance [meters] from p0 to p1 and from p0 to p2 +// +// \return Vector of points p0, p1, p2. +std::vector calc_corner_points(double fov, double offset, double clip_far) { + std::vector ret_points; + + // initialize points + Point p0{0.0, 0.0}; + Point p1{p0}; + Point p2{p0}; + + ret_points.push_back(p0); + + p1.x = clip_far * std::cos(-fov / 2.0); + p2.x = clip_far * std::cos(fov / 2.0); + + p1.y = clip_far * std::sin(-fov / 2.0); + p2.y = clip_far * std::sin(fov / 2.0); + + p1 = rotate_point(p1, offset); + p2 = rotate_point(p2, offset); + + ret_points.push_back(p1); + ret_points.push_back(p2); + + return ret_points; +} + +// Returns true if a given Point c is "on the left" of the line of two points a and b. +// "On the left" means the angle from the line a-b to the line a-c is in the range (0, M_PI) +bool is_left(Point a, Point b, Point c) { + return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x) > 0; +} + +bool is_inside_fov(double fov, bool is_left_0_p1, bool is_left_0_p2, std::string error_message) { + bool is_inside_fov = false; + if (fov >= M_PI && fov <= 2 * M_PI) { + // if the opening angle is between PI and 2 PI (180 and 360 degree) + // then everything is inside the fov that is _not_ right of p1 AND is _not_ left of p2 + is_inside_fov = !(!is_left_0_p1 && is_left_0_p2); + } else if (fov > 0.0 && fov < M_PI) { + // if the opening angle is greater 0 and smaller than M_PI + is_inside_fov = is_left_0_p1 && !is_left_0_p2; + } else { + throw std::runtime_error(error_message); + } + return is_inside_fov; +} + +bool is_point_inside_frustum(const cloe::Frustum& frustum, const Eigen::Vector3d& point) { + bool ret_val{false}; + + // calculate points in "frustum" sensor coordinate system, which starts at the frustum root + // and has x in viewing direction, and y to the left and z in the up direction. + auto corner_points_x_y_plane = + calc_corner_points(frustum.fov_h, frustum.offset_h, frustum.clip_far); + auto corner_points_x_z_plane = + calc_corner_points(frustum.fov_v, frustum.offset_v, frustum.clip_far); + + // calculate for xy plane + bool is_left_0_p1 = + is_left(corner_points_x_y_plane[0], corner_points_x_y_plane[1], Point{point.x(), point.y()}); + bool is_left_0_p2 = + is_left(corner_points_x_y_plane[0], corner_points_x_y_plane[2], Point{point.x(), point.y()}); + + bool is_inside_fov_xy = + is_inside_fov(frustum.fov_h, is_left_0_p1, is_left_0_p2, + fmt::format("The field of view in horizontal direction of your function is not " + "in the expected range of (0, 2*PI]. The value we got was {}", + frustum.fov_h)); + + // now calculate for xz plane + is_left_0_p1 = + is_left(corner_points_x_z_plane[0], corner_points_x_z_plane[1], Point{point.z(), point.x()}); + is_left_0_p2 = + is_left(corner_points_x_z_plane[0], corner_points_x_z_plane[2], Point{point.z(), point.x()}); + + bool is_inside_fov_xz = + is_inside_fov(frustum.fov_v, is_left_0_p1, is_left_0_p2, + fmt::format("The field of view in vertical direction of your function is not " + "in the expected range of (0, 2*PI]. The value we got was {}", + frustum.fov_v)); + + // if we are inside the fovs, we still need to check if the distance is within clip_near and clip_far + if (is_inside_fov_xy && is_inside_fov_xz) { + double distance = + std::sqrt(std::pow(point.x(), 2) + std::pow(point.y(), 2) + std::pow(point.z(), 2)); + + if (distance >= frustum.clip_near && distance < frustum.clip_far) { + ret_val = true; + } + } + return ret_val; +} + +} // namespace cloe::utility diff --git a/models/src/cloe/utility/frustum_culling_test.cpp b/models/src/cloe/utility/frustum_culling_test.cpp new file mode 100644 index 000000000..846f311e3 --- /dev/null +++ b/models/src/cloe/utility/frustum_culling_test.cpp @@ -0,0 +1,240 @@ +/* + * Copyright 2024 Robert Bosch GmbH + * + * 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. + * + * SPDX-License-Identifier: Apache-2.0 + */ +/** + * \file cloe/utility/frustum_culling_test.cpp + * \see cloe/utility/frustum_culling.hpp + */ + +#include +#include + +#include +#include + +TEST(models_frustum_culling, rotate_point) { + using Point = cloe::utility::Point; + // Inputs are + // Point --> point to be rotated + // double --> angle by which we rotate + // + // Output is + // Point --> expected rotated point + using input_type = std::tuple; + + // clang-format off + std::list input_list = + { + {Point{1.0, 0.0}, M_PI / 4.0, Point{0.707107, 0.707107}}, + {Point{1.0, 0.0}, M_PI / 2.0, Point{0.0, 1.0}}, + {Point{1.0, 0.0}, M_PI, Point{-1.0, 0.0}}, + {Point{1.0, 0.0}, -M_PI / 4.0, Point{0.707107, -0.707107}}, + {Point{1.0, 0.0}, -M_PI / 2.0, Point{0.0, -1.0}}, + {Point{1.0, 0.0}, -M_PI, Point{-1.0, 0.0}} + }; + // clang-format on + + int i = 0; + for (const auto& input : input_list) { + std::cout << "test case: " << i << std::endl; + const auto& rotated_point = cloe::utility::rotate_point(std::get<0>(input), std::get<1>(input)); + EXPECT_NEAR(rotated_point.x, std::get<2>(input).x, 0.001); + EXPECT_NEAR(rotated_point.y, std::get<2>(input).y, 0.001); + i++; + } +} + +TEST(models_frustum_culling, calc_corner_points) { + using Point = cloe::utility::Point; + // inputs are + // double --> field of view angle + // double --> offset to input coordinate system + // double --> clip far + // + // output is + // std::array --> expected output points + using input_type = std::tuple>; + // clang-format off + std::list input_list = + { + {M_PI / 4.0, 0.0, 200.0, {Point{0.0, 0.0}, Point{184.776, -76.537}, Point{184.776, 76.537}}}, + {M_PI / 2.0, 0.0, 300.0, {Point{0.0, 0.0}, Point{212.132, -212.132}, Point{212.132, 212.132}}}, + {1.5 * M_PI, 0.0, 200.0, {Point{0.0, 0.0}, Point{-141.421, -141.421}, Point{-141.421, 141.421}}}, + {M_PI / 2.0, -M_PI / 2.0, 200.0, {Point{0.0, 0.0}, Point{-141.421, -141.421}, Point{141.421, -141.421}}}, + {1.5 * M_PI, -M_PI / 2.0, 200.0, {Point{0.0, 0.0}, Point{-141.421, 141.421}, Point{141.421, 141.421}}}, + }; + // clang-format on + + int i = 0; + for (const auto& input : input_list) { + std::cout << "test case: " << i << std::endl; + const auto& output = cloe::utility::calc_corner_points(std::get<0>(input), std::get<1>(input), + std::get<2>(input)); + + EXPECT_NEAR(output[0].x, std::get<3>(input)[0].x, 0.01); + EXPECT_NEAR(output[0].y, std::get<3>(input)[0].y, 0.01); + EXPECT_NEAR(output[1].x, std::get<3>(input)[1].x, 0.01); + EXPECT_NEAR(output[1].y, std::get<3>(input)[1].y, 0.01); + EXPECT_NEAR(output[2].x, std::get<3>(input)[2].x, 0.01); + EXPECT_NEAR(output[2].y, std::get<3>(input)[2].y, 0.01); + i++; + } +} + +TEST(models_frustum_culling, is_left) { + using Point = cloe::utility::Point; + using input_type = std::tuple; + // clang-format off + std::list input_list = + { + {Point{0.0, 0.0}, Point{1.0, 0.0}, Point{0.0, 0.0}, false}, + {Point{0.0, 0.0}, Point{1.0, 0.0}, Point{0.5, 0.5}, true}, + {Point{0.0, 0.0}, Point{1.0, 0.0}, Point{0.0, 1.0}, true}, + {Point{0.0, 0.0}, Point{1.0, 0.0}, Point{-0.5, 0.5}, true}, + {Point{0.0, 0.0}, Point{1.0, 0.0}, Point{-1.0, 0.0}, false}, + {Point{0.0, 0.0}, Point{1.0, 0.0}, Point{-0.5, -0.5}, false}, + {Point{0.0, 0.0}, Point{1.0, 0.0}, Point{1.0, 0.0}, false}, + }; + // clang-format on + int i = 0; + for (const auto& input : input_list) { + std::cout << "test case: " << i << std::endl; + const bool output = + cloe::utility::is_left(std::get<0>(input), std::get<1>(input), std::get<2>(input)); + + EXPECT_EQ(output, std::get<3>(input)); + i++; + } +} + +TEST(models_frustum_culling, is_inside_fov) { + using input_type = std::tuple; + // clang-format off + std::list input_list = { + // fov, is_left_0_p1, is_left_0_p2, expect_error, result + {0.0, true, false, true, true}, // result does not matter as we expect an error + {M_PI / 2.0, true, false, false, true}, + {3.0 / 2.0 * M_PI, true, true, false, true}, + {3.0 / 2.0 * M_PI, true, false, false, true}, + {3.0 / 2.0 * M_PI, false, true, false, false}, + {3.0 / 2.0 * M_PI, false, false, false, true}, + {2.0 * M_PI, true, true, false, true}, + {2.0 * M_PI, true, false, false, true}, + {2.0 * M_PI, false, true, false, false}, + {2.0 * M_PI, false, false, false, true}, + {2.5 * M_PI, false, false, true, false}, + }; + // clang-format on + + int i = 0; + for (const auto& input : input_list) { + std::cout << "test_case: " << i << std::endl; + // the third input argument gives the information if we expect an error or not + // if we do not expect an error then compare the result of the function with expected result. + if (!std::get<3>(input)) { + bool result = cloe::utility::is_inside_fov(std::get<0>(input), std::get<1>(input), + std::get<2>(input), ""); + EXPECT_EQ(std::get<4>(input), result); + } else { + EXPECT_THROW(cloe::utility::is_inside_fov(std::get<0>(input), std::get<1>(input), + std::get<2>(input), ""), + std::runtime_error); + } + i++; + } +} + +TEST(models_frustum_culling, is_point_inside_frustum_default) { + cloe::Frustum frustum{}; + + // inputs are point that is passed to the function and expected result + using input_type = std::tuple; + std::list input_list = { + {{0.0, 0.0, 0.0}, true}, + {{0.0, 0.0, 1.0}, true}, + {{0.0, 1.0, 0.0}, true}, + {{1.0, 0.0, 0.0}, true}, + }; + + int i = 0; + for (const auto& input : input_list) { + std::cout << "test case: " << i << std::endl; + EXPECT_EQ(std::get<1>(input), + cloe::utility::is_point_inside_frustum(frustum, std::get<0>(input))); + i++; + } +} + +TEST(models_frustum_culling, is_point_inside_frustum_vary_fov_h) { + cloe::Frustum frustum{}; + + // inputs are frustm fov_h + using input_type = std::tuple; + // clang-format off + std::list input_list = { + {{0.01, 0.0, 0.0}, M_PI, true}, + {{-0.01, 0.0, 0.0}, M_PI, false}, + {{0.01, 0.0, 0.0}, M_PI, true}, + {{-0.01, 0.0, 0.0}, M_PI, false}, + {{0.01, 0.01, 1.0}, M_PI, true}, + {{0.01, 0.01, -1.0}, M_PI, true}, + {{1.0, 0.01, 0.0}, M_PI / 2.0, true}, + {{1.0, 100.0, 0.0}, M_PI / 2.0, false}, + {{1.0, -100.0, 0.0}, M_PI / 2.0, false}, + {{-1.0, 100.0, 0.0}, M_PI / 2.0, false}, + }; + // clang-format on + + int i = 0; + for (const auto& input : input_list) { + std::cout << "test case: " << i << std::endl; + frustum.fov_h = std::get<1>(input); + EXPECT_EQ(std::get<2>(input), + cloe::utility::is_point_inside_frustum(frustum, std::get<0>(input))); + i++; + } +} + +TEST(models_frustum_culling, is_point_inside_frustum_vary_offset_h) { + cloe::Frustum frustum{}; + frustum.fov_h = M_PI; + + using input_type = std::tuple; + // clang-format off + std::list input_list = { + {{1.0, 0.0, 0.0}, 0.0, true}, + {{-1.0, 0.0, 0.0}, 0.0, false}, + {{0.0, 1.0, 0.0}, M_PI / 2.0, true}, + {{0.0, -1.0, 0.0}, M_PI / 2.0, false}, + {{1.0, 0.0, 0.0}, M_PI, false}, + {{-1.0, 0.0, 0.0}, M_PI, true}, + {{0.0, 1.0, 0.0}, 1.5 * M_PI, false}, + {{0.0, -1.0, 0.0}, 1.5 * M_PI, true}, + {{1.0, 0.0, 0.0}, 2.0 * M_PI, true}, + {{-1.0, 0.0, 0.0}, 2.0 * M_PI, false}, + }; + // clang-format on + + int i = 0; + for (const auto& input : input_list) { + std::cout << "test case: " << i << std::endl; + frustum.offset_h = std::get<1>(input); + EXPECT_EQ(std::get<2>(input), + cloe::utility::is_point_inside_frustum(frustum, std::get<0>(input))); + i++; + } +} diff --git a/plugins/clothoid_fit/src/clothoid_fit.hpp b/plugins/clothoid_fit/src/clothoid_fit.hpp index 59f3b70ec..23e07d12d 100644 --- a/plugins/clothoid_fit/src/clothoid_fit.hpp +++ b/plugins/clothoid_fit/src/clothoid_fit.hpp @@ -101,6 +101,19 @@ uint8_t get_plane_mask(const Frustum& frustum, const Eigen::Vector3d& pt) { double fov_h_u = 0.5 * frustum.fov_h + frustum.offset_h; double fov_v_l = -0.5 * frustum.fov_v + frustum.offset_v; double fov_v_u = 0.5 * frustum.fov_v + frustum.offset_v; + + if (fov_h_l < -M_PI || fov_h_u > M_PI || fov_v_l < -M_PI || fov_v_u > M_PI) { + // clang-format off + throw std::runtime_error(fmt::format( + "The get_plane_mask function of the clothoid fit plugin only works in the range [-M_PI, M_PI]." + "However, the following values were calculated and at least one condition was not met: \n" + "fov_h_l: {}; has to be greater equal to -M_PI, \n" + "fov_h_u: {}; has to be smaller equal to M_PI, \n" + "fov_v_l: {}; has to be greater equal to -M_PI,\n" + "fov_v_u: {}; has to be smaller equal to M_PI. \n", + fov_h_l, fov_h_u, fov_v_l, fov_v_u)); + // clang-format on + } double range_mult = cos(frustum.offset_h) * cos(frustum.offset_v); uint8_t plane_mask{0}; // Horizontal field of view. diff --git a/plugins/clothoid_fit/src/clothoid_test.cpp b/plugins/clothoid_fit/src/clothoid_test.cpp index 25e0cb46a..8a5daf775 100644 --- a/plugins/clothoid_fit/src/clothoid_test.cpp +++ b/plugins/clothoid_fit/src/clothoid_test.cpp @@ -194,3 +194,47 @@ TEST(lane_boundary, clothoid_line) { EXPECT_NEAR(lb.curv_hor_change, 0.0, tol); EXPECT_NEAR(lb.dx_end, (lb.points.back() - lb.points.front()).norm(), tol); } + +TEST(frustum_exceed, error) { + cloe::LaneBoundary lb; + // Polyline in x-direction, culling (x-range). Cull at near-plane. + bool frustum_culling = true; + lb.points = + get_line_lb_points(Eigen::Vector3d(10.0, 0.0, 0.0), Eigen::Vector3d(10.0, 25.0, 0.0), 6); + + auto logger = cloe::logger::get("clothoid"); + + cloe::Frustum frustum; + // fov within limits, do not throw + frustum.fov_h = M_2X_PI; + frustum.fov_v = M_2X_PI - 0.01; + EXPECT_NO_THROW(cloe::component::fit_clothoid(logger, frustum_culling, frustum, lb)); + + // horizontal field of view exceed limits, expect runtime error + frustum.fov_h = M_2X_PI + 0.01; + frustum.fov_v = M_2X_PI; + EXPECT_THROW(cloe::component::fit_clothoid(logger, frustum_culling, frustum, lb), + std::runtime_error); + + // vertical field of view exceed limits, expect runtime error + frustum.fov_h = M_2X_PI; + frustum.fov_v = M_2X_PI + 0.01; + EXPECT_THROW(cloe::component::fit_clothoid(logger, frustum_culling, frustum, lb), + std::runtime_error); + + // horizontal offset exceeds limits + frustum.fov_h = M_PI; + frustum.fov_v = M_2X_PI; + frustum.offset_h = 0.5 * M_PI + 0.01; + EXPECT_THROW(cloe::component::fit_clothoid(logger, frustum_culling, frustum, lb), + std::runtime_error); + + // within limits + frustum.offset_h = 0.5 * M_PI - 0.01; + EXPECT_NO_THROW(cloe::component::fit_clothoid(logger, frustum_culling, frustum, lb)); + + // exeeds limits + frustum.offset_h = -0.5 * M_PI - 0.01; + EXPECT_THROW(cloe::component::fit_clothoid(logger, frustum_culling, frustum, lb), + std::runtime_error); +} diff --git a/plugins/frustum_culling/.vscode/launch.json b/plugins/frustum_culling/.vscode/launch.json new file mode 100644 index 000000000..fccd2c370 --- /dev/null +++ b/plugins/frustum_culling/.vscode/launch.json @@ -0,0 +1,27 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "name": "Debug plugins/frustum_culling [local]", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/build/Debug/test-frustum-culling", + "args": [], + "stopAtEntry": false, + "cwd": "${workspaceFolder}", + "environment": [], + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + } + ] +} \ No newline at end of file diff --git a/plugins/frustum_culling/CMakeLists.txt b/plugins/frustum_culling/CMakeLists.txt new file mode 100644 index 000000000..da8e22870 --- /dev/null +++ b/plugins/frustum_culling/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.15 FATAL_ERROR) + +project(cloe_plugin_frustum_culling LANGUAGES CXX) + +set(CLOE_FIND_PACKAGES ON CACHE BOOL "Call find_package() for cloe packages") +if(CLOE_FIND_PACKAGES) + find_package(cloe-runtime REQUIRED) + find_package(cloe-models REQUIRED) +endif() + +include(CloePluginSetup) +cloe_add_plugin( + TARGET component_frustum_culling_objects + SOURCES + src/frustum_culling_objects.cpp + LINK_LIBRARIES + cloe::runtime + cloe::models +) +cloe_add_plugin( + TARGET component_frustum_culling_lanes + SOURCES + src/frustum_culling_lanes.cpp + LINK_LIBRARIES + cloe::runtime + cloe::models +) + +set(CMAKE_CTEST_ARGUMENTS "--output-on-failure") +include(CTest) +if(BUILD_TESTING) + find_package(GTest REQUIRED) + include(GoogleTest) + + add_executable(test-frustum-culling + src/frustum_culling_test.cpp + ) + set_target_properties(test-frustum-culling PROPERTIES + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED ON + ) + target_link_libraries(test-frustum-culling + PRIVATE + GTest::gtest + GTest::gtest_main + cloe::runtime + cloe::models + ) + gtest_add_tests(TARGET test-frustum-culling) +endif() diff --git a/plugins/frustum_culling/Makefile b/plugins/frustum_culling/Makefile new file mode 100644 index 000000000..759272674 --- /dev/null +++ b/plugins/frustum_culling/Makefile @@ -0,0 +1 @@ +include ../../Makefile.package diff --git a/plugins/frustum_culling/README.md b/plugins/frustum_culling/README.md new file mode 100644 index 000000000..851d86b52 --- /dev/null +++ b/plugins/frustum_culling/README.md @@ -0,0 +1,37 @@ +# How to build + +Follow the instructions in the README.md in the repo root for building Cloe. + +# Debugging this plugin + +First, make this plugin editable: +``` +cd plugins/frustum_culling && make editable +``` + +Follow the instructions in the README.md in the repo root for building Cloe, e.g. call this from root to export the packages: +``` +make export-vendor export +``` + +For the next step, you need to set your version the profile you want to use. +Here in the example the profile is "debug_default" and the version is "0.22.0-83-gbe9777d-dirty". +Replace them in the following lines of code so they fit your setup. + +From plugins/frustum_culling, call: +``` +conan lock create --lockfile-out "build/conan.lock" -pr=debug_default --build=missing ../../conanfile.py +conan install . cloe-plugin-frustum-culling/0.22.0-83-gbe9777d-dirty@cloe/develop --install-folder="build" --build=missing --lockfile="build/conan.lock" +conan build . --source-folder="." --install-folder="build" +``` + +Make sure to build with a debug profile, i.e. use a conan profile with build_type "Debug". +If you have built before with a different profile, redo the steps in "How to build" with the correct profile. + +We provide a launch.json in the .vscode folder. +In order for vscode to find the launch.json, you need to open code from that directory. +``` +cd plugins/frustum_culling && code . +``` + +Then in the newly opened vs code window, you can select the target "Debug plugins/frustum_culling [local]" and run the debugger. diff --git a/plugins/frustum_culling/conanfile.py b/plugins/frustum_culling/conanfile.py new file mode 100644 index 000000000..d70987b5d --- /dev/null +++ b/plugins/frustum_culling/conanfile.py @@ -0,0 +1,85 @@ +# mypy: ignore-errors +# pylint: skip-file + +import os +from pathlib import Path + +from conan import ConanFile +from conan.errors import ConanInvalidConfiguration +from conan.tools import cmake, files, scm, env + +required_conan_version = ">=1.52.0" + + +class CloeComponentFrustumCulling(ConanFile): + name = "cloe-plugin-frustum-culling" + url = "https://github.com/eclipse/cloe" + description = "Cloe component plugin that changes coordinates of the sensed data and applies frustum culling" + license = "Apache-2.0" + settings = "os", "compiler", "build_type", "arch" + options = { + "pedantic": [True, False], + } + default_options = { + "pedantic": True + } + generators = "CMakeDeps", "VirtualRunEnv" + no_copy_source = True + exports_sources = [ + "src/*", + "CMakeLists.txt", + ] + + def set_version(self): + version_file = Path(self.recipe_folder) / "../../VERSION" + if version_file.exists(): + self.version = files.load(self, version_file).strip() + else: + git = scm.Git(self, self.recipe_folder) + self.version = git.run("describe --dirty=-dirty")[1:] + + def requirements(self): + self.requires(f"cloe-runtime/{self.version}@cloe/develop") + self.requires(f"cloe-models/{self.version}@cloe/develop") + self.requires("fmt/9.1.0") + self.requires("nlohmann_json/3.11.2", override=True) + + def build_requirements(self): + self.test_requires("gtest/1.13.0") + + def layout(self): + cmake.cmake_layout(self) + + def generate(self): + tc = cmake.CMakeToolchain(self) + tc.cache_variables["CMAKE_EXPORT_COMPILE_COMMANDS"] = True + tc.cache_variables["CLOE_PROJECT_VERSION"] = self.version + tc.cache_variables["TargetLintingExtended"] = self.options.pedantic + tc.generate() + + def build(self): + cm = cmake.CMake(self) + if self.should_configure: + cm.configure() + if self.should_build: + cm.build() + if self.should_test: + cm.test() + + def package(self): + cm = cmake.CMake(self) + if self.should_install: + cm.install() + + def package_id(self): + self.info.requires["boost"].full_package_mode() + del self.info.options.pedantic + + def package_info(self): + self.cpp_info.set_property("cmake_find_mode", "both") + self.cpp_info.set_property("cmake_file_name", self.name) + self.cpp_info.set_property("pkg_config_name", self.name) + + if not self.in_local_cache: # editable mode + libdir = os.path.join(self.build_folder, "lib"); + self.runenv_info.append_path("LD_LIBRARY_PATH", libdir) diff --git a/plugins/frustum_culling/src/frustum_culling_conf.hpp b/plugins/frustum_culling/src/frustum_culling_conf.hpp new file mode 100644 index 000000000..fb00bbf5e --- /dev/null +++ b/plugins/frustum_culling/src/frustum_culling_conf.hpp @@ -0,0 +1,103 @@ +/* + * Copyright 2024 Robert Bosch GmbH + * + * 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. + * + * SPDX-License-Identifier: Apache-2.0 + */ +/** + * \file frustum_culling_conf.hpp + */ + +#pragma once + +#include // for shared_ptr<> +#include // for default_random_engine, normal_distribution<> +#include // for string +#include // for move + +#include // for Component, ComponentFactory, ... +#include // for Frustum +#include // for Confable, Schema +#include // for Entity +#include // for ModelError +#include // for quaternion_from_rpy +#include // for to_json + +namespace cloe::frustum_culling_plugin { + +/** + * This class describes the frustum of a sensor. + */ +struct MountPose : public fable::Confable { + Eigen::Isometry3d pose; + double x, y, z; + double roll, pitch, yaw; + + void to_json(fable::Json& j) const override { + j = fable::Json{ + {"pose", pose}, + }; + } + + void convert() { + Eigen::Quaterniond q = cloe::utility::quaternion_from_rpy(roll, pitch, yaw); + pose = cloe::utility::pose_from_rotation_translation(q, Eigen::Vector3d(x, y, z)); + } + + protected: + fable::Schema schema_impl() override { + // clang-format off + using namespace fable::schema; // NOLINT + return Struct{ + {"x", make_schema(&x, "x-position in ego reference frame [m]").require()}, + {"y", make_schema(&y, "y-position in ego reference frame [m]").require()}, + {"z", make_schema(&z, "z-position in ego reference frame [m]").require()}, + {"roll", make_schema(&roll, "roll angle relative to ego reference frame [rad]").require()}, + {"pitch", make_schema(&pitch, "pitch angle relative to ego reference frame [rad]").require()}, + {"yaw", make_schema(&yaw, "yaw angle relative to ego reference frame [rad]").require()}, + }; + // clang-format on + } + + CONFABLE_FRIENDS(MountPose) +}; + +struct FrustumCullingConf : public Confable { + /** + * Configured sensor pose used as reference frame. + */ + MountPose ref_frame; + + /** + * Configured sensor frustum. + */ + Frustum frustum; + + CONFABLE_SCHEMA(FrustumCullingConf) { + using namespace fable::schema; + return Struct{ + {"reference_frame", make_schema(&ref_frame, "sensor frame of reference").require()}, + {"frustum", make_schema(&frustum, "sensor frustum").require()}, + }; + }; + + void to_json(Json& j) const override { + j = Json{ + {"reference_frame", ref_frame}, + {"frustum", frustum}, + }; + } +}; + +} // namespace cloe::frustum_culling_plugin diff --git a/plugins/frustum_culling/src/frustum_culling_lanes.cpp b/plugins/frustum_culling/src/frustum_culling_lanes.cpp new file mode 100644 index 000000000..74aef7cce --- /dev/null +++ b/plugins/frustum_culling/src/frustum_culling_lanes.cpp @@ -0,0 +1,132 @@ +/* + * Copyright 2024 Robert Bosch GmbH + * + * 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. + * + * SPDX-License-Identifier: Apache-2.0 + */ +/** + * \file frustum_culling_lanes.cpp + */ + +#include // for Isometry3d +#include // for shared_ptr<> +#include // for random_device +#include // for string +#include // for pair + +#include // for Component, Json +#include // for Frustum +#include // for LaneBoundary +#include // for LaneBoundarySensor +#include // for actions::ConfigureFactory +#include // for EXPORT_CLOE_PLUGIN +#include // for Registrar +#include // for Sync +#include // for actions::SetVariableActionFactory +#include "frustum_culling_conf.hpp" // for NoiseData, NoiseConf + +namespace cloe::frustum_culling_plugin { + +class LaneBoundaryFrustumCulling : public LaneBoundarySensor { + public: + LaneBoundaryFrustumCulling(const std::string& name, const FrustumCullingConf& conf, + std::shared_ptr obs) + : LaneBoundarySensor(name), config_(conf), sensor_(obs) {} + + virtual ~LaneBoundaryFrustumCulling() noexcept = default; + + const LaneBoundaries& sensed_lane_boundaries() const override { + if (cached_) { + return lbs_; + } + for (const auto& kv : sensor_->sensed_lane_boundaries()) { + auto lb = kv.second; + auto lb_mod = apply_frustum_culling(lb); + lbs_.insert(std::pair(kv.first, lb_mod)); + } + cached_ = true; + return lbs_; + } + + const Frustum& frustum() const override { return config_.frustum; } + + const Eigen::Isometry3d& mount_pose() const override { return config_.ref_frame.pose; } + + /** + * Process the underlying sensor and clear the cache. + * + * We could process and create the filtered list of lane boundaries now, but we can + * also delay it (lazy computation) and only do it when absolutely necessary. + * This comes at the minor cost of checking whether cached_ is true every + * time sensed_lane_boundaries() is called. + */ + Duration process(const Sync& sync) override { + // This currently shouldn't do anything, but this class acts as a prototype + // for How It Should Be Done. + Duration t = LaneBoundarySensor::process(sync); + if (t < sync.time()) { + return t; + } + + // Process the underlying sensor and clear the cache. + t = sensor_->process(sync); + clear_cache(); + return t; + } + + void reset() override { + LaneBoundarySensor::reset(); + sensor_->reset(); + clear_cache(); + } + + void abort() override { + LaneBoundarySensor::abort(); + sensor_->abort(); + } + + void enroll(Registrar& r) override { + r.register_action(std::make_unique( + &config_, "config", "configure lane sensor culling component")); + } + + protected: + LaneBoundary apply_frustum_culling(const LaneBoundary& lb) const { + LaneBoundary lb_m = lb; + // TODO(tobias): transform coordinate system and check if inside frustum + return lb_m; + } + + void clear_cache() { + lbs_.clear(); + cached_ = false; + } + + private: + FrustumCullingConf config_; + std::shared_ptr sensor_; + mutable bool cached_; + mutable LaneBoundaries lbs_; +}; + +DEFINE_COMPONENT_FACTORY( + LaneBoundaryFrustumCullingFactory, FrustumCullingConf, "frustum_culling_lanes", + "transform lane boundaries to given reference frame and apply frustum culling") + +DEFINE_COMPONENT_FACTORY_MAKE(LaneBoundaryFrustumCullingFactory, LaneBoundaryFrustumCulling, + LaneBoundarySensor) + +} // namespace cloe::frustum_culling_plugin + +EXPORT_CLOE_PLUGIN(cloe::frustum_culling_plugin::LaneBoundaryFrustumCullingFactory) diff --git a/plugins/frustum_culling/src/frustum_culling_objects.cpp b/plugins/frustum_culling/src/frustum_culling_objects.cpp new file mode 100644 index 000000000..c37700780 --- /dev/null +++ b/plugins/frustum_culling/src/frustum_culling_objects.cpp @@ -0,0 +1,152 @@ +/* + * Copyright 2024 Robert Bosch GmbH + * + * 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. + * + * SPDX-License-Identifier: Apache-2.0 + */ +/** + * \file frustum_culling_objects.cpp + */ + +#include // for Isometry3d, Vector3d +#include // for shared_ptr<> +#include // for random_device +#include // for string + +#include // for Component, Json +#include // for Frustum +#include // for Object +#include // for ObjectSensor +#include // for actions::ConfigureFactory +#include // for EXPORT_CLOE_PLUGIN +#include // for Registrar +#include // for Sync +#include // for actions::SetVariableActionFactory +#include // for Point, is_point_inside_frustum +#include "frustum_culling_conf.hpp" // for FrustumCullingConf + +namespace cloe::frustum_culling_plugin { + +/// @brief This class rotates objects to the coordinate system of a different sensor +/// +/// @details An object given in coordinate system c1 is converted to an object in coordinate system c2 via the configured reference frame in the configuration. +/// The reference frame configuration expects the values from c1 to c2, +/// e.g. if c2 is rotated by 90 degrees in mathematic positive direction from c1, the yaw should be set to +90 degree (in radians) +/// Analoguely, if the origin of c2 is translated 5 m in positive x direction from c1, the configuration should be set to +5 m. +/// The class considers first the translation in the original coordinate system (c1) and then the rotation. +class ObjectFrustumCulling : public ObjectSensor { + public: + ObjectFrustumCulling(const std::string& name, const FrustumCullingConf& conf, + std::shared_ptr obs) + : ObjectSensor(name), config_(conf), sensor_(obs), cached_(false) { + config_.ref_frame.convert(); + } + + virtual ~ObjectFrustumCulling() noexcept = default; + + const Objects& sensed_objects() const override { + if (cached_) { + return objects_; + } + for (const auto& o : sensor_->sensed_objects()) { + auto obj = apply_frustum_culling(o); + + bool is_object_inside = + cloe::utility::is_point_inside_frustum(config_.frustum, obj->pose.translation()); + + if (is_object_inside) { + objects_.push_back(obj); + } + } + cached_ = true; + return objects_; + } + + const Frustum& frustum() const override { return config_.frustum; } + + const Eigen::Isometry3d& mount_pose() const override { return config_.ref_frame.pose; } + + /** + * Process the underlying sensor and clear the cache. + * + * We could process and create the filtered list of objects now, but we can + * also delay it (lazy computation) and only do it when absolutely necessary. + * This comes at the minor cost of checking whether cached_ is true every + * time sensed_objects() is called. + */ + Duration process(const Sync& sync) override { + // This currently shouldn't do anything, but this class acts as a prototype + // for How It Should Be Done. + Duration t = ObjectSensor::process(sync); + if (t < sync.time()) { + return t; + } + + // Process the underlying sensor and clear the cache. + t = sensor_->process(sync); + clear_cache(); + return t; + } + + void reset() override { + ObjectSensor::reset(); + sensor_->reset(); + clear_cache(); + } + + void abort() override { + ObjectSensor::abort(); + sensor_->abort(); + } + + void enroll(Registrar& r) override { + r.register_action(std::make_unique( + &config_, "config", "configure object sensor culling component")); + } + + protected: + std::shared_ptr apply_frustum_culling(const std::shared_ptr& o) const { + auto obj = std::make_shared(*o); + + // Assumption: + // * cog_offset is in detected objects coordinate system + // * dimensions is in absolute values and not provided as a vector + // * the coordinate systems do not have any relative velocity/acceleration/angular velocity, both have same velocity/acceleration/angular velocity + obj->pose = this->mount_pose().inverse() * obj->pose; + obj->velocity = this->mount_pose().inverse().rotation() * obj->velocity; + obj->acceleration = this->mount_pose().inverse().rotation() * obj->acceleration; + obj->angular_velocity = this->mount_pose().inverse().rotation() * obj->angular_velocity; + return obj; + } + + void clear_cache() { + objects_.clear(); + cached_ = false; + } + + private: + FrustumCullingConf config_; + std::shared_ptr sensor_; + mutable bool cached_; + mutable Objects objects_; +}; + +DEFINE_COMPONENT_FACTORY(ObjectFrustumCullingFactory, FrustumCullingConf, "frustum_culling_objects", + "transform objects to given reference frame and apply frustum culling") + +DEFINE_COMPONENT_FACTORY_MAKE(ObjectFrustumCullingFactory, ObjectFrustumCulling, ObjectSensor) + +} // namespace cloe::frustum_culling_plugin + +EXPORT_CLOE_PLUGIN(cloe::frustum_culling_plugin::ObjectFrustumCullingFactory) diff --git a/plugins/frustum_culling/src/frustum_culling_test.cpp b/plugins/frustum_culling/src/frustum_culling_test.cpp new file mode 100644 index 000000000..916369735 --- /dev/null +++ b/plugins/frustum_culling/src/frustum_culling_test.cpp @@ -0,0 +1,413 @@ +/* + * Copyright 2024 Robert Bosch GmbH + * + * 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. + * + * SPDX-License-Identifier: Apache-2.0 + */ +/** + * \file frustum_culling_test.cpp + * \see frustum_culling_conf.hpp + * \see frustum_culling_objects.cpp + * \see frustum_culling_lanes.cpp + */ + +#include + +#include // for assert_validate +#include "frustum_culling_conf.hpp" // for NoiseConf +#include "frustum_culling_objects.cpp" + +using namespace cloe; // NOLINT(build/namespaces) +using namespace cloe::frustum_culling_plugin; + +TEST(frustum_culling, deserialize) { + FrustumCullingConf c; + + fable::assert_validate(c, R"({ + "reference_frame": { + "x": 2.5, + "y": 0.0, + "z": 0.6, + "roll": 0.0, + "pitch": 0.1, + "yaw": 0.0 + }, + "frustum" : { + "clip_near": 0.0, + "clip_far": 100.0, + "fov_h": 0.7854, + "fov_v": 0.7854, + "offset_h": 0.0, + "offset_v": 0.0 + } + })"); +} + +class MyObjectSensor : public NopObjectSensor { + public: + MyObjectSensor() = default; + + void add_object(const Object& object) { objects_.push_back(std::make_shared(object)); } + + void clear_objects() { objects_.clear(); } + void set_mount(const Eigen::Isometry3d& mount) { mount_ = mount; } +}; + +class ObjectFrustumCullingTest : public ::testing::Test { + public: + void createObjectFustumCullingSensor() { + culling_sensor_ = std::make_shared( + "test_controller", config_, std::make_shared(object_sensor_)); + } + + std::shared_ptr culling_sensor_; + MyObjectSensor object_sensor_{}; + FrustumCullingConf config_{}; +}; + +cloe::Object createDefaultObject() { + cloe::Object object{}; + + // Unfortunately cloe uses this complicated classes + Eigen::Quaterniond qt = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + object.pose.setIdentity(); + object.pose.linear() = qt.matrix(); + + object.pose.translation() = Eigen::Vector3d{0.0, 0.0, 0.0}; + // set the length, width and height of the ego + object.dimensions = Eigen::Vector3d{4.0, 2.0, 1.5}; + + return object; +} + +TEST_F(ObjectFrustumCullingTest, test_rotation) { + // configuration + config_.ref_frame.yaw = M_PI / 2.0; + config_.frustum.fov_h = M_PI; + config_.frustum.clip_far = 500; + double epsilon = 0.01; + config_.frustum.fov_v = 2 * M_PI - epsilon; + + //set input object + auto object = createDefaultObject(); + object.pose.translation() = Eigen::Vector3d{10.0, 15.0, 0.0}; + object_sensor_.add_object(object); + + createObjectFustumCullingSensor(); + + auto objects = culling_sensor_->sensed_objects(); + if (objects.size() > 0) { + EXPECT_NEAR(objects.front()->pose.translation().x(), 15.0, 0.01); + EXPECT_NEAR(objects.front()->pose.translation().y(), -10.0, 0.01); + EXPECT_NEAR(objects.front()->pose.translation().z(), 0.0, 0.01); + } else { + FAIL() << "Fail: No object inside the fov found. Evaluation not possible."; + } +} + +TEST_F(ObjectFrustumCullingTest, test_translation) { + // configuration + config_.ref_frame.x = 30.0; + config_.ref_frame.y = 30.0; + + config_.frustum.offset_h = M_PI; + config_.frustum.fov_h = M_PI; + config_.frustum.clip_far = 500; + double epsilon = 0.01; + config_.frustum.fov_v = 2 * M_PI - epsilon; + + //set input object + auto object = createDefaultObject(); + object.pose.translation() = Eigen::Vector3d{10.0, 0.0, 0.0}; + object_sensor_.add_object(object); + + createObjectFustumCullingSensor(); + + auto objects = culling_sensor_->sensed_objects(); + + if (objects.size() > 0) { + // expect the difference of the "config_.ref_frame - object.pose.translation" + EXPECT_NEAR(objects.front()->pose.translation().x(), -20.0, 0.01); + EXPECT_NEAR(objects.front()->pose.translation().y(), -30.0, 0.01); + EXPECT_NEAR(objects.front()->pose.translation().z(), 0.0, 0.01); + } else { + FAIL() << "Fail: No object inside the fov found. Evaluation not possible."; + } +} + +TEST_F(ObjectFrustumCullingTest, test_translation_2) { + // configuration + config_.ref_frame.x = 30.0; + config_.ref_frame.y = 30.0; + + config_.frustum.offset_h = -M_PI; + config_.frustum.fov_h = M_PI; + config_.frustum.clip_far = 500; + double epsilon = 0.01; + config_.frustum.fov_v = 2 * M_PI - epsilon; + + //set input object + auto object = createDefaultObject(); + object.pose.translation() = Eigen::Vector3d{10.0, 0.0, 0.0}; + object_sensor_.add_object(object); + + createObjectFustumCullingSensor(); + + auto objects = culling_sensor_->sensed_objects(); + + if (objects.size() > 0) { + // expect the difference of the "config_.ref_frame - object.pose.translation" + EXPECT_NEAR(objects.front()->pose.translation().x(), -20.0, 0.01); + EXPECT_NEAR(objects.front()->pose.translation().y(), -30.0, 0.01); + EXPECT_NEAR(objects.front()->pose.translation().z(), 0.0, 0.01); + } else { + FAIL() << "Fail: No object inside the fov found. Evaluation not possible."; + } +} + +TEST_F(ObjectFrustumCullingTest, rotation_and_translation) { + // configuration + config_.ref_frame.x = 30.0; + config_.ref_frame.y = 40.0; + config_.ref_frame.yaw = M_PI / 2.0; + + config_.frustum.offset_h = M_PI; + config_.frustum.fov_h = M_PI; + config_.frustum.clip_far = 500; + + double epsilon = 0.01; + config_.frustum.fov_v = 2 * M_PI - epsilon; + + //set input object + auto object = createDefaultObject(); + object.pose.translation() = Eigen::Vector3d{10.0, 0.0, 0.0}; + object_sensor_.add_object(object); + + createObjectFustumCullingSensor(); + + auto objects = culling_sensor_->sensed_objects(); + + /* + c2 + x <-------| + | + | + P1 + |y + \/ ^ x + | + | + y <-----| c1 + */ + if (objects.size() > 0) { + EXPECT_NEAR(objects.front()->pose.translation().x(), -40.0, 0.01); + EXPECT_NEAR(objects.front()->pose.translation().y(), 20.0, 0.01); + EXPECT_NEAR(objects.front()->pose.translation().z(), 0.0, 0.01); + } else { + FAIL() << "Fail: No object inside the fov found. Evaluation not possible."; + } +} + +TEST_F(ObjectFrustumCullingTest, rotation_and_translation_including_obj_rotation) { + // configuration + config_.ref_frame.x = 30.0; + config_.ref_frame.y = 40.0; + config_.ref_frame.yaw = M_PI / 2.0; + + config_.frustum.offset_h = M_PI; + config_.frustum.fov_h = M_PI; + config_.frustum.clip_far = 500; + + double epsilon = 0.01; + config_.frustum.fov_v = 2 * M_PI - epsilon; + + //set input object + auto object = createDefaultObject(); + // rotate object by 135 degree in mathematical negative direction + Eigen::Quaterniond qt = Eigen::AngleAxisd(3.0 / 4.0 * M_PI, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + object.pose.linear() = qt.toRotationMatrix(); + object.pose.translation() = Eigen::Vector3d{10.0, 0.0, 0.0}; + object_sensor_.add_object(object); + + createObjectFustumCullingSensor(); + + auto objects = culling_sensor_->sensed_objects(); + + /* + c2 + x <-------| ___ + | / / + | /x / P1 + | /__/ + |y / + \/ \/ (direction of orientation) + + ^ x + | + | + y <-----| c1 + */ + if (objects.size() > 0) { + EXPECT_NEAR(objects.front()->pose.translation().x(), -40.0, 0.01); + EXPECT_NEAR(objects.front()->pose.translation().y(), 20.0, 0.01); + EXPECT_NEAR(objects.front()->pose.translation().z(), 0.0, 0.01); + + // be aware that for calculating the rotation from an object, several solutions exist + // hence you have to take care of all values returned by the eulerAngles function + // here, the solution was designed in a way so it works with the input values of the test case + Eigen::Vector3d yaw_pitch_roll = objects.front()->pose.rotation().eulerAngles(2, 1, 0); + + // Expect a rotation of 45 degree in positive direction in the new coordinate system. + EXPECT_NEAR(yaw_pitch_roll[0], M_PI / 4.0, 0.01); + } else { + FAIL() << "Fail: No object inside the fov found. Evaluation not possible."; + } +} + +TEST_F(ObjectFrustumCullingTest, object_in_fov) { + // configuration + config_.ref_frame.x = 10.0; + config_.ref_frame.y = 10.0; + config_.ref_frame.yaw = 0.0; + + // cover 1 quadrant with field of view + config_.frustum.offset_h = M_PI / 4.0; + config_.frustum.fov_h = M_PI / 2.0; + config_.frustum.clip_far = 500; + + double epsilon = 0.01; + config_.frustum.fov_v = 2 * M_PI - epsilon; + + // set input object + auto object = createDefaultObject(); + object.pose.translation() = Eigen::Vector3d{30.0, 20.0, 0.0}; + object_sensor_.add_object(object); + + createObjectFustumCullingSensor(); + + auto objects = culling_sensor_->sensed_objects(); + + if (objects.size() > 0) { + EXPECT_NEAR(objects.front()->pose.translation().x(), 20.0, 0.01); + EXPECT_NEAR(objects.front()->pose.translation().y(), 10.0, 0.01); + EXPECT_NEAR(objects.front()->pose.translation().z(), 0.0, 0.01); + } else { + FAIL() << "Fail: No object inside the fov found. Evaluation not possible."; + } +} + +TEST_F(ObjectFrustumCullingTest, object_outside_fov) { + // configuration + config_.ref_frame.x = 10.0; + config_.ref_frame.y = 10.0; + config_.ref_frame.yaw = 0.0; + + // cover 1 quadrant with field of view + config_.frustum.offset_h = M_PI / 4.0; + config_.frustum.fov_h = M_PI / 2.0; + config_.frustum.clip_far = 500; + + double epsilon = 0.01; + config_.frustum.fov_v = 2 * M_PI - epsilon; + + // set input object + auto object = createDefaultObject(); + object.pose.translation() = Eigen::Vector3d{30.0, -20.0, 0.0}; + object_sensor_.add_object(object); + + createObjectFustumCullingSensor(); + + auto objects = culling_sensor_->sensed_objects(); + + if (objects.size() > 0) { + FAIL() << "Fail: Expected no object inside the field of view in this test case, but objects " + "size was greater 0"; + } +} + +TEST_F(ObjectFrustumCullingTest, velocity) { + // configuration + config_.ref_frame.x = 10.0; + config_.ref_frame.y = 10.0; + config_.ref_frame.yaw = M_PI / 2.0; + + // cover 1 quadrant with field of view + config_.frustum.offset_h = 0.0; + config_.frustum.fov_h = M_PI; + config_.frustum.clip_far = 500; + + double epsilon = 0.01; + config_.frustum.fov_v = 2 * M_PI - epsilon; + + // set input object + auto object = createDefaultObject(); + object.pose.translation() = Eigen::Vector3d{30.0, 30.0, 0.0}; + object.velocity.x() = 10.0; + object.acceleration.y() = 5.0; + object.angular_velocity.x() = -1.0; + object_sensor_.add_object(object); + + createObjectFustumCullingSensor(); + + auto objects = culling_sensor_->sensed_objects(); + + if (objects.size() > 0) { + EXPECT_NEAR(objects.front()->velocity.y(), -10.0, 0.01); + EXPECT_NEAR(objects.front()->acceleration.x(), 5.0, 0.01); + EXPECT_NEAR(objects.front()->angular_velocity.y(), 1.0, 0.01); + EXPECT_NEAR(objects.front()->dimensions.x(), 4.0, 0.01); + EXPECT_NEAR(objects.front()->dimensions.y(), 2.0, 0.01); + } else { + FAIL() << "Expected at least 1 object"; + } +} + +TEST_F(ObjectFrustumCullingTest, expect_error_horizontal_fov) { + // configuration + + config_.frustum.fov_h = 3 * M_PI; + // set input object + auto object = createDefaultObject(); + object.pose.translation() = Eigen::Vector3d{30.0, 30.0, 0.0}; + object.velocity.x() = 10.0; + object.acceleration.y() = 5.0; + object.angular_velocity.x() = -1.0; + object_sensor_.add_object(object); + + createObjectFustumCullingSensor(); + + EXPECT_THROW(culling_sensor_->sensed_objects(), std::runtime_error); +} + +TEST_F(ObjectFrustumCullingTest, expect_error_vertical_fov) { + // configuration + config_.frustum.fov_v = 3 * M_PI; + + // set input object + auto object = createDefaultObject(); + object.pose.translation() = Eigen::Vector3d{30.0, 30.0, 0.0}; + object.velocity.x() = 10.0; + object.acceleration.y() = 5.0; + object.angular_velocity.x() = -1.0; + object_sensor_.add_object(object); + + createObjectFustumCullingSensor(); + + EXPECT_THROW(culling_sensor_->sensed_objects(), std::runtime_error); +}