CARLA
MeshFactory.cpp
Go to the documentation of this file.
1 // Copyright (c) 2020 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 
8 
9 #include <vector>
10 
11 #include <carla/geom/Vector3D.h>
12 #include <carla/geom/Rtree.h>
13 
14 namespace carla {
15 namespace geom {
16 
18  road_param.resolution = static_cast<float>(params.vertex_distance);
19  road_param.max_road_len = static_cast<float>(params.max_road_length);
20  road_param.extra_lane_width = static_cast<float>(params.additional_width);
21  road_param.wall_height = static_cast<float>(params.wall_height);
22  }
23 
24  /// We use this epsilon to shift the waypoints away from the edges of the lane
25  /// sections to avoid floating point precision errors.
26  static constexpr double EPSILON = 10.0 * std::numeric_limits<double>::epsilon();
27  static constexpr double MESH_EPSILON = 50.0 * std::numeric_limits<double>::epsilon();
28 
29  std::unique_ptr<Mesh> MeshFactory::Generate(const road::Road &road) const {
30  Mesh out_mesh;
31  for (auto &&lane_section : road.GetLaneSections()) {
32  out_mesh += *Generate(lane_section);
33  }
34  return std::make_unique<Mesh>(out_mesh);
35  }
36 
37  std::unique_ptr<Mesh> MeshFactory::Generate(const road::LaneSection &lane_section) const {
38  Mesh out_mesh;
39  for (auto &&lane_pair : lane_section.GetLanes()) {
40  out_mesh += *Generate(lane_pair.second);
41  }
42  return std::make_unique<Mesh>(out_mesh);
43  }
44 
45  std::unique_ptr<Mesh> MeshFactory::Generate(const road::Lane &lane) const {
46  const double s_start = lane.GetDistance() + EPSILON;
47  const double s_end = lane.GetDistance() + lane.GetLength() - EPSILON;
48  return Generate(lane, s_start, s_end);
49  }
50 
51  std::unique_ptr<Mesh> MeshFactory::Generate(
52  const road::Lane &lane, const double s_start, const double s_end) const {
54  DEBUG_ASSERT(s_start >= 0.0);
55  DEBUG_ASSERT(s_end <= lane.GetDistance() + lane.GetLength());
56  DEBUG_ASSERT(s_end >= EPSILON);
57  DEBUG_ASSERT(s_start < s_end);
58  // The lane with lane_id 0 have no physical representation in OpenDRIVE
59  Mesh out_mesh;
60  if (lane.GetId() == 0) {
61  return std::make_unique<Mesh>(out_mesh);
62  }
63  double s_current = s_start;
64 
65  std::vector<geom::Vector3D> vertices;
66  if (lane.IsStraight()) {
67  // Mesh optimization: If the lane is straight just add vertices at the
68  // begining and at the end of it
69  const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
70  vertices.push_back(edges.first);
71  vertices.push_back(edges.second);
72  } else {
73  // Iterate over the lane's 's' and store the vertices based on it's width
74  do {
75  // Get the location of the edges of the current lane at the current waypoint
76  const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
77  vertices.push_back(edges.first);
78  vertices.push_back(edges.second);
79 
80  // Update the current waypoint's "s"
81  s_current += road_param.resolution;
82  } while(s_current < s_end);
83  }
84 
85  // This ensures the mesh is constant and have no gaps between roads,
86  // adding geometry at the very end of the lane
87  if (s_end - (s_current - road_param.resolution) > EPSILON) {
88  const auto edges = lane.GetCornerPositions(s_end - MESH_EPSILON, road_param.extra_lane_width);
89  vertices.push_back(edges.first);
90  vertices.push_back(edges.second);
91  }
92 
93  // Add the adient material, create the strip and close the material
94  out_mesh.AddMaterial(
95  lane.GetType() == road::Lane::LaneType::Sidewalk ? "sidewalk" : "road");
96  out_mesh.AddTriangleStrip(vertices);
97  out_mesh.EndMaterial();
98  return std::make_unique<Mesh>(out_mesh);
99  }
100 
101  std::unique_ptr<Mesh> MeshFactory::GenerateWalls(const road::LaneSection &lane_section) const {
102  Mesh out_mesh;
103 
104  const auto min_lane = lane_section.GetLanes().begin()->first == 0 ?
105  1 : lane_section.GetLanes().begin()->first;
106  const auto max_lane = lane_section.GetLanes().rbegin()->first == 0 ?
107  -1 : lane_section.GetLanes().rbegin()->first;
108 
109  for (auto &&lane_pair : lane_section.GetLanes()) {
110  const auto &lane = lane_pair.second;
111  const double s_start = lane.GetDistance() + EPSILON;
112  const double s_end = lane.GetDistance() + lane.GetLength() - EPSILON;
113  if (lane.GetId() == max_lane) {
114  out_mesh += *GenerateLeftWall(lane, s_start, s_end);
115  }
116  if (lane.GetId() == min_lane) {
117  out_mesh += *GenerateRightWall(lane, s_start, s_end);
118  }
119  }
120  return std::make_unique<Mesh>(out_mesh);
121  }
122 
123  std::unique_ptr<Mesh> MeshFactory::GenerateRightWall(
124  const road::Lane &lane, const double s_start, const double s_end) const {
126  DEBUG_ASSERT(s_start >= 0.0);
127  DEBUG_ASSERT(s_end <= lane.GetDistance() + lane.GetLength());
128  DEBUG_ASSERT(s_end >= EPSILON);
129  DEBUG_ASSERT(s_start < s_end);
130  // The lane with lane_id 0 have no physical representation in OpenDRIVE
131  Mesh out_mesh;
132  if (lane.GetId() == 0) {
133  return std::make_unique<Mesh>(out_mesh);
134  }
135  double s_current = s_start;
136  const geom::Vector3D height_vector = geom::Vector3D(0.f, 0.f, road_param.wall_height);
137 
138  std::vector<geom::Vector3D> r_vertices;
139  if (lane.IsStraight()) {
140  // Mesh optimization: If the lane is straight just add vertices at the
141  // begining and at the end of it
142  const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
143  r_vertices.push_back(edges.first + height_vector);
144  r_vertices.push_back(edges.first);
145  } else {
146  // Iterate over the lane's 's' and store the vertices based on it's width
147  do {
148  // Get the location of the edges of the current lane at the current waypoint
149  const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
150  r_vertices.push_back(edges.first + height_vector);
151  r_vertices.push_back(edges.first);
152 
153  // Update the current waypoint's "s"
154  s_current += road_param.resolution;
155  } while(s_current < s_end);
156  }
157 
158  // This ensures the mesh is constant and have no gaps between roads,
159  // adding geometry at the very end of the lane
160  if (s_end - (s_current - road_param.resolution) > EPSILON) {
161  const auto edges = lane.GetCornerPositions(s_end - MESH_EPSILON, road_param.extra_lane_width);
162  r_vertices.push_back(edges.first + height_vector);
163  r_vertices.push_back(edges.first);
164  }
165 
166  // Add the adient material, create the strip and close the material
167  out_mesh.AddMaterial(
168  lane.GetType() == road::Lane::LaneType::Sidewalk ? "sidewalk" : "road");
169  out_mesh.AddTriangleStrip(r_vertices);
170  out_mesh.EndMaterial();
171  return std::make_unique<Mesh>(out_mesh);
172  }
173 
174  std::unique_ptr<Mesh> MeshFactory::GenerateLeftWall(
175  const road::Lane &lane, const double s_start, const double s_end) const {
177  DEBUG_ASSERT(s_start >= 0.0);
178  DEBUG_ASSERT(s_end <= lane.GetDistance() + lane.GetLength());
179  DEBUG_ASSERT(s_end >= EPSILON);
180  DEBUG_ASSERT(s_start < s_end);
181  // The lane with lane_id 0 have no physical representation in OpenDRIVE
182  Mesh out_mesh;
183  if (lane.GetId() == 0) {
184  return std::make_unique<Mesh>(out_mesh);
185  }
186  double s_current = s_start;
187  const geom::Vector3D height_vector = geom::Vector3D(0.f, 0.f, road_param.wall_height);
188 
189  std::vector<geom::Vector3D> l_vertices;
190  if (lane.IsStraight()) {
191  // Mesh optimization: If the lane is straight just add vertices at the
192  // begining and at the end of it
193  const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
194  l_vertices.push_back(edges.second);
195  l_vertices.push_back(edges.second + height_vector);
196  } else {
197  // Iterate over the lane's 's' and store the vertices based on it's width
198  do {
199  // Get the location of the edges of the current lane at the current waypoint
200  const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
201  l_vertices.push_back(edges.second);
202  l_vertices.push_back(edges.second + height_vector);
203 
204  // Update the current waypoint's "s"
205  s_current += road_param.resolution;
206  } while(s_current < s_end);
207  }
208 
209  // This ensures the mesh is constant and have no gaps between roads,
210  // adding geometry at the very end of the lane
211  if (s_end - (s_current - road_param.resolution) > EPSILON) {
212  const auto edges = lane.GetCornerPositions(s_end - MESH_EPSILON, road_param.extra_lane_width);
213  l_vertices.push_back(edges.second);
214  l_vertices.push_back(edges.second + height_vector);
215  }
216 
217  // Add the adient material, create the strip and close the material
218  out_mesh.AddMaterial(
219  lane.GetType() == road::Lane::LaneType::Sidewalk ? "sidewalk" : "road");
220  out_mesh.AddTriangleStrip(l_vertices);
221  out_mesh.EndMaterial();
222  return std::make_unique<Mesh>(out_mesh);
223  }
224 
225  std::vector<std::unique_ptr<Mesh>> MeshFactory::GenerateWithMaxLen(
226  const road::Road &road) const {
227  std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
228  for (auto &&lane_section : road.GetLaneSections()) {
229  auto section_uptr_list = GenerateWithMaxLen(lane_section);
230  mesh_uptr_list.insert(
231  mesh_uptr_list.end(),
232  std::make_move_iterator(section_uptr_list.begin()),
233  std::make_move_iterator(section_uptr_list.end()));
234  }
235  return mesh_uptr_list;
236  }
237 
238  std::vector<std::unique_ptr<Mesh>> MeshFactory::GenerateWithMaxLen(
239  const road::LaneSection &lane_section) const {
240  std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
241  if (lane_section.GetLength() < road_param.max_road_len) {
242  mesh_uptr_list.emplace_back(Generate(lane_section));
243  } else {
244  double s_current = lane_section.GetDistance() + EPSILON;
245  const double s_end = lane_section.GetDistance() + lane_section.GetLength() - EPSILON;
246  while(s_current + road_param.max_road_len < s_end) {
247  const auto s_until = s_current + road_param.max_road_len;
248  Mesh lane_section_mesh;
249  for (auto &&lane_pair : lane_section.GetLanes()) {
250  lane_section_mesh += *Generate(lane_pair.second, s_current, s_until);
251  }
252  mesh_uptr_list.emplace_back(std::make_unique<Mesh>(lane_section_mesh));
253  s_current = s_until;
254  }
255  if (s_end - s_current > EPSILON) {
256  Mesh lane_section_mesh;
257  for (auto &&lane_pair : lane_section.GetLanes()) {
258  lane_section_mesh += *Generate(lane_pair.second, s_current, s_end);
259  }
260  mesh_uptr_list.emplace_back(std::make_unique<Mesh>(lane_section_mesh));
261  }
262  }
263  return mesh_uptr_list;
264  }
265 
266  std::vector<std::unique_ptr<Mesh>> MeshFactory::GenerateWallsWithMaxLen(
267  const road::Road &road) const {
268  std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
269  for (auto &&lane_section : road.GetLaneSections()) {
270  auto section_uptr_list = GenerateWallsWithMaxLen(lane_section);
271  mesh_uptr_list.insert(
272  mesh_uptr_list.end(),
273  std::make_move_iterator(section_uptr_list.begin()),
274  std::make_move_iterator(section_uptr_list.end()));
275  }
276  return mesh_uptr_list;
277  }
278 
279  std::vector<std::unique_ptr<Mesh>> MeshFactory::GenerateWallsWithMaxLen(
280  const road::LaneSection &lane_section) const {
281  std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
282 
283  const auto min_lane = lane_section.GetLanes().begin()->first == 0 ?
284  1 : lane_section.GetLanes().begin()->first;
285  const auto max_lane = lane_section.GetLanes().rbegin()->first == 0 ?
286  -1 : lane_section.GetLanes().rbegin()->first;
287 
288  if (lane_section.GetLength() < road_param.max_road_len) {
289  mesh_uptr_list.emplace_back(GenerateWalls(lane_section));
290  } else {
291  double s_current = lane_section.GetDistance() + EPSILON;
292  const double s_end = lane_section.GetDistance() + lane_section.GetLength() - EPSILON;
293  while(s_current + road_param.max_road_len < s_end) {
294  const auto s_until = s_current + road_param.max_road_len;
295  Mesh lane_section_mesh;
296  for (auto &&lane_pair : lane_section.GetLanes()) {
297  const auto &lane = lane_pair.second;
298  if (lane.GetId() == max_lane) {
299  lane_section_mesh += *GenerateLeftWall(lane, s_current, s_until);
300  }
301  if (lane.GetId() == min_lane) {
302  lane_section_mesh += *GenerateRightWall(lane, s_current, s_until);
303  }
304  }
305  mesh_uptr_list.emplace_back(std::make_unique<Mesh>(lane_section_mesh));
306  s_current = s_until;
307  }
308  if (s_end - s_current > EPSILON) {
309  Mesh lane_section_mesh;
310  for (auto &&lane_pair : lane_section.GetLanes()) {
311  const auto &lane = lane_pair.second;
312  if (lane.GetId() == max_lane) {
313  lane_section_mesh += *GenerateLeftWall(lane, s_current, s_end);
314  }
315  if (lane.GetId() == min_lane) {
316  lane_section_mesh += *GenerateRightWall(lane, s_current, s_end);
317  }
318  }
319  mesh_uptr_list.emplace_back(std::make_unique<Mesh>(lane_section_mesh));
320  }
321  }
322  return mesh_uptr_list;
323  }
324 
325  std::vector<std::unique_ptr<Mesh>> MeshFactory::GenerateAllWithMaxLen(
326  const road::Road &road) const {
327  std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
328 
329  // Get road meshes
330  auto roads = GenerateWithMaxLen(road);
331  mesh_uptr_list.insert(
332  mesh_uptr_list.end(),
333  std::make_move_iterator(roads.begin()),
334  std::make_move_iterator(roads.end()));
335 
336  // Get wall meshes only if is not a junction
337  if (!road.IsJunction()) {
338  auto walls = GenerateWallsWithMaxLen(road);
339 
340  if (roads.size() == walls.size()) {
341  for (size_t i = 0; i < walls.size(); ++i) {
342  *mesh_uptr_list[i] += *walls[i];
343  }
344  } else {
345  mesh_uptr_list.insert(
346  mesh_uptr_list.end(),
347  std::make_move_iterator(walls.begin()),
348  std::make_move_iterator(walls.end()));
349  }
350  }
351 
352  return mesh_uptr_list;
353  }
354 
355  struct VertexWeight {
357  double weight;
358  };
361  std::vector<VertexWeight> neighbors;
362  };
363  struct VertexInfo {
366  bool is_static;
367  };
368 
369  // Helper function to compute the weight of neighboring vertices
372  const VertexInfo &vertex_info,
373  const VertexInfo &neighbor_info) {
374  const float distance3D = geom::Math::Distance(*vertex_info.vertex, *neighbor_info.vertex);
375  // Ignore vertices beyond a certain distance
376  if(distance3D > road_param.max_weight_distance) {
377  return {neighbor_info.vertex, 0};
378  }
379  if(abs(distance3D) < EPSILON) {
380  return {neighbor_info.vertex, 0};
381  }
382  float weight = geom::Math::Clamp<float>(1.0f / distance3D, 0.0f, 100000.0f);
383 
384  // Additional weight to vertices in the same lane
385  if(vertex_info.lane_mesh_idx == neighbor_info.lane_mesh_idx) {
386  weight *= road_param.same_lane_weight_multiplier;
387  // Further additional weight for fixed verices
388  if(neighbor_info.is_static) {
389  weight *= road_param.lane_ends_multiplier;
390  }
391  }
392  return {neighbor_info.vertex, weight};
393  }
394 
395  // Helper function to compute neighborhoord of vertices and their weights
396  std::vector<VertexNeighbors> GetVertexNeighborhoodAndWeights(
398  std::vector<std::unique_ptr<Mesh>> &lane_meshes) {
399  // Build rtree for neighborhood queries
400  using Rtree = geom::PointCloudRtree<VertexInfo>;
401  using Point = Rtree::BPoint;
402  Rtree rtree;
403  for (size_t lane_mesh_idx = 0; lane_mesh_idx < lane_meshes.size(); ++lane_mesh_idx) {
404  auto& mesh = lane_meshes[lane_mesh_idx];
405  for(size_t i = 0; i < mesh->GetVerticesNum(); ++i) {
406  auto& vertex = mesh->GetVertices()[i];
407  Point point(vertex.x, vertex.y, vertex.z);
408  if (i < 2 || i >= mesh->GetVerticesNum() - 2) {
409  rtree.InsertElement({point, {&vertex, lane_mesh_idx, true}});
410  } else {
411  rtree.InsertElement({point, {&vertex, lane_mesh_idx, false}});
412  }
413  }
414  }
415 
416  // Find neighbors for each vertex and compute their weight
417  std::vector<VertexNeighbors> vertices_neighborhoods;
418  for (size_t lane_mesh_idx = 0; lane_mesh_idx < lane_meshes.size(); ++lane_mesh_idx) {
419  auto& mesh = lane_meshes[lane_mesh_idx];
420  for(size_t i = 0; i < mesh->GetVerticesNum(); ++i) {
421  if (i > 2 && i < mesh->GetVerticesNum() - 2) {
422  auto& vertex = mesh->GetVertices()[i];
423  Point point(vertex.x, vertex.y, vertex.z);
424  auto closest_vertices = rtree.GetNearestNeighbours(point, 20);
425  VertexNeighbors vertex_neighborhood;
426  vertex_neighborhood.vertex = &vertex;
427  for(auto& close_vertex : closest_vertices) {
428  auto &vertex_info = close_vertex.second;
429  if(&vertex == vertex_info.vertex) {
430  continue;
431  }
432  auto vertex_weight = ComputeVertexWeight(
433  road_param, {&vertex, lane_mesh_idx, false}, vertex_info);
434  if(vertex_weight.weight > 0)
435  vertex_neighborhood.neighbors.push_back(vertex_weight);
436  }
437  vertices_neighborhoods.push_back(vertex_neighborhood);
438  }
439  }
440  }
441  return vertices_neighborhoods;
442  }
443 
444  std::unique_ptr<Mesh> MeshFactory::MergeAndSmooth(std::vector<std::unique_ptr<Mesh>> &lane_meshes) const {
445  geom::Mesh out_mesh;
446 
447  auto vertices_neighborhoods = GetVertexNeighborhoodAndWeights(road_param, lane_meshes);
448 
449  // Laplacian function
450  auto Laplacian = [&](const Mesh::vertex_type* vertex, const std::vector<VertexWeight> &neighbors) -> double {
451  double sum = 0;
452  double sum_weight = 0;
453  for(auto &element : neighbors) {
454  sum += (element.vertex->z - vertex->z)*element.weight;
455  sum_weight += element.weight;
456  }
457  if(sum_weight > 0)
458  return sum / sum_weight;
459  else
460  return 0;
461  };
462  // Run iterative algorithm
463  double lambda = 0.5;
464  int iterations = 100;
465  for(int iter = 0; iter < iterations; ++iter) {
466  for (auto& vertex_neighborhood : vertices_neighborhoods) {
467  auto * vertex = vertex_neighborhood.vertex;
468  vertex->z += static_cast<float>(lambda*Laplacian(vertex, vertex_neighborhood.neighbors));
469  }
470  }
471 
472  for(auto &mesh : lane_meshes) {
473  out_mesh += *mesh;
474  }
475 
476  return std::make_unique<Mesh>(out_mesh);
477  }
478 
479 } // namespace geom
480 } // namespace carla
static auto Distance(const Vector3D &a, const Vector3D &b)
Definition: Math.h:74
LaneType GetType() const
Definition: Lane.cpp:38
Seting for map generation from opendrive without additional geometry.
double GetDistance() const
Definition: LaneSection.cpp:13
static VertexWeight ComputeVertexWeight(const MeshFactory::RoadParameters &road_param, const VertexInfo &vertex_info, const VertexInfo &neighbor_info)
std::unique_ptr< Mesh > Generate(const road::Road &road) const
Generates a mesh that defines a road.
Definition: MeshFactory.cpp:29
std::unique_ptr< Mesh > MergeAndSmooth(std::vector< std::unique_ptr< Mesh >> &lane_meshes) const
bool IsJunction() const
Definition: Road.cpp:42
std::vector< VertexNeighbors > GetVertexNeighborhoodAndWeights(const MeshFactory::RoadParameters &road_param, std::vector< std::unique_ptr< Mesh >> &lane_meshes)
double GetDistance() const
Definition: Lane.cpp:46
double GetLength() const
Definition: Lane.cpp:51
std::unique_ptr< Mesh > GenerateRightWall(const road::Lane &lane, const double s_start, const double s_end) const
Generates a wall-like mesh at the right side of the lane.
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
Mesh::vertex_type * vertex
double GetLength() const
Definition: LaneSection.cpp:17
geom::Vector3D Vector3D
Definition: rpc/Vector3D.h:14
void AddMaterial(const std::string &material_name)
Starts applying a new material to the new added triangles.
Definition: Mesh.cpp:107
void EndMaterial()
Stops applying the material to the new added triangles.
Definition: Mesh.cpp:123
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
auto GetLaneSections() const
Definition: Road.h:127
RoadParameters road_param
Definition: MeshFactory.h:103
std::vector< std::unique_ptr< Mesh > > GenerateWithMaxLen(const road::Road &road) const
Generates a list of meshes that defines a road with a maximum length.
#define RELEASE_ASSERT(pred)
Definition: Debug.h:84
MeshFactory(rpc::OpendriveGenerationParameters params=rpc::OpendriveGenerationParameters())
Definition: MeshFactory.cpp:17
std::vector< VertexWeight > neighbors
void AddTriangleStrip(const std::vector< vertex_type > &vertices)
Adds a triangle strip to the mesh, vertex order is counterclockwise.
Definition: Mesh.cpp:45
std::unique_ptr< Mesh > GenerateLeftWall(const road::Lane &lane, const double s_start, const double s_end) const
Generates a wall-like mesh at the left side of the lane.
Mesh data container, validator and exporter.
Definition: Mesh.h:42
std::unique_ptr< Mesh > GenerateWalls(const road::LaneSection &lane_section) const
Genrates a mesh representing a wall on the road corners to avoid cars falling down.
static constexpr double MESH_EPSILON
Definition: MeshFactory.cpp:27
std::map< LaneId, Lane > & GetLanes()
Definition: LaneSection.cpp:47
bool IsStraight() const
Checks whether the geometry is straight or not.
Definition: Lane.cpp:65
std::vector< std::unique_ptr< Mesh > > GenerateWallsWithMaxLen(const road::Road &road) const
Generates a list of meshes that defines a road safety wall with a maximum length. ...
std::vector< std::unique_ptr< Mesh > > GenerateAllWithMaxLen(const road::Road &road) const
Generates a chunked road with all the features needed for simulation.
static constexpr double EPSILON
We use this epsilon to shift the waypoints away from the edges of the lane sections to avoid floating...
Definition: MeshFactory.cpp:26
Parameters for the road generation.
Definition: MeshFactory.h:92
Mesh::vertex_type * vertex
Rtree class working with 3D point clouds.
Definition: Rtree.h:22
Mesh::vertex_type * vertex
std::pair< geom::Vector3D, geom::Vector3D > GetCornerPositions(const double s, const float extra_width=0.f) const
Computes the location of the edges given a s.
Definition: Lane.cpp:204
LaneId GetId() const
Definition: Lane.cpp:34