CARLA
test_geom.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #include "test.h"
8 
9 #include <carla/geom/Vector3D.h>
10 #include <carla/geom/Math.h>
11 #include <carla/geom/BoundingBox.h>
12 #include <carla/geom/Transform.h>
13 #include <limits>
14 
15 namespace carla {
16 namespace geom {
17 
18  std::ostream &operator<<(std::ostream &out, const Vector3D &vector3D) {
19  out << "{x=" << vector3D.x << ", y=" << vector3D.y << ", z=" << vector3D.z << '}';
20  return out;
21  }
22 
23 } // namespace geom
24 } // namespace carla
25 
26 using namespace carla::geom;
27 
28 TEST(geom, single_point_no_transform) {
29  constexpr double error = 0.001;
30 
31  Location translation (0.0, 0.0, 0.0);
32  Rotation rotation(0.0, 0.0, 0.0);
33  Transform transform (translation, rotation);
34 
35  Location point (1.0,1.0,1.0);
36  transform.TransformPoint(point);
37  Location result_point(1.0, 1.0, 1.0);
38 
39  ASSERT_NEAR(point.x, result_point.x, error);
40  ASSERT_NEAR(point.y, result_point.y, error);
41  ASSERT_NEAR(point.z, result_point.z, error);
42 
43 }
44 
45 TEST(geom, single_point_translation) {
46  constexpr double error = 0.001;
47 
48  Location translation (2.0,5.0,7.0);
49  Rotation rotation (0.0, 0.0, 0.0);
50  Transform transform (translation, rotation);
51 
52  Location point (0.0, 0.0, 0.0);
53  transform.TransformPoint(point);
54  Location result_point(2.0, 5.0, 7.0);
55 
56  ASSERT_NEAR(point.x, result_point.x, error);
57  ASSERT_NEAR(point.y, result_point.y, error);
58  ASSERT_NEAR(point.z, result_point.z, error);
59 }
60 
61 
62 TEST(geom, single_point_transform_inverse_transform_coherence) {
63  constexpr double error = 0.001;
64 
65  const Location point(-3.14f, 1.337f, 4.20f);
66  const Location translation (1.41f, -4.7f, 9.2f);
67  const Rotation rotation (-47.0f, 37.0f, 250.2f);
68  const Transform transform (translation, rotation);
69 
70  auto transformed_point = point;
71  transform.TransformPoint(transformed_point);
72 
73  auto point_back_to_normal = transformed_point;
74  transform.InverseTransformPoint(point_back_to_normal);
75 
76  ASSERT_NEAR(point.x, point_back_to_normal.x, error) << "result.x is " << point_back_to_normal.x << " but expected " << point.x;
77  ASSERT_NEAR(point.y, point_back_to_normal.y, error) << "result.y is " << point_back_to_normal.y << " but expected " << point.y;
78  ASSERT_NEAR(point.z, point_back_to_normal.z, error) << "result.z is " << point_back_to_normal.z << " but expected " << point.z;
79 }
80 
81 
82 TEST(geom, bbox_get_local_vertices_get_world_vertices_coherence) {
83  constexpr double error = 0.001;
84 
85  const BoundingBox bbox (Location(10.2f, -32.4f, 15.6f), Vector3D(9.2f, 13.5f, 20.3f));
86 
87  const Location bbox_location(-3.14f, 1.337f, 4.20f);
88  const Rotation bbox_rotation (-59.0f, 17.0f, -650.2f);
89  const Transform bbox_transform(bbox_location, bbox_rotation);
90 
91  const auto local_vertices = bbox.GetLocalVertices();
92  const auto world_vertices = bbox.GetWorldVertices(bbox_transform);
93  for (auto i = 0u; i < local_vertices.size(); ++i){
94  const auto &local_vertex = local_vertices[i];
95 
96  auto transformed_local_vertex = local_vertex;
97  bbox_transform.TransformPoint(transformed_local_vertex);
98 
99  const auto &world_vertex = world_vertices[i];
100 
101  ASSERT_NEAR(transformed_local_vertex.x, world_vertex.x, error) << "result.x is " << transformed_local_vertex.x << " but expected " << world_vertex.x;
102  ASSERT_NEAR(transformed_local_vertex.y, world_vertex.y, error) << "result.y is " << transformed_local_vertex.y << " but expected " << world_vertex.y;
103  ASSERT_NEAR(transformed_local_vertex.z, world_vertex.z, error) << "result.z is " << transformed_local_vertex.z << " but expected " << world_vertex.z;
104  }
105 }
106 
107 
108 TEST(geom, single_point_rotation) {
109  constexpr double error = 0.001;
110 
111  Location translation (0.0,0.0,0.0);
112  Rotation rotation (0.0,180.0,0.0); // y z x
113  Transform transform (translation, rotation);
114 
115  Location point (0.0, 0.0, 1.0);
116  transform.TransformPoint(point);
117  Location result_point(0.0, 0.0, 1.0);
118  ASSERT_NEAR(point.x, result_point.x, error);
119  ASSERT_NEAR(point.y, result_point.y, error);
120  ASSERT_NEAR(point.z, result_point.z, error);
121 }
122 
123 TEST(geom, single_point_translation_and_rotation) {
124  constexpr double error = 0.001;
125 
126  Location translation (0.0,0.0,-1.0); // x y z
127  Rotation rotation (90.0,0.0,0.0); // y z x
128  Transform transform (translation, rotation);
129 
130  Location point (0.0, 0.0, 2.0);
131  transform.TransformPoint(point);
132  Location result_point(-2.0, 0.0, -1.0);
133  ASSERT_NEAR(point.x, result_point.x, error);
134  ASSERT_NEAR(point.y, result_point.y, error);
135  ASSERT_NEAR(point.z, result_point.z, error);
136 }
137 
138 TEST(geom, distance) {
139  constexpr double error = .01;
140  ASSERT_NEAR(Math::Distance({0, 0, 0}, {0, 0, 0}), 0.0, error);
141  ASSERT_NEAR(Math::Distance({1, 1, 1}, {0, 0, 0}), 1.732051, error);
142  ASSERT_NEAR(Math::Distance({0, 0, 0}, {1, 1, 1}), 1.732051, error);
143  ASSERT_NEAR(Math::Distance({-1, -1, -1}, {0, 0, 0}), 1.732051, error);
144  ASSERT_NEAR(Math::Distance({0, 0, 0}, {-1, -1, -1}), 1.732051, error);
145  ASSERT_NEAR(Math::Distance({7, 4, 3}, {17, 6, 2}), 10.246951, error);
146  ASSERT_NEAR(Math::Distance({7, -4, 3}, {-17, 6, 2}), 26.019224, error);
147  ASSERT_NEAR(Math::Distance({5, 6, 7}, {-6, 3, -4}), 15.84298, error);
148  ASSERT_NEAR(Math::Distance({7, 4, 3}, {17, 6, 2}), 10.246951, error);
149 }
150 
151 TEST(geom, nearest_point_segment) {
152  const float segment[] = {
153  0, 0, 10, 0,
154  2, 5, 10, 8,
155  -6, 8, 8, -2,
156  8, 2,-10, 3,
157  3, 3, -6, -5,
158  3, -3, 2, 5,
159  4, -6, 5, 4,
160  -1, -4,-10, 8,
161  -7, -5, 5, 5,
162  -5, 6, 3, -9
163  };
164 
165  const Vector3D point[] = {
166  { 1, -1, 0},
167  { 10, 10, 0},
168  {-10, 10, 0},
169  { 10, -10, 0},
170  {-10, -10, 0},
171  { 0, 5, 0},
172  { 0, -5, 0},
173  { 1, 4, 0},
174  { -1, 1, 0},
175  { 3, 2.5, 0}
176  };
177 
178  const int results[] = {
179  0, 1, 7, 9, 8, 2, 9, 2, 8, 3
180  };
181 
182  for (int i = 0; i < 10; ++i) {
183  double min_dist = std::numeric_limits<double>::max();
184  int id = -1;
185  for (int j = 0; j < 40; j += 4) {
186  const double dist = Math::DistanceSegmentToPoint(
187  point[i],
188  {segment[j + 0], segment[j + 1], 0},
189  {segment[j + 2], segment[j + 3], 0}).second;
190  if (dist < min_dist) {
191  min_dist = dist;
192  id = j / 4;
193  }
194  }
195  ASSERT_EQ(id, results[i]) << "Fails point number: " << i;
196  }
197 }
198 
199 TEST(geom, forward_vector) {
200  auto compare = [](Rotation rotation, Vector3D expected) {
201  constexpr float eps = 2.0f * std::numeric_limits<float>::epsilon();
202  auto result = rotation.GetForwardVector();
203  EXPECT_TRUE(
204  (std::abs(expected.x - result.x) < eps) &&
205  (std::abs(expected.y - result.y) < eps) &&
206  (std::abs(expected.z - result.z) < eps))
207  << "result = " << result << '\n'
208  << "expected = " << expected;
209  };
210  // pitch yaw roll x y z
211  compare({ 0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f});
212  compare({ 0.0f, 0.0f, 123.0f}, {1.0f, 0.0f, 0.0f});
213  compare({360.0f, 360.0f, 0.0f}, {1.0f, 0.0f, 0.0f});
214  compare({ 0.0f, 90.0f, 0.0f}, {0.0f, 1.0f, 0.0f});
215  compare({ 0.0f, -90.0f, 0.0f}, {0.0f,-1.0f, 0.0f});
216  compare({ 90.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 1.0f});
217  compare({180.0f, -90.0f, 0.0f}, {0.0f, 1.0f, 0.0f});
218 }
219 
220 TEST(geom, nearest_point_arc) {
221  ASSERT_NEAR(Math::DistanceArcToPoint(Vector3D(1,0,0),
222  Vector3D(0,0,0), 1.57f, 0, 1).second, 0.414214f, 0.01f);
223  ASSERT_NEAR(Math::DistanceArcToPoint(Vector3D(2,1,0),
224  Vector3D(0,0,0), 1.57f, 0, 1).second, 1.0f, 0.01f);
225  ASSERT_NEAR(Math::DistanceArcToPoint(Vector3D(0,1,0),
226  Vector3D(0,0,0), 1.57f, 0, 1).second, 1.0f, 0.01f);
227  ASSERT_NEAR(Math::DistanceArcToPoint(Vector3D(1,2,0),
228  Vector3D(0,0,0), 1.57f, 0, 1).second, 1.0f, 0.01f);
229 }
std::array< Location, 8 > GetLocalVertices() const
Returns the positions of the 8 vertices of this BoundingBox in local space.
static auto Distance(const Vector3D &a, const Vector3D &b)
Definition: Math.h:78
static std::pair< float, float > DistanceSegmentToPoint(const Vector3D &p, const Vector3D &v, const Vector3D &w)
Returns a pair containing:
Definition: Math.cpp:18
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
std::ostream & operator<<(std::ostream &out, const Vector3D &vector3D)
Definition: test_geom.cpp:18
static std::pair< float, float > DistanceArcToPoint(Vector3D p, Vector3D start_pos, float length, float heading, float curvature)
Returns a pair containing:
Definition: Math.cpp:33
geom::Vector3D Vector3D
Definition: rpc/Vector3D.h:14
geom::Location Location
Definition: rpc/Location.h:14
std::array< Location, 8 > GetWorldVertices(const Transform &in_bbox_to_world_tr) const
Returns the positions of the 8 vertices of this BoundingBox in world space.
TEST(geom, single_point_no_transform)
Definition: test_geom.cpp:28
void TransformPoint(Vector3D &in_point) const
Applies this transformation to in_point (first translation then rotation).
Vector3D GetForwardVector() const
Definition: Rotation.h:50
void InverseTransformPoint(Vector3D &in_point) const
Applies the inverse of this transformation to in_point.