From 5295261aca54f62519a9f0c269fb67fa50ed97bc Mon Sep 17 00:00:00 2001 From: nsubiron Date: Thu, 17 Jan 2019 11:27:57 +0100 Subject: [PATCH] Add forward vector to rotations and transforms --- CHANGELOG.md | 1 + Docs/python_api.md | 5 ++-- LibCarla/source/carla/geom/Math.cpp | 10 ++++++++ LibCarla/source/carla/geom/Math.h | 5 ++++ LibCarla/source/carla/geom/Rotation.h | 10 ++++++++ LibCarla/source/carla/geom/Transform.h | 4 ++++ LibCarla/source/test/test_geom.cpp | 33 +++++++++++++++++++++++++- PythonAPI/source/libcarla/Geom.cpp | 7 +++--- PythonAPI/source/libcarla/libcarla.cpp | 2 +- 9 files changed, 70 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7e0b1389e..b134165df 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,7 @@ * Added point transformation functionality for LibCarla and PythonAPI * Added "sensor_tick" attribute to sensors (cameras and lidars) to specify the capture rate in seconds + * Added "get_forward_vector()" to rotation and transform, retrieves the unit vector on the rotation's X-axis * Added support for Deepin in PythonAPI's setup.py ## CARLA 0.9.2 diff --git a/Docs/python_api.md b/Docs/python_api.md index 20d87d571..750e8661f 100644 --- a/Docs/python_api.md +++ b/Docs/python_api.md @@ -243,6 +243,7 @@ Static presets - `pitch` - `yaw` - `roll` +- `get_forward_vector()` - `__eq__(other)` - `__ne__(other)` @@ -250,10 +251,10 @@ Static presets - `location` - `rotation` +- `transform(geom_object)` +- `get_forward_vector()` - `__eq__(other)` - `__ne__(other)` -- `transform_point` -- `transform_point_list` ## `carla.BoundingBox` diff --git a/LibCarla/source/carla/geom/Math.cpp b/LibCarla/source/carla/geom/Math.cpp index cdb0c5e28..993497bb0 100644 --- a/LibCarla/source/carla/geom/Math.cpp +++ b/LibCarla/source/carla/geom/Math.cpp @@ -6,6 +6,8 @@ #include "carla/geom/Math.h" +#include "carla/geom/Rotation.h" + namespace carla { namespace geom { @@ -127,5 +129,13 @@ namespace geom { transf_p.x >= -extent.x && transf_p.y >= -extent.y; } + Vector3D Math::GetForwardVector(const Rotation &rotation) { + const float cp = std::cos(to_radians(rotation.pitch)); + const float sp = std::sin(to_radians(rotation.pitch)); + const float cy = std::cos(to_radians(rotation.yaw)); + const float sy = std::sin(to_radians(rotation.yaw)); + return {cy * cp, sy * cp, sp}; + } + } // namespace geom } // namespace carla diff --git a/LibCarla/source/carla/geom/Math.h b/LibCarla/source/carla/geom/Math.h index c7ba8916d..c076d755b 100644 --- a/LibCarla/source/carla/geom/Math.h +++ b/LibCarla/source/carla/geom/Math.h @@ -15,6 +15,8 @@ namespace carla { namespace geom { + class Rotation; + class Math { public: @@ -99,6 +101,9 @@ namespace geom { const Vector3D &, double, // [radians] const Vector3D &); + + /// Compute the unit vector pointing towards the X-axis of @a rotation. + static Vector3D GetForwardVector(const Rotation &rotation); }; } // namespace geom diff --git a/LibCarla/source/carla/geom/Rotation.h b/LibCarla/source/carla/geom/Rotation.h index e5d987b48..77832695e 100644 --- a/LibCarla/source/carla/geom/Rotation.h +++ b/LibCarla/source/carla/geom/Rotation.h @@ -7,6 +7,8 @@ #pragma once #include "carla/MsgPack.h" +#include "carla/geom/Math.h" +#include "carla/geom/Vector3D.h" #ifdef LIBCARLA_INCLUDED_FROM_UE4 # include "Math/Rotator.h" @@ -41,6 +43,14 @@ namespace geom { yaw(y), roll(r) {} + // ========================================================================= + // -- Other methods -------------------------------------------------------- + // ========================================================================= + + Vector3D GetForwardVector() const { + return Math::GetForwardVector(*this); + } + // ========================================================================= // -- Comparison operators ------------------------------------------------- // ========================================================================= diff --git a/LibCarla/source/carla/geom/Transform.h b/LibCarla/source/carla/geom/Transform.h index 808827b96..ce27ed420 100644 --- a/LibCarla/source/carla/geom/Transform.h +++ b/LibCarla/source/carla/geom/Transform.h @@ -45,6 +45,10 @@ namespace geom { // -- Other methods -------------------------------------------------------- // ========================================================================= + Vector3D GetForwardVector() const { + return rotation.GetForwardVector(); + } + void TransformPoint(Vector3D &in_point) const { // Rotate double cy = cos(Math::to_radians(rotation.yaw)); diff --git a/LibCarla/source/test/test_geom.cpp b/LibCarla/source/test/test_geom.cpp index d6278d3c8..b9c2b62ec 100644 --- a/LibCarla/source/test/test_geom.cpp +++ b/LibCarla/source/test/test_geom.cpp @@ -11,6 +11,17 @@ #include #include +namespace carla { +namespace geom { + + std::ostream &operator<<(std::ostream &out, const Vector3D &vector3D) { + out << "{x=" << vector3D.x << ", y=" << vector3D.y << ", z=" << vector3D.z << '}'; + return out; + } + +} // namespace geom +} // namespace carla + using namespace carla::geom; TEST(geom, single_point_no_transform) { @@ -76,7 +87,6 @@ TEST(geom, single_point_translation_and_rotation) { ASSERT_NEAR(point.z, result_point.z, error); } - TEST(geom, distance) { constexpr double error = .01; ASSERT_NEAR(Math::Distance({0, 0, 0}, {0, 0, 0}), 0.0, error); @@ -138,6 +148,27 @@ TEST(geom, nearest_point_segment) { } } +TEST(geom, forward_vector) { + auto compare = [](Rotation rotation, Vector3D expected) { + constexpr float eps = 2.0f * std::numeric_limits::epsilon(); + auto result = rotation.GetForwardVector(); + EXPECT_TRUE( + (std::abs(expected.x - result.x) < eps) && + (std::abs(expected.y - result.y) < eps) && + (std::abs(expected.z - result.z) < eps)) + << "result = " << result << '\n' + << "expected = " << expected; + }; + // pitch yaw roll x y z + compare({ 0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}); + compare({ 0.0f, 0.0f, 123.0f}, {1.0f, 0.0f, 0.0f}); + compare({360.0f, 360.0f, 0.0f}, {1.0f, 0.0f, 0.0f}); + compare({ 0.0f, 90.0f, 0.0f}, {0.0f, 1.0f, 0.0f}); + compare({ 0.0f, -90.0f, 0.0f}, {0.0f,-1.0f, 0.0f}); + compare({ 90.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 1.0f}); + compare({180.0f, -90.0f, 0.0f}, {0.0f, 1.0f, 0.0f}); +} + TEST(geom, point_in_rectangle) { ASSERT_TRUE(Math::PointInRectangle( Vector3D(0, 0, 0), Vector3D(1, 1, 0), 0, Vector3D(0, 0, 0))); diff --git a/PythonAPI/source/libcarla/Geom.cpp b/PythonAPI/source/libcarla/Geom.cpp index 7162481b5..0c4a30f16 100644 --- a/PythonAPI/source/libcarla/Geom.cpp +++ b/PythonAPI/source/libcarla/Geom.cpp @@ -100,24 +100,25 @@ class_>("Location") .def_readwrite("pitch", &cg::Rotation::pitch) .def_readwrite("yaw", &cg::Rotation::yaw) .def_readwrite("roll", &cg::Rotation::roll) + .def("get_forward_vector", &cg::Rotation::GetForwardVector) .def("__eq__", &cg::Rotation::operator==) .def("__ne__", &cg::Rotation::operator!=) .def(self_ns::str(self_ns::self)) ; class_("Transform") - .def(init( (arg("location")=cg::Location(), arg("rotation")=cg::Rotation()))) .def_readwrite("location", &cg::Transform::location) .def_readwrite("rotation", &cg::Transform::rotation) - .def("__eq__", &cg::Transform::operator==) - .def("__ne__", &cg::Transform::operator!=) .def("transform", &TransformList) .def("transform", +[](const cg::Transform &self, cg::Vector3D &location) { self.TransformPoint(location); return location; }, arg("in_point")) + .def("get_forward_vector", &cg::Transform::GetForwardVector) + .def("__eq__", &cg::Transform::operator==) + .def("__ne__", &cg::Transform::operator!=) .def(self_ns::str(self_ns::self)) ; diff --git a/PythonAPI/source/libcarla/libcarla.cpp b/PythonAPI/source/libcarla/libcarla.cpp index 223104835..9349e36ad 100644 --- a/PythonAPI/source/libcarla/libcarla.cpp +++ b/PythonAPI/source/libcarla/libcarla.cpp @@ -119,12 +119,12 @@ static auto MakeCallback(boost::python::object callback) { }; } +#include "Geom.cpp" #include "Actor.cpp" #include "Blueprint.cpp" #include "Client.cpp" #include "Control.cpp" #include "Exception.cpp" -#include "Geom.cpp" #include "Map.cpp" #include "Sensor.cpp" #include "SensorData.cpp"