Merge branch 'master' into patch-1

CHANGELOG.md

@@ -1,3 +1,7 @@
+## Latest Release Version
+
+  * Added point transformation functionality for LibCarla and PythonAPI.
+
 ## CARLA 0.9.2
 
   * Updated ROS bridge for CARLA 0.9.X (moved to its own repository)

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/client/detail/Simulator.cpp

@@ -32,7 +32,7 @@ namespace detail {
       log_warning(
           "Version mismatch detected: You are trying to connect to a simulator",
           "that might be incompatible with this API");
-      log_warning("Client API version:    =", vc);
+      log_warning("Client API version     =", vc);
       log_warning("Simulator API version  =", vs);
     }
   }

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

@@ -8,6 +8,7 @@
 
 #include "carla/MsgPack.h"
 #include "carla/geom/Location.h"
+#include "carla/geom/Math.h"
 #include "carla/geom/Rotation.h"
 
 #ifdef LIBCARLA_INCLUDED_FROM_UE4
@@ -37,6 +38,34 @@ namespace geom {
       return !(*this == rhs);
     }
 
+    void TransformPoint(Vector3D &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));
+
+      Vector3D 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

@@ -55,6 +55,13 @@ namespace geom {
 } // namespace geom
 } // namespace carla
 
+static void TransformList(const carla::geom::Transform &self, boost::python::list &list) {
+  auto length = boost::python::len(list);
+   for (auto i = 0u; i < length; ++i) {
+    self.TransformPoint(boost::python::extract<carla::geom::Vector3D &>(list[i]));
+  }
+}
+
 void export_geom() {
   using namespace boost::python;
   namespace cg = carla::geom;
@@ -73,7 +80,7 @@ void export_geom() {
     .def(self_ns::str(self_ns::self))
   ;
 
-  class_<cg::Location>("Location")
+class_<cg::Location, bases<cg::Vector3D>>("Location")
     .def(init<float, float, float>((arg("x")=0.0f, arg("y")=0.0f, arg("z")=0.0f)))
     .add_property("x", +[](const cg::Location &self) { return self.x; }, +[](cg::Location &self, float x) { self.x = x; })
     .add_property("y", +[](const cg::Location &self) { return self.y; }, +[](cg::Location &self, float y) { self.y = y; })
@@ -99,12 +106,18 @@ void export_geom() {
   ;
 
   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", &TransformList)
+    .def("transform", +[](const cg::Transform &self, cg::Vector3D &location) {
+      self.TransformPoint(location);
+      return location;
+    }, arg("in_point"))
     .def(self_ns::str(self_ns::self))
   ;
 

PythonAPI/test/test_transform.py

@@ -115,3 +115,82 @@ 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)
+        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)
+
+        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)
+
+        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_location(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.Location(x=0.0, y=0.0, z=2.0),
+                      carla.Location(x=0.0, y=10.0, z=1.0),
+                      carla.Location(x=0.0, y=18.0, z=2.0)
+                      ]
+        t.transform(point_list)
+
+        solution_list = [carla.Location(-2.0, 0.0, -1.0),
+                         carla.Location(-1.0, 10.0, -1.0),
+                         carla.Location(-2.0, 18.0, -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)
+
+    def test_list_rotation_and_translation_vector3d(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.Vector3D(0.0, 0.0, 2.0),
+                      carla.Vector3D(0.0, 10.0, 1.0),
+                      carla.Vector3D(0.0, 18.0, 2.0)
+                      ]
+        t.transform(point_list)
+
+        solution_list = [carla.Vector3D(-2.0, 0.0, -1.0),
+                         carla.Vector3D(-1.0, 10.0, -1.0),
+                         carla.Vector3D(-2.0, 18.0, -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)