From 26f8a6547e0c4573a9ca4738e5486335a2c9e8b4 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 19 Dec 2024 01:00:16 +0100 Subject: [PATCH] ref(geometry): add axis header, improve --- .../geometry/include/geometry/axis/axis.hpp | 41 ++++++++ .../include/geometry/polygon/polygon.hpp | 5 - .../include/geometry/quaternion/normalize.hpp | 13 ++- .../include/geometry/quaternion/operator.hpp | 30 +++--- .../include/geometry/vector3/rotate.hpp | 31 ++---- common/math/geometry/src/axis/axis.cpp | 94 +++++++++++++++++++ common/math/geometry/src/polygon/polygon.cpp | 67 ------------- .../test/src/polygon/test_polygon.cpp | 2 + .../primitives/primitive.hpp | 1 + 9 files changed, 170 insertions(+), 114 deletions(-) create mode 100644 common/math/geometry/include/geometry/axis/axis.hpp create mode 100644 common/math/geometry/src/axis/axis.cpp diff --git a/common/math/geometry/include/geometry/axis/axis.hpp b/common/math/geometry/include/geometry/axis/axis.hpp new file mode 100644 index 00000000000..e0da906d9f5 --- /dev/null +++ b/common/math/geometry/include/geometry/axis/axis.hpp @@ -0,0 +1,41 @@ + +// Copyright 2015 TIER IV, Inc. 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. + +#ifndef GEOMETRY__AXIS__AXIS_HPP_ +#define GEOMETRY__AXIS__AXIS_HPP_ + +#include +#include + +namespace math +{ +namespace geometry +{ +enum class Axis { X = 0, Y = 1, Z = 2 }; + +auto axisToEigenAxis(Axis axis) -> Eigen::Vector3d; + +auto getMaxValue(const std::vector & points, const Axis & axis) + -> double; + +auto getMinValue(const std::vector & points, const Axis & axis) + -> double; + +auto filterByAxis(const std::vector & points, const Axis & axis) + -> std::vector; +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__AXIS__AXIS_HPP_ diff --git a/common/math/geometry/include/geometry/polygon/polygon.hpp b/common/math/geometry/include/geometry/polygon/polygon.hpp index 02f0374368c..231dae67ac6 100644 --- a/common/math/geometry/include/geometry/polygon/polygon.hpp +++ b/common/math/geometry/include/geometry/polygon/polygon.hpp @@ -21,11 +21,6 @@ namespace math { namespace geometry { -enum class Axis { X = 0, Y = 1, Z = 2 }; -double getMaxValue(const std::vector & points, const Axis & axis); -double getMinValue(const std::vector & points, const Axis & axis); -std::vector filterByAxis( - const std::vector & points, const Axis & axis); std::vector get2DConvexHull( const std::vector & points); } // namespace geometry diff --git a/common/math/geometry/include/geometry/quaternion/normalize.hpp b/common/math/geometry/include/geometry/quaternion/normalize.hpp index 9b819b7bf3f..8e83a7f364e 100644 --- a/common/math/geometry/include/geometry/quaternion/normalize.hpp +++ b/common/math/geometry/include/geometry/quaternion/normalize.hpp @@ -27,15 +27,18 @@ namespace geometry template ::value, std::nullptr_t> = nullptr> auto normalize(const T & q) { - const auto n = norm(q); - if (std::fabs(n) <= std::numeric_limits::epsilon()) { + if (const auto n = norm(q); std::fabs(n) <= std::numeric_limits::epsilon()) { THROW_SIMULATION_ERROR( - "Norm of quaternion (", q.w, ",", q.x, ",", q.y, ",", q.z, ") is ", n, + "Norm of Quaternion(", q.w, ",", q.x, ",", q.y, ",", q.z, ") is ", n, ". The norm of the quaternion you want to normalize should be greater than ", std::numeric_limits::epsilon()); + } else { + return geometry_msgs::build() + .x(q.x / n) + .y(q.y / n) + .z(q.z / n) + .w(q.w / n); } - return geometry_msgs::build().x(q.x / n).y(q.y / n).z(q.z / n).w( - q.w / n); } } // namespace geometry } // namespace math diff --git a/common/math/geometry/include/geometry/quaternion/operator.hpp b/common/math/geometry/include/geometry/quaternion/operator.hpp index ca124ed5e8d..4839a282333 100644 --- a/common/math/geometry/include/geometry/quaternion/operator.hpp +++ b/common/math/geometry/include/geometry/quaternion/operator.hpp @@ -69,26 +69,26 @@ template < typename T, typename U, std::enable_if_t, IsLikeVector3>, std::nullptr_t> = nullptr> -auto operator*(const T & a, const U & b) +auto operator*(const T & rotation, const U & vector) { - T b_quat; - b_quat.x = b.x; - b_quat.y = b.y; - b_quat.z = b.z; - b_quat.w = 0.0; + T vector_as_quaternion; + vector_as_quaternion.x = vector.x; + vector_as_quaternion.y = vector.y; + vector_as_quaternion.z = vector.z; + vector_as_quaternion.w = 0.0; - T a_inv = a; - a_inv.x = -a.x; - a_inv.y = -a.y; - a_inv.z = -a.z; - a_inv.w = a.w; + T inverse_rotation = rotation; + inverse_rotation.x = -rotation.x; + inverse_rotation.y = -rotation.y; + inverse_rotation.z = -rotation.z; + inverse_rotation.w = rotation.w; - T result_quat = a * b_quat * a_inv; + T rotated_quaternion = rotation * vector_as_quaternion * inverse_rotation; U rotated_vector; - rotated_vector.x = result_quat.x; - rotated_vector.y = result_quat.y; - rotated_vector.z = result_quat.z; + rotated_vector.x = vector_as_quaternion.x; + rotated_vector.y = vector_as_quaternion.y; + rotated_vector.z = vector_as_quaternion.z; return rotated_vector; } diff --git a/common/math/geometry/include/geometry/vector3/rotate.hpp b/common/math/geometry/include/geometry/vector3/rotate.hpp index 33648f01f83..91c5b592bae 100644 --- a/common/math/geometry/include/geometry/vector3/rotate.hpp +++ b/common/math/geometry/include/geometry/vector3/rotate.hpp @@ -15,6 +15,7 @@ #ifndef GEOMETRY__VECTOR3__ROTATE_HPP_ #define GEOMETRY__VECTOR3__ROTATE_HPP_ +#include #include #include @@ -22,20 +23,6 @@ namespace math { namespace geometry { -inline Eigen::Vector3d axisToEigenAxis(Axis axis) -{ - switch (axis) { - case Axis::X: - return Eigen::Vector3d::UnitX(); - case Axis::Y: - return Eigen::Vector3d::UnitY(); - case Axis::Z: - return Eigen::Vector3d::UnitZ(); - default: - THROW_SIMULATION_ERROR("Invalid axis specified."); - } -} - // Rotate a vector by a given angle around a specified axis template < typename T, std::enable_if_t>, std::nullptr_t> = nullptr> @@ -43,15 +30,15 @@ auto rotate(T & v, const double angle, const Axis axis) { if (!std::isfinite(angle)) { THROW_SIMULATION_ERROR("The provided angle for rotation is not finite."); - } - - const Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axisToEigenAxis(axis))); - const Eigen::Vector3d eigen_vector(v.x, v.y, v.z); - const Eigen::Vector3d rotated_vector = rotation * eigen_vector; + } else { + const Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axisToEigenAxis(axis))); + const Eigen::Vector3d eigen_vector(v.x, v.y, v.z); + const Eigen::Vector3d rotated_vector = rotation * eigen_vector; - v.x = rotated_vector.x(); - v.y = rotated_vector.y(); - v.z = rotated_vector.z(); + v.x = rotated_vector.x(); + v.y = rotated_vector.y(); + v.z = rotated_vector.z(); + } } } // namespace geometry } // namespace math diff --git a/common/math/geometry/src/axis/axis.cpp b/common/math/geometry/src/axis/axis.cpp new file mode 100644 index 00000000000..422ddbbc36c --- /dev/null +++ b/common/math/geometry/src/axis/axis.cpp @@ -0,0 +1,94 @@ +// Copyright 2015 TIER IV, Inc. 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. + +#include +// #include +// #include +// #include +// #include +#include +#include + +namespace math +{ +namespace geometry +{ +auto axisToEigenAxis(Axis axis) -> Eigen::Vector3d +{ + switch (axis) { + case Axis::X: + return Eigen::Vector3d::UnitX(); + case Axis::Y: + return Eigen::Vector3d::UnitY(); + case Axis::Z: + return Eigen::Vector3d::UnitZ(); + default: + THROW_SIMULATION_ERROR("Invalid axis specified."); + } +} + +auto getMaxValue(const std::vector & points, const Axis & axis) -> double +{ + if (const auto values = filterByAxis(points, axis); values.size() == 1) { + return values.front(); + } else { + return *std::max_element(values.begin(), values.end()); + } +} + +auto getMinValue(const std::vector & points, const Axis & axis) -> double +{ + if (const auto values = filterByAxis(points, axis); values.size() == 1) { + return values.front(); + } else { + return *std::min_element(values.begin(), values.end()); + } +} + +auto filterByAxis(const std::vector & points, const Axis & axis) + -> std::vector +{ + if (points.empty()) { + THROW_SIMULATION_ERROR( + "Invalid point list is specified, while trying to filter ", + axis == Axis::X ? "X" + : axis == Axis::Y ? "Y" + : "Z", + " axis. ", + "The point list in filterByAxis() should have at least one point to filter. " + "This message is not originally intended to be displayed, if you see it, please " + "contact the developer of traffic_simulator."); + } else { + const auto axisExtractor = + [](const Axis axis) -> std::function { + switch (axis) { + case Axis::X: + return [](const geometry_msgs::msg::Point & p) { return p.x; }; + case Axis::Y: + return [](const geometry_msgs::msg::Point & p) { return p.y; }; + case Axis::Z: + return [](const geometry_msgs::msg::Point & p) { return p.z; }; + default: + throw std::invalid_argument("Invalid axis"); + } + }; + + std::vector single_axis; + std::transform( + points.begin(), points.end(), std::back_inserter(single_axis), axisExtractor(axis)); + return single_axis; + } +} +} // namespace geometry +} // namespace math diff --git a/common/math/geometry/src/polygon/polygon.cpp b/common/math/geometry/src/polygon/polygon.cpp index 7a7148b40d8..06886e6299a 100644 --- a/common/math/geometry/src/polygon/polygon.cpp +++ b/common/math/geometry/src/polygon/polygon.cpp @@ -47,72 +47,5 @@ std::vector get2DConvexHull( } return polygon; } - -double getMaxValue(const std::vector & points, const Axis & axis) -{ - if (points.empty()) { - THROW_SIMULATION_ERROR( - "Invalid point list is specified, while getting max value on ", - axis == Axis::X ? "X" - : axis == Axis::Y ? "Y" - : "Z", - " axis. ", - "The point list in getMaxValue should have at least one point to get the max value from. " - "This message is not originally intended to be displayed, if you see it, please " - "contact the developer of traffic_simulator."); - } - const auto values = filterByAxis(points, axis); - if (values.size() == 1) { - return values.front(); - } - return *std::max_element(values.begin(), values.end()); -} - -double getMinValue(const std::vector & points, const Axis & axis) -{ - if (points.empty()) { - THROW_SIMULATION_ERROR( - "Invalid point list is specified, while getting min value on ", - axis == Axis::X ? "X" - : axis == Axis::Y ? "Y" - : "Z", - " axis. ", - "The point list in getMinValue should have at least one point to get the min value from. " - "This message is not originally intended to be displayed, if you see it, please " - "contact the developer of traffic_simulator."); - } - const auto values = filterByAxis(points, axis); - if (values.size() == 1) { - return values.front(); - } - return *std::min_element(values.begin(), values.end()); -} - -std::vector filterByAxis( - const std::vector & points, const Axis & axis) -{ - std::vector ret; - switch (axis) { - case Axis::X: { - std::transform( - points.begin(), points.end(), std::back_inserter(ret), - [](const geometry_msgs::msg::Point & point) { return point.x; }); - break; - } - case Axis::Y: { - std::transform( - points.begin(), points.end(), std::back_inserter(ret), - [](const geometry_msgs::msg::Point & point) { return point.y; }); - break; - } - case Axis::Z: { - std::transform( - points.begin(), points.end(), std::back_inserter(ret), - [](const geometry_msgs::msg::Point & point) { return point.z; }); - break; - } - } - return ret; -} } // namespace geometry } // namespace math diff --git a/common/math/geometry/test/src/polygon/test_polygon.cpp b/common/math/geometry/test/src/polygon/test_polygon.cpp index e7278fa6566..9b4596e9913 100644 --- a/common/math/geometry/test/src/polygon/test_polygon.cpp +++ b/common/math/geometry/test/src/polygon/test_polygon.cpp @@ -14,6 +14,7 @@ #include +#include #include #include @@ -41,6 +42,7 @@ TEST(Polygon, filterByAxis) EXPECT_DOUBLE_EQ(values_z[2], -3.0); } +/// @todo refactor test to throw exception TEST(Polygon, filterByAxisEmptyVector) { std::vector points; diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp index c514d40247e..b9c6b9d2fb4 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include