From 7a163c2320f36c8f916de5d3f10bca74e37dcad0 Mon Sep 17 00:00:00 2001 From: manishthani Date: Tue, 8 Jan 2019 11:06:05 +0100 Subject: [PATCH] Extended functionality of point transforms --- Docs/python_api.md | 2 + LibCarla/source/carla/geom/Math.h | 5 +- LibCarla/source/carla/geom/Transform.h | 51 +++++++++++++++++++- LibCarla/source/test/test_geom.cpp | 66 +++++++++++++++++++++++++- PythonAPI/source/libcarla/Geom.cpp | 17 +++++++ PythonAPI/test/test_transform.py | 62 +++++++++++++++++++++++- 6 files changed, 198 insertions(+), 5 deletions(-) diff --git a/Docs/python_api.md b/Docs/python_api.md index 60d87ca78..20d87d571 100644 --- a/Docs/python_api.md +++ b/Docs/python_api.md @@ -252,6 +252,8 @@ Static presets - `rotation` - `__eq__(other)` - `__ne__(other)` +- `transform_point` +- `transform_point_list` ## `carla.BoundingBox` diff --git a/LibCarla/source/carla/geom/Math.h b/LibCarla/source/carla/geom/Math.h index 698ec66e2..df247b074 100644 --- a/LibCarla/source/carla/geom/Math.h +++ b/LibCarla/source/carla/geom/Math.h @@ -17,7 +17,6 @@ namespace geom { class Math { public: - static constexpr auto pi() { return 3.14159265358979323846264338327950288; } @@ -34,6 +33,10 @@ namespace geom { return rad * (180.0 / pi()); } + static constexpr auto to_radians(double deg) { + return deg * (pi() / 180.0); + } + template static T clamp( const T &a, diff --git a/LibCarla/source/carla/geom/Transform.h b/LibCarla/source/carla/geom/Transform.h index 5402729d7..f981e5359 100644 --- a/LibCarla/source/carla/geom/Transform.h +++ b/LibCarla/source/carla/geom/Transform.h @@ -9,11 +9,13 @@ #include "carla/MsgPack.h" #include "carla/geom/Location.h" #include "carla/geom/Rotation.h" +#include "carla/geom/Math.h" #ifdef LIBCARLA_INCLUDED_FROM_UE4 -# include "Math/Transform.h" +#include "Math/Transform.h" #endif // LIBCARLA_INCLUDED_FROM_UE4 + namespace carla { namespace geom { @@ -24,7 +26,8 @@ namespace geom { Transform(const Location &in_location, const Rotation &in_rotation) : location(in_location), - rotation(in_rotation) {} + rotation(in_rotation) { + } Location location; Rotation rotation; @@ -37,6 +40,50 @@ namespace geom { return !(*this == rhs); } + + Location GetLocation() const { + return location; + } + + Rotation GetRotation() const { + return rotation; + } + + template + inline void TransformPointList(std::vector &in_point_list) const { + for (T in_point : in_point_list) { + TransformPoint(in_point); + } + } + + inline void TransformPoint (Location &in_point) const { + + // Rotate + double cy = cos(Math::to_radians(rotation.yaw)); + double sy = sin(Math::to_radians(rotation.yaw)); + double cr = cos(Math::to_radians(rotation.roll)); + double sr = sin(Math::to_radians(rotation.roll)); + double cp = cos(Math::to_radians(rotation.pitch)); + double sp = sin(Math::to_radians(rotation.pitch)); + + Location out_point; + out_point.x = in_point.x * ( cp * cy ) + + in_point.y * ( cy * sp * sr - sy * cr) + + in_point.z * (-cy * sp * cr - sy * sr); + out_point.y = in_point.x * ( cp * sy) + + in_point.y * ( sy * sp * sr + cy * cr) + + in_point.z * (-sy * sp * cr + cy * sr); + + out_point.z = in_point.x * (sp) + + in_point.y * -(cp * sr) + + in_point.z * (cp * cr); + + // Translate + out_point += location; + + in_point = out_point; + } + #ifdef LIBCARLA_INCLUDED_FROM_UE4 Transform(const FTransform &transform) diff --git a/LibCarla/source/test/test_geom.cpp b/LibCarla/source/test/test_geom.cpp index 3aae10673..d6278d3c8 100644 --- a/LibCarla/source/test/test_geom.cpp +++ b/LibCarla/source/test/test_geom.cpp @@ -8,11 +8,75 @@ #include #include - +#include #include using namespace carla::geom; +TEST(geom, single_point_no_transform) { + constexpr double error = 0.001; + + Location translation (0.0, 0.0, 0.0); + Rotation rotation(0.0, 0.0, 0.0); + Transform transform (translation, rotation); + + Location point (1.0,1.0,1.0); + transform.TransformPoint(point); + Location result_point(1.0, 1.0, 1.0); + + ASSERT_NEAR(point.x, result_point.x, error); + ASSERT_NEAR(point.y, result_point.y, error); + ASSERT_NEAR(point.z, result_point.z, error); + +} + +TEST(geom, single_point_translation) { + constexpr double error = 0.001; + + Location translation (2.0,5.0,7.0); + Rotation rotation (0.0, 0.0, 0.0); + Transform transform (translation, rotation); + + Location point (0.0, 0.0, 0.0); + transform.TransformPoint(point); + Location result_point(2.0, 5.0, 7.0); + + ASSERT_NEAR(point.x, result_point.x, error); + ASSERT_NEAR(point.y, result_point.y, error); + ASSERT_NEAR(point.z, result_point.z, error); +} + +TEST(geom, single_point_rotation) { + constexpr double error = 0.001; + + Location translation (0.0,0.0,0.0); + Rotation rotation (0.0,180.0,0.0); // y z x + Transform transform (translation, rotation); + + Location point (0.0, 0.0, 1.0); + transform.TransformPoint(point); + Location result_point(0.0, 0.0, 1.0); + ASSERT_NEAR(point.x, result_point.x, error); + ASSERT_NEAR(point.y, result_point.y, error); + ASSERT_NEAR(point.z, result_point.z, error); +} + +TEST(geom, single_point_translation_and_rotation) { + constexpr double error = 0.001; + + Location translation (0.0,0.0,-1.0); // x y z + Rotation rotation (90.0,0.0,0.0); // y z x + Transform transform (translation, rotation); + + Location point (0.0, 0.0, 2.0); + transform.TransformPoint(point); + Location result_point(-2.0, 0.0, -1.0); + ASSERT_NEAR(point.x, result_point.x, error); + ASSERT_NEAR(point.y, result_point.y, error); + 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); diff --git a/PythonAPI/source/libcarla/Geom.cpp b/PythonAPI/source/libcarla/Geom.cpp index b1f799df3..455f1cb52 100644 --- a/PythonAPI/source/libcarla/Geom.cpp +++ b/PythonAPI/source/libcarla/Geom.cpp @@ -98,13 +98,30 @@ void export_geom() { .def(self_ns::str(self_ns::self)) ; + class_ >("vector_of_locations") + .def(vector_indexing_suite>()) + .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_point", +[](const cg::Transform &self, cg::Location &location) { + self.TransformPoint(location); + return location; + }, arg("in_point")) + + .def("transform_point_list", +[](const cg::Transform &self, std::vector &location_list) { + for (cg::Location &location : location_list) { + self.TransformPoint(location); + } + return location_list; + }) .def(self_ns::str(self_ns::self)) ; diff --git a/PythonAPI/test/test_transform.py b/PythonAPI/test/test_transform.py index 897fae8bb..f040cb7ec 100644 --- a/PythonAPI/test/test_transform.py +++ b/PythonAPI/test/test_transform.py @@ -8,7 +8,6 @@ import carla import unittest - class testLocation(unittest.TestCase): def test_default_values(self): location = carla.Location() @@ -115,3 +114,64 @@ class testTransform(unittest.TestCase): carla.Rotation(pitch=4.0, yaw=5.0, roll=6.0)) s = 'Transform(Location(x=1, y=2, z=3), Rotation(pitch=4, yaw=5, roll=6))' self.assertEqual(str(t), s) + + def test_translation(self): + error = .001 + t = carla.Transform( + carla.Location(x=8.0, y=19.0, z=20.0), + carla.Rotation(pitch=0.0, yaw=0.0, roll=0.0)) + point = carla.Location(x=0.0, y=0.0, z=0.0) + t.transform_point(point) + self.assertTrue(abs(point.x - 8.0) <= error) + self.assertTrue(abs(point.y - 19.0) <= error) + self.assertTrue(abs(point.z - 20.0) <= error) + + def test_rotation(self): + error = .001 + t = carla.Transform( + carla.Location(x=0.0, y=0.0, z=0.0), + carla.Rotation(pitch=180.0, yaw=0.0, roll=0.0)) + point = carla.Location(x=0.0, y=0.0, z=1.0) + t.transform_point(point) + + self.assertTrue(abs(point.x - 0.0) <= error) + self.assertTrue(abs(point.y - 0.0) <= error) + self.assertTrue(abs(point.z - (-1.0)) <= error) + + def test_rotation_and_translation(self): + error = .001 + t = carla.Transform( + carla.Location(x=0.0, y=0.0, z=-1.0), + carla.Rotation(pitch=90.0, yaw=0.0, roll=0.0)) + point = carla.Location(x=0.0, y=0.0, z=2.0) + t.transform_point(point) + + self.assertTrue(abs(point.x - (-2.0)) <= error) + self.assertTrue(abs(point.y - 0.0) <= error) + self.assertTrue(abs(point.z - (-1.0)) <= error) + + def test_list_rotation_and_translation(self): + error = .001 + t = carla.Transform( + carla.Location(x=0.0, y=0.0, z=-1.0), + carla.Rotation(pitch=90.0, yaw=0.0, roll=0.0)) + + point_list = carla.vector_of_locations() + point_list.append(carla.Location(x=0.0, y=0.0, z=2.0)) + point_list.append(carla.Location(x=0.0, y=10.0, z=1.0)) + point_list.append(carla.Location(x=0.0, y=18.0, z=2.0)) + + t.transform_point_list(point_list) + + solution_list = carla.vector_of_locations() + solution_list.append(carla.Location(x=-2.0, y=0.0, z=-1.0)) + solution_list.append(carla.Location(x=-1.0, y=10.0, z=-1.0)) + solution_list.append(carla.Location(x=-2.0, y=18.0, z=-1.0)) + + for i in range(len(point_list)): + self.assertTrue(abs(point_list[i].x - solution_list[i].x) <= error) + self.assertTrue(abs(point_list[i].y - solution_list[i].y) <= error) + self.assertTrue(abs(point_list[i].z - solution_list[i].z) <= error) + + + \ No newline at end of file