Extended functionality of point transforms

Docs/python_api.md

@@ -252,6 +252,8 @@ Static presets
 - `rotation`
 - `__eq__(other)`
 - `__ne__(other)`
+- `transform_point`
+- `transform_point_list`
 
 ## `carla.BoundingBox`
 

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 <typename T>
     static T clamp(
         const T &a,

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<class T>
+    inline void TransformPointList(std::vector<T> &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)

LibCarla/source/test/test_geom.cpp

@@ -8,11 +8,75 @@
 
 #include <carla/geom/Vector3D.h>
 #include <carla/geom/Math.h>
-
+#include <carla/geom/Transform.h>
 #include <limits>
 
 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);

PythonAPI/source/libcarla/Geom.cpp

@@ -98,13 +98,30 @@ void export_geom() {
     .def(self_ns::str(self_ns::self))
   ;
 
+  class_<std::vector<cg::Location> >("vector_of_locations")
+    .def(vector_indexing_suite<std::vector<cg::Location>>())
+    .def(self_ns::str(self_ns::self))
+  ;
+
   class_<cg::Transform>("Transform")
+
     .def(init<cg::Location, cg::Rotation>(
         (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<cg::Location> &location_list) {
+      for (cg::Location &location : location_list) {
+        self.TransformPoint(location);
+      }
+      return location_list;
+    })
     .def(self_ns::str(self_ns::self))
   ;
 

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)
+
+        
+