p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added support for nearest neighbour for all geometries. Added geometry poly3.

This commit is contained in:
Axel 2020-01-31 16:57:25 +01:00 committed by Marc Garcia Puig
parent ae2ae92bf1
commit aa78942489
12 changed files with 641 additions and 324 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
LibCarla/source/carla/geom/CubicPolynomial.h
View File

@ -29,24 +29,24 @@ namespace geom {
CubicPolynomial(const CubicPolynomial &) = default;
CubicPolynomial(
const value_type &a,
const value_type &b,
const value_type &c,
const value_type &d)
const value_type &a,
const value_type &b,
const value_type &c,
const value_type &d)
: _v{ {a, b, c, d} },
_s(0.0) {}
_s(0.0) {}
CubicPolynomial(
const value_type &a,
const value_type &b,
const value_type &c,
const value_type &d,
const value_type &s) // lateral offset
const value_type &a,
const value_type &b,
const value_type &c,
const value_type &d,
const value_type &s) // lateral offset
: _v{ {a - b * s + c * s * s - d * s * s * s,
b - 2 * c * s + 3 * d * s * s,
c - 3 * d * s,
d} },
_s(s) {}
b - 2 * c * s + 3 * d * s * s,
c - 3 * d * s,
d} },
_s(s) {}
// =========================================================================
// -- Getters --------------------------------------------------------------
@ -77,11 +77,11 @@ namespace geom {
// =========================================================================
void Set(
const value_type &a,
const value_type &b,
const value_type &c,
const value_type &d,
const value_type &s) { // lateral offset
const value_type &a,
const value_type &b,
const value_type &c,
const value_type &d,
const value_type &s) { // lateral offset
_v = { a - b * s + c * s * s - d * s * s * s,
b - 2 * c * s + 3 * d * s * s,
c - 3 * d * s,
@ -89,6 +89,15 @@ namespace geom {
_s = s;
}
void Set(
const value_type &a,
const value_type &b,
const value_type &c,
const value_type &d) {
_v = {{a, b, c, d}};
_s = (0.0);
}
// =========================================================================
// -- Evaluate methods -----------------------------------------------------
// =========================================================================

36
LibCarla/source/carla/geom/Math.cpp
View File

@ -11,10 +11,14 @@
namespace carla {
namespace geom {
double Math::GetVectorAngle(const Vector3D &a, const Vector3D &b) {
return std::acos(Dot(a, b) / (a.Length() * b.Length()));
}
std::pair<float, float> Math::DistanceSegmentToPoint(
const Vector3D &p,
const Vector3D &v,
const Vector3D &w) {
const Vector3D &p,
const Vector3D &v,
const Vector3D &w) {
const float l2 = DistanceSquared2D(v, w);
const float l = std::sqrt(l2);
if (l2 == 0.0f) {
@ -27,11 +31,11 @@ namespace geom {
}
std::pair<float, float> Math::DistanceArcToPoint(
Vector3D p,
Vector3D start_pos,
const float length,
float heading, // [radians]
float curvature) {
Vector3D p,
Vector3D start_pos,
const float length,
float heading, // [radians]
float curvature) {
/// @todo: Because Unreal's coordinates, hacky way to correct
/// the -y, this must be changed in the future
@ -87,21 +91,21 @@ namespace geom {
DEBUG_ASSERT(angle >= 0.0f);
if (angle <= last_point_angle) {
return std::make_pair(
angle * radius,
Distance2D(intersection, rotated_p));
angle * radius,
Distance2D(intersection, rotated_p));
}
// find the nearest point, start or end to intersection
const float start_dist = Distance2D(Vector3D(), rotated_p);
const Vector3D end_pos(
radius * std::cos(last_point_angle - pi_half),
radius * std::sin(last_point_angle - pi_half) + circ_center.y,
0.0f);
radius * std::cos(last_point_angle - pi_half),
radius * std::sin(last_point_angle - pi_half) + circ_center.y,
0.0f);
const float end_dist = Distance2D(end_pos, rotated_p);
return (start_dist < end_dist) ?
std::make_pair(0.0f, start_dist) :
std::make_pair(length, end_dist);
std::make_pair(0.0f, start_dist) :
std::make_pair(length, end_dist);
}
Vector3D Math::RotatePointOnOrigin2D(Vector3D p, float angle) {
@ -115,7 +119,7 @@ namespace geom {
const float sp = std::sin(ToRadians(rotation.pitch));
const float cy = std::cos(ToRadians(rotation.yaw));
const float sy = std::sin(ToRadians(rotation.yaw));
return {cy * cp, sy * cp, sp};
return {cy *cp, sy *cp, sp};
}
} // namespace geom

2
LibCarla/source/carla/geom/Math.h
View File

@ -79,6 +79,8 @@ namespace geom {
return std::sqrt(DistanceSquared2D(a, b));
}
static double GetVectorAngle(const Vector3D &a, const Vector3D &b);
/// Returns a pair containing:
/// - @b first: distance from v to p' where p' = p projected on segment
/// (w - v)

122
LibCarla/source/carla/geom/Rtree.h Normal file
View File

@ -0,0 +1,122 @@
// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
// de Barcelona (UAB).
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#pragma once
#include <vector>
#include <boost/optional.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/index/rtree.hpp>
namespace carla {
namespace geom {
// Rtree class working with 3D point clouds.
// Asociates a T element with a 3D point
// Useful to perform fast k-NN searches
template <typename T>
class PointCloudRtree{
public:
typedef boost::geometry::model::point<float, 3, boost::geometry::cs::cartesian> BPoint;
typedef std::pair<BPoint, T> TreeElement;
void InsertElement(const BPoint &point, const T &element) {
_rtree.insert(std::make_pair(point, element));
}
void InsertElement(const TreeElement &element) {
_rtree.insert(element);
}
void InsertElements(const std::vector<TreeElement> &elements) {
_rtree.insert(elements.begin(), elements.end());
}
// Return nearest neighbors with a user defined filter.
// The filter reveices as an argument a TreeElement value and needs to
// return a bool to accept or reject the value
// [&](Rtree::TreeElement const &element){if (IsOk(element)) return true;
// else return false;}
template <typename Filter>
std::vector<TreeElement> GetNearestNeighboursWithFilter(const BPoint &point, Filter filter,
size_t number_neighbours = 1) const {
std::vector<TreeElement> query_result;
_rtree.query(boost::geometry::index::nearest(point,
static_cast<unsigned int>(number_neighbours)) && boost::geometry::index::satisfies(filter),
std::back_inserter(query_result));
return query_result;
}
std::vector<TreeElement> GetNearestNeighbours(const BPoint &point, size_t number_neighbours = 1) const {
std::vector<TreeElement> query_result;
_rtree.query(boost::geometry::index::nearest(point, static_cast<unsigned int>(number_neighbours)),
std::back_inserter(query_result));
return query_result;
}
size_t GetTreeSize() const { return _rtree.size();
}
private:
boost::geometry::index::rtree<TreeElement, boost::geometry::index::linear<16>> _rtree;
};
// Rtree class working with 3D segment clouds.
// Stores a pair of T elements (one for each end of the segment)
// Useful to perform fast k-NN searches.
template <typename T>
class SegmentCloudRtree {
public:
typedef boost::geometry::model::point<float, 3, boost::geometry::cs::cartesian> BPoint;
typedef boost::geometry::model::segment<BPoint> BSegment;
typedef std::pair<BSegment, std::pair<T, T>> TreeElement;
void InsertElement(const BSegment &segment, const T &element_start, const T &element_end) {
_rtree.insert(std::make_pair(segment, std::make_pair(element_start, element_end)));
}
void InsertElement(const TreeElement &element) {
_rtree.insert(element);
}
void InsertElements(const std::vector<TreeElement> &elements) {
_rtree.insert(elements.begin(), elements.end());
}
// Return nearest neighbors with a user defined filter.
// The filter reveices as an argument a TreeElement value and needs to
// return a bool to accept or reject the value
// [&](Rtree::TreeElement const &element){if (IsOk(element)) return true;
// else return false;}
template <typename Filter>
std::vector<TreeElement> GetNearestNeighboursWithFilter(const BPoint &point, Filter filter,
size_t number_neighbours = 1) const {
std::vector<TreeElement> query_result;
_rtree.query(boost::geometry::index::nearest(point,
static_cast<unsigned int>(number_neighbours)) && boost::geometry::index::satisfies(filter),
std::back_inserter(query_result));
return query_result;
}
std::vector<TreeElement> GetNearestNeighbours(const BPoint &point, size_t number_neighbours = 1) const {
std::vector<TreeElement> query_result;
_rtree.query(boost::geometry::index::nearest(point,
static_cast<unsigned int>(number_neighbours)),
std::back_inserter(query_result));
return query_result;
}
size_t GetTreeSize() const { return _rtree.size();
}
private:
boost::geometry::index::rtree<TreeElement, boost::geometry::index::linear<16>> _rtree;
};
} // namespace geom
} // namespace carla

6
LibCarla/source/carla/road/Lane.h
View File

@ -79,6 +79,12 @@ namespace road {
return _info.GetInfo<T>(s);
}
template <typename T>
std::vector<const T*> GetInfos() const {
DEBUG_ASSERT(_lane_section != nullptr);
return _info.GetInfos<T>();
}
const std::vector<Lane *> &GetNextLanes() const {
return _next_lanes;
}

365
LibCarla/source/carla/road/Map.cpp
View File

@ -13,6 +13,7 @@
#include "carla/road/element/RoadInfoMarkRecord.h"
#include "carla/road/element/RoadInfoLaneOffset.h"
#include "carla/road/element/RoadObjectCrosswalk.h"
#include "carla/road/element/RoadInfoElevation.h"
#include "carla/geom/Math.h"
#include <stdexcept>
@ -36,9 +37,9 @@ namespace road {
return ConcatVectors(src, dst);
}
dst.insert(
dst.end(),
std::make_move_iterator(src.begin()),
std::make_move_iterator(src.end()));
dst.end(),
std::make_move_iterator(src.begin()),
std::make_move_iterator(src.end()));
return dst;
}
@ -61,18 +62,18 @@ namespace road {
/// Return a waypoint for each drivable lane on @a lane_section.
template <typename FuncT>
static void ForEachDrivableLaneImpl(
RoadId road_id,
const LaneSection &lane_section,
double distance,
FuncT &&func) {
RoadId road_id,
const LaneSection &lane_section,
double distance,
FuncT && func) {
for (const auto &pair : lane_section.GetLanes()) {
const auto &lane = pair.second;
if ((static_cast<uint32_t>(lane.GetType()) & static_cast<uint32_t>(Lane::LaneType::Driving)) > 0) {
std::forward<FuncT>(func)(Waypoint{
road_id,
lane_section.GetId(),
lane.GetId(),
distance < 0.0 ? GetDistanceAtStartOfLane(lane) : distance});
road_id,
lane_section.GetId(),
lane.GetId(),
distance < 0.0 ? GetDistanceAtStartOfLane(lane) : distance});
}
}
}
@ -98,7 +99,7 @@ namespace road {
/// Return a waypoint for each drivable lane on each lane section of @a road.
template <typename FuncT>
static void ForEachDrivableLane(const Road &road, FuncT &&func) {
static void ForEachDrivableLane(const Road &road, FuncT && func) {
for (const auto &lane_section : road.GetLaneSections()) {
ForEachDrivableLaneImpl(
road.GetId(),
@ -123,13 +124,13 @@ namespace road {
/// Return a waypoint for each drivable lane at @a distance on @a road.
template <typename FuncT>
static void ForEachDrivableLaneAt(const Road &road, double distance, FuncT &&func) {
static void ForEachDrivableLaneAt(const Road &road, double distance, FuncT && func) {
for (const auto &lane_section : road.GetLaneSectionsAt(distance)) {
ForEachDrivableLaneImpl(
road.GetId(),
lane_section,
distance,
std::forward<FuncT>(func));
road.GetId(),
lane_section,
distance,
std::forward<FuncT>(func));
}
}
@ -137,9 +138,9 @@ namespace road {
/// for an specific Lane given an s and a iterator over lanes
template <typename T>
static std::pair<double, double> ComputeTotalLaneWidth(
const T container,
const double s,
const LaneId lane_id) {
const T container,
const double s,
const LaneId lane_id) {
// lane_id can't be 0
RELEASE_ASSERT(lane_id != 0);
@ -177,87 +178,38 @@ namespace road {
// ===========================================================================
boost::optional<Waypoint> Map::GetClosestWaypointOnRoad(
const geom::Location &pos,
uint32_t lane_type) const {
// max_nearests represents the max nearests roads
// where we will search for nearests lanes
constexpr size_t max_nearests = 50u;
// in case that map has less than max_nearests lanes,
// we will use the maximum lanes
const size_t max_nearest_allowed = std::min(_data.GetRoadCount(), max_nearests);
const geom::Location &pos,
uint32_t lane_type) const {
std::vector<Rtree::TreeElement> query_result =
_rtree.GetNearestNeighboursWithFilter(Rtree::BPoint(pos.x, pos.y, pos.z),
[&](Rtree::TreeElement const &element) {
const Lane &lane = GetLane(element.second.first);
return (lane_type & static_cast<uint32_t>(lane.GetType())) > 0;
});
// Unreal's Y axis hack
const auto pos_inverted_y = geom::Location(pos.x, -pos.y, pos.z);
double nearest_dist[max_nearests];
std::fill(nearest_dist, nearest_dist + max_nearest_allowed,
std::numeric_limits<double>::max());
RoadId ids[max_nearests];
std::fill(ids, ids + max_nearest_allowed, 0);
double dists[max_nearests];
std::fill(dists, dists + max_nearest_allowed, 0.0);
for (const auto &road_pair : _data.GetRoads()) {
const auto road = &road_pair.second;
const auto current_dist = road->GetNearestPoint(pos_inverted_y);
for (size_t i = 0u; i < max_nearest_allowed; ++i) {
if (current_dist.second < nearest_dist[i]) {
// reorder nearest_dist
for (size_t j = max_nearest_allowed - 1u; j > i; --j) {
DEBUG_ASSERT(j > 0u);
nearest_dist[j] = nearest_dist[j - 1];
ids[j] = ids[j - 1];
dists[j] = dists[j - 1];
}
nearest_dist[i] = current_dist.second;
ids[i] = road->GetId();
dists[i] = current_dist.first;
break;
}
}
}
// search for the nearest lane in nearest_dist
Waypoint waypoint;
auto nearest_lane_dist = std::numeric_limits<double>::max();
for (size_t i = 0u; i < max_nearest_allowed; ++i) {
auto lane_dist = _data.GetRoad(ids[i]).GetNearestLane(dists[i], pos_inverted_y, lane_type);
if (lane_dist.second < nearest_lane_dist) {
nearest_lane_dist = lane_dist.second;
waypoint.lane_id = lane_dist.first->GetId();
waypoint.road_id = ids[i];
waypoint.s = dists[i];
}
}
if (nearest_lane_dist == std::numeric_limits<double>::max()) {
if (query_result.size() == 0) {
return boost::optional<Waypoint>{};
}
const auto &road = _data.GetRoad(waypoint.road_id);
Rtree::BSegment segment = query_result.front().first;
Rtree::BPoint s1 = segment.first;
Rtree::BPoint s2 = segment.second;
auto distance_to_segment = geom::Math::DistanceSegmentToPoint(pos,
geom::Vector3D(s1.get<0>(), s1.get<1>(), s1.get<2>()),
geom::Vector3D(s2.get<0>(), s2.get<1>(), s2.get<2>()));
// Make sure 0.0 < waipoint.s < Road's length
constexpr double margin = 5.0 * EPSILON;
DEBUG_ASSERT(margin < road.GetLength() - margin);
waypoint.s = geom::Math::Clamp(waypoint.s, margin, road.GetLength() - margin);
Waypoint result = query_result.front().second.first;
auto &lane = road.GetLaneByDistance(waypoint.s, waypoint.lane_id);
const auto lane_section = lane.GetLaneSection();
RELEASE_ASSERT(lane_section != nullptr);
const auto lane_section_id = lane_section->GetId();
waypoint.section_id = lane_section_id;
return waypoint;
if (distance_to_segment.first <= 0) {
return result;
} else {
return GetNext(result, distance_to_segment.first).front();
}
}
boost::optional<Waypoint> Map::GetWaypoint(
const geom::Location &pos,
uint32_t lane_type) const {
const geom::Location &pos,
uint32_t lane_type) const {
boost::optional<Waypoint> w = GetClosestWaypointOnRoad(pos, lane_type);
if (!w.has_value()) {
@ -267,7 +219,7 @@ namespace road {
const auto dist = geom::Math::Distance2D(ComputeTransform(*w).location, pos);
const auto lane_width_info = GetLane(*w).GetInfo<RoadInfoLaneWidth>(w->s);
const auto half_lane_width =
lane_width_info->GetPolynomial().Evaluate(w->s) * 0.5;
lane_width_info->GetPolynomial().Evaluate(w->s) * 0.5;
if (dist < half_lane_width) {
return w;
@ -340,26 +292,27 @@ namespace road {
const auto side_lanes = MakeListView(
std::make_reverse_iterator(lanes.lower_bound(0)), lanes.rend());
const auto computed_width =
ComputeTotalLaneWidth(side_lanes, waypoint.s, waypoint.lane_id);
ComputeTotalLaneWidth(side_lanes, waypoint.s, waypoint.lane_id);
lane_width = static_cast<float>(computed_width.first);
lane_tangent = static_cast<float>(computed_width.second);
} else if (waypoint.lane_id > 0) {
// left lane
const auto side_lanes = MakeListView(lanes.lower_bound(1), lanes.end());
const auto computed_width =
ComputeTotalLaneWidth(side_lanes, waypoint.s, waypoint.lane_id);
ComputeTotalLaneWidth(side_lanes, waypoint.s, waypoint.lane_id);
lane_width = static_cast<float>(computed_width.first);
lane_tangent = static_cast<float>(computed_width.second);
}
//log_warning("Got to computetransform, road: ", road.GetId(), " ", road.GetLength());
// log_warning("Got to computetransform, road: ", road.GetId(), " ",
// road.GetLength());
// get a directed point in s and apply the computed lateral offet
DirectedPoint dp = road.GetDirectedPointIn(waypoint.s);
// compute the tangent of the laneOffset
const auto lane_offset_info = road.GetInfo<RoadInfoLaneOffset>(waypoint.s);
float lane_offset_tangent = 0;
if(lane_offset_info){
if (lane_offset_info) {
lane_offset_tangent = static_cast<float>(lane_offset_info->GetPolynomial().Tangent(waypoint.s));
}
@ -369,9 +322,10 @@ namespace road {
lane_tangent *= -1;
geom::Rotation rot(
geom::Math::ToDegrees(static_cast<float>(dp.pitch)),
geom::Math::ToDegrees(-static_cast<float>(dp.tangent)), // Unreal's Y axis hack
0.0f);
geom::Math::ToDegrees(static_cast<float>(dp.pitch)),
geom::Math::ToDegrees(-static_cast<float>(dp.tangent)), // Unreal's Y
// axis hack
0.0f);
dp.ApplyLateralOffset(lane_width);
@ -430,8 +384,8 @@ namespace road {
RELEASE_ASSERT(s <= current_lane.GetRoad()->GetLength());
const auto inner_lane_id = waypoint.lane_id < 0 ?
waypoint.lane_id + 1 :
waypoint.lane_id - 1;
waypoint.lane_id + 1 :
waypoint.lane_id - 1;
const auto &inner_lane = current_lane.GetRoad()->GetLaneById(waypoint.section_id, inner_lane_id);
@ -442,15 +396,15 @@ namespace road {
}
std::vector<LaneMarking> Map::CalculateCrossedLanes(
const geom::Location &origin,
const geom::Location &destination) const {
const geom::Location &origin,
const geom::Location &destination) const {
return LaneCrossingCalculator::Calculate(*this, origin, destination);
}
std::vector<geom::Location> Map::GetAllCrosswalkZones() const {
std::vector<geom::Location> result;
for (const auto &pair : _data.GetRoads()) {
for (const auto &pair : _data.GetRoads()) {
const auto &road = pair.second;
std::vector<const RoadObjectCrosswalk *> crosswalks = road.GetObjects<RoadObjectCrosswalk>();
if (crosswalks.size() > 0) {
@ -477,7 +431,8 @@ namespace road {
// move perpendicular ('t')
geom::Transform pivot = base;
pivot.rotation.yaw -= geom::Math::ToDegrees<float>(static_cast<float>(crosswalk->GetHeading()));
pivot.rotation.yaw -= 90; // move perpendicular to 's' for the lateral offset
pivot.rotation.yaw -= 90; // move perpendicular to 's' for the
// lateral offset
geom::Vector3D v(static_cast<float>(crosswalk->GetT()), 0.0f, 0.0f);
pivot.TransformPoint(v);
// restore pivot position and orientation
@ -487,12 +442,15 @@ namespace road {
// calculate all the corners
for (auto corner : crosswalk->GetPoints()) {
geom::Vector3D v2(static_cast<float>(corner.u), static_cast<float>(corner.v), static_cast<float>(corner.z));
// set the width larger to contact with the sidewalk (in case they have gutter area)
if (corner.u < 0)
geom::Vector3D v2(static_cast<float>(corner.u), static_cast<float>(corner.v),
static_cast<float>(corner.z));
// set the width larger to contact with the sidewalk (in case they
// have gutter area)
if (corner.u < 0) {
v2.x -= 1.0f;
else
} else {
v2.x += 1.0f;
}
pivot.TransformPoint(v2);
result.push_back(v2);
}
@ -543,8 +501,8 @@ namespace road {
}
std::vector<Waypoint> Map::GetNext(
const Waypoint waypoint,
const double distance) const {
const Waypoint waypoint,
const double distance) const {
RELEASE_ASSERT(distance > 0.0);
const auto &lane = GetLane(waypoint);
const bool forward = (waypoint.lane_id <= 0);
@ -567,9 +525,9 @@ namespace road {
std::vector<Waypoint> result;
for (const auto &successor : GetSuccessors(waypoint)) {
DEBUG_ASSERT(
successor.road_id != waypoint.road_id ||
successor.section_id != waypoint.section_id ||
successor.lane_id != waypoint.lane_id);
successor.road_id != waypoint.road_id ||
successor.section_id != waypoint.section_id ||
successor.lane_id != waypoint.lane_id);
result = ConcatVectors(result, GetNext(successor, distance - remaining_lane_length));
}
return result;
@ -663,7 +621,8 @@ namespace road {
for (const auto &lane : lane_section.GetLanes()) {
// add only the left (positive) lanes
if (lane.first > 0 && lane.second.GetType() == Lane::LaneType::Driving) {
result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(),
road_len });
}
}
}
@ -708,6 +667,180 @@ namespace road {
return _data.GetRoad(waypoint.road_id).GetLaneById(waypoint.section_id, waypoint.lane_id);
}
void Map::CreateRtree() {
const double epsilon = 0.000001;
const double min_delta_s = 1;
const double angle_threshold = geom::Math::Pi<double>() / 20.0; // 9 degrees
std::vector<Waypoint> topology;
for (const auto &pair : _data.GetRoads()) {
const auto &road = pair.second;
// right lanes start at s 0
for (const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
for (const auto &lane : lane_section.GetLanes()) {
// add only the right (negative) lanes
if (lane.first < 0 && lane.second.GetType() != Lane::LaneType::None) {
topology.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
}
}
}
// left lanes start at s max
const auto road_len = road.GetLength();
for (const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
for (const auto &lane : lane_section.GetLanes()) {
// add only the left (positive) lanes
if (lane.first > 0 && lane.second.GetType() != Lane::LaneType::None) {
topology.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(),
road_len });
}
}
}
}
std::vector<Rtree::TreeElement> rtree_elements;
for (auto &waypoint : topology) {
auto &lane_start_waypoint = waypoint;
auto current_waypoint = lane_start_waypoint;
const Lane &lane = GetLane(current_waypoint);
const Road &road = _data.GetRoad(current_waypoint.road_id);
while (current_waypoint.s <= road.GetLength() && current_waypoint.s >= 0) {
bool shouldBreak = false;
const auto *geometry = road.GetInfo<element::RoadInfoGeometry>(current_waypoint.s);
double geometry_start_s = geometry->GetGeometry().GetStartOffset();
double geometry_end_s = geometry_start_s + geometry->GetGeometry().GetLength();
double delta_s = min_delta_s;
geom::Transform current_transform = ComputeTransform(current_waypoint);
element::GeometryType geometry_type = geometry->GetGeometry().GetType();
bool is_straight = true;
switch (geometry_type) {
case element::GeometryType::LINE: {
auto lane_offsets = lane.GetInfos<element::RoadInfoLaneOffset>();
for (auto *lane_offset : lane_offsets) {
if (abs(lane_offset->GetPolynomial().GetC()) > 0 ||
abs(lane_offset->GetPolynomial().GetD()) > 0) {
is_straight = false;
break;
}
}
auto elevations = road.GetInfos<element::RoadInfoElevation>();
for (auto *elevation : elevations) {
if (abs(elevation->GetPolynomial().GetC()) > 0 ||
abs(elevation->GetPolynomial().GetD()) > 0) {
is_straight = false;
break;
}
}
}
if (is_straight) {
if (lane.GetId() < 0) {
delta_s = geometry_end_s - current_waypoint.s;
} else {
delta_s = current_waypoint.s - geometry_start_s;
}
delta_s -= epsilon;
if (delta_s < epsilon) {
break;
}
auto next = GetNext(current_waypoint, delta_s);
RELEASE_ASSERT(next.size() == 1);
RELEASE_ASSERT(next.front().road_id == current_waypoint.road_id);
auto next_waypoint = next.front();
geom::Transform next_transform = ComputeTransform(next_waypoint);
Rtree::BPoint init =
Rtree::BPoint(current_transform.location.x, current_transform.location.y,
current_transform.location.z);
Rtree::BPoint end =
Rtree::BPoint(next_transform.location.x, next_transform.location.y, next_transform.location.z);
rtree_elements.emplace_back(std::make_pair(Rtree::BSegment(init, end),
std::make_pair(current_waypoint, next_waypoint)));
current_waypoint = next_waypoint;
break;
}
case element::GeometryType::ARC:
case element::GeometryType::SPIRAL:
case element::GeometryType::POLY3:
case element::GeometryType::POLY3PARAM: {
auto next_waypoint = current_waypoint;
while (true) {
current_transform = ComputeTransform(current_waypoint);
if (lane.GetId() < 0) {
double remaining_length = (geometry_end_s - current_waypoint.s) - epsilon;
delta_s = std::min(delta_s, remaining_length);
} else {
double remaining_length = (current_waypoint.s - geometry_start_s) - epsilon;
delta_s = std::min(delta_s, remaining_length);
}
if (delta_s < epsilon) {
break;
}
auto next = GetNext(next_waypoint, delta_s);
if (next.size() != 1 ||
current_waypoint.road_id != next.front().road_id) {
geom::Transform next_transform = ComputeTransform(next_waypoint);
Rtree::BPoint init = Rtree::BPoint(current_transform.location.x,
current_transform.location.y, current_transform.location.z);
Rtree::BPoint end = Rtree::BPoint(next_transform.location.x,
next_transform.location.y, next_transform.location.z);
rtree_elements.emplace_back(std::make_pair(Rtree::BSegment(init,
end), std::make_pair(current_waypoint, next_waypoint)));
current_waypoint = next_waypoint;
break;
}
next_waypoint = next.front();
geom::Transform next_transform = ComputeTransform(next_waypoint);
double angle = geom::Math::GetVectorAngle(current_transform.GetForwardVector(),
next_transform.GetForwardVector());
if (abs(angle) > angle_threshold) {
Rtree::BPoint init = Rtree::BPoint(current_transform.location.x,
current_transform.location.y, current_transform.location.z);
Rtree::BPoint end = Rtree::BPoint(next_transform.location.x,
next_transform.location.y, next_transform.location.z);
rtree_elements.emplace_back(std::make_pair(Rtree::BSegment(init,
end), std::make_pair(current_waypoint, next_waypoint)));
current_waypoint = next_waypoint;
}
}
break;
}
}
auto next = GetNext(current_waypoint, 10.0 * epsilon);
if (next.size() != 1 || next.front().road_id != current_waypoint.road_id) {
shouldBreak = true;
} else {
current_waypoint = next.front();
}
if (shouldBreak) {
break;
}
}
}
_rtree.InsertElements(rtree_elements);
}
Junction* Map::GetJunction(JuncId id) {
return _data.GetJunction(id);
}

16
LibCarla/source/carla/road/Map.h
View File

@ -13,10 +13,8 @@
#include "carla/road/element/LaneMarking.h"
#include "carla/road/element/RoadInfoMarkRecord.h"
#include "carla/road/element/Waypoint.h"
#include "carla/geom/Rtree.h"
//#include <boost/geometry.hpp>
//#include <boost/geometry/geometries/point.hpp>
//#include <boost/geometry/index/rtree.hpp>
#include <boost/optional.hpp>
#include <vector>
@ -33,7 +31,7 @@ namespace road {
/// -- Constructor ---------------------------------------------------------
/// ========================================================================
Map(MapData m) : _data(std::move(m)) {}
Map(MapData m) : _data(std::move(m)) {CreateRtree();}
/// ========================================================================
/// -- Georeference --------------------------------------------------------
@ -136,14 +134,10 @@ private:
friend MapBuilder;
MapData _data;
//namespace bg = boost::geometry;
//namespace bgi = boost::geometry::index;
using Rtree = geom::SegmentCloudRtree<Waypoint>;
Rtree _rtree;
/*struct rtreecontent{
LaneId lid;
RoadId rid;
};*/
//bgi::rtree rtree;
void CreateRtree();
};
} // namespace road

5
LibCarla/source/carla/road/Road.h
View File

@ -115,6 +115,11 @@ namespace road {
return _info.GetInfo<T>(s);
}
template <typename T>
std::vector<const T*> GetInfos() const {
return _info.GetInfos<T>();
}
template <typename T>
const T *GetObject(const double s) const {
return _objects.GetObject<T>(s);

114
LibCarla/source/carla/road/element/Geometry.cpp
View File

@ -15,7 +15,6 @@
#include <boost/array.hpp>
#include <boost/math/tools/rational.hpp>
//#include <cephes/fresnel.h>
#include <odrSpiral/odrSpiral.h>
#include <algorithm>
@ -58,12 +57,13 @@ namespace element {
return p;
}
geom::Vector2D RotatebyAngle(double angle, double x, double y){
// helper function for rotating points
geom::Vector2D RotatebyAngle(double angle, double x, double y) {
const double cos_a = std::cos(angle);
const double sin_a = std::sin(angle);
return geom::Vector2D(
static_cast<float>(x * cos_a - y * sin_a),
static_cast<float>(y * cos_a + x * sin_a));
static_cast<float>(x * cos_a - y * sin_a),
static_cast<float>(y * cos_a + x * sin_a));
}
DirectedPoint GeometrySpiral::PosFromDist(double dist) const {
@ -75,7 +75,6 @@ namespace element {
const double curve_start = (_curve_start);
const double curve_dot = (curve_end - curve_start) / (_length);
const double s_o = curve_start / curve_dot;
//const double s_f = curve_end / curve_dot;
double s = s_o + dist;
double x;
@ -102,72 +101,93 @@ namespace element {
/// @todo
std::pair<float, float> GeometrySpiral::DistanceTo(const geom::Location &location) const {
//Not analytic, discretize and find nearest point
//throw_exception(std::runtime_error("not implemented"));
// Not analytic, discretize and find nearest point
// throw_exception(std::runtime_error("not implemented"));
return {location.x - _start_position.x, location.y - _start_position.y};
}
DirectedPoint GeometryPoly3::PosFromDist(double dist) const{
double u = dist;
double v = _a + _b*u + _c*u*u + _d*u*u*u;
DirectedPoint GeometryPoly3::PosFromDist(double dist) const {
auto result = _rtree.GetNearestNeighbours(Rtree::BPoint(static_cast<float>(dist), 0.0f, 0.0f)).front();
double cos_hdg = std::cos(_heading);
double sin_hdg = std::cos(_heading);
auto &val1 = result.second.first;
auto &val2 = result.second.second;
double x = u * cos_hdg - v * sin_hdg;
double y = u * sin_hdg + v * cos_hdg;
double rate = (val2.s - dist) / (val2.s - val1.s);
double u = rate * val1.u + (1.0 - rate) * val2.u;
double v = rate * val1.v + (1.0 - rate) * val2.v;
double tangent = atan((rate * val1.t + (1.0 - rate) * val2.t)); // ?
double tangentv = _b + 2 * _c * u + 3 * _d * u * u;
double theta = std::atan2(tangentv, 1.0);
DirectedPoint p(_start_position, _heading + theta);
p.location.x += static_cast<float>(x);
p.location.y += static_cast<float>(y);
geom::Vector2D pos = RotatebyAngle(_heading, u, v);
DirectedPoint p(_start_position, _heading + tangent);
p.location.x += pos.x;
p.location.y += pos.y;
return p;
}
std::pair<float, float> GeometryPoly3::DistanceTo(const geom::Location &/*p*/) const{
//No analytical expression (Newton-Raphson?/point search)
//throw_exception(std::runtime_error("not implemented"));
return {_start_position.x,_start_position.y};
std::pair<float, float> GeometryPoly3::DistanceTo(const geom::Location & /*p*/) const {
// No analytical expression (Newton-Raphson?/point search)
// throw_exception(std::runtime_error("not implemented"));
return {_start_position.x, _start_position.y};
}
DirectedPoint GeometryParamPoly3::PosFromDist(const double dist) const
void GeometryPoly3::PreComputeSpline() {
const double delta_u = 0.01; // interval between values of u
double current_s = 0;
double current_u = 0;
double last_u = 0;
double last_v = 0;
double last_s = 0;
RtreeValue last_val;
while (current_s < _length + delta_u) {
double current_v = _poly.Evaluate(current_u);
double du = current_u - last_u;
double dv = current_v - last_v;
double ds = sqrt(du * du + dv * dv);
current_s += ds;
double current_t = _poly.Tangent(current_u);
RtreeValue current_val{current_u, current_v, current_s, current_t};
Rtree::BPoint p1(static_cast<float>(last_s), 0.0f, 0.0f);
Rtree::BPoint p2(static_cast<float>(current_s), 0.0f, 0.0f);
_rtree.InsertElement(Rtree::BSegment(p1, p2), last_val, current_val);
last_u = current_u;
last_v = current_v;
last_s = current_s;
last_val = current_val;
current_u += delta_u;
}
}
DirectedPoint GeometryParamPoly3::PosFromDist(double dist) const
{
//from map repo
double p = dist;
if(!_arcLength){
p = geom::Math::Clamp(dist / _length, -1.0, 1.0);
if (!_arcLength) {
p = dist / _length;
}
auto polyU = boost::array<double, 4>{{_aU, _bU, _cU, _dU}};
auto polyV = boost::array<double, 4>{{_aV, _bV, _cV, _dV}};
auto polyU = geom::CubicPolynomial(_aU, _bU, _cU, _dU);
auto polyV = geom::CubicPolynomial(_aV, _bV, _cV, _dV);
double u = boost::math::tools::evaluate_polynomial(polyU, p);
double v = boost::math::tools::evaluate_polynomial(polyV, p);
double u = polyU.Evaluate(p);
double v = polyV.Evaluate(p);
const double cos_t = std::cos(_heading);
const double sin_t = std::sin(_heading);
auto pos = RotatebyAngle(_heading, u, v);
double x = u * cos_t - v * sin_t;
double y = u * sin_t + v * cos_t;
auto tangentPolyU = boost::array<double, 4>{{_bU, 2.0 * _cU, 3.0 * _dU, 0.0}};
auto tangentPolyV = boost::array<double, 4>{{_bV, 2.0 * _cV, 3.0 * _dV, 0.0}};
double tangentU = boost::math::tools::evaluate_polynomial(tangentPolyU, p);
double tangentV = boost::math::tools::evaluate_polynomial(tangentPolyV, p);
double tangentU = polyU.Tangent(p);
double tangentV = polyV.Tangent(p);
double theta = atan2(tangentV, tangentU);
DirectedPoint point(_start_position, _heading + theta);
point.location.x += static_cast<float>(x);
point.location.y += static_cast<float>(y);
point.location.x += pos.x;
point.location.y += pos.y;
return point;
}
std::pair<float, float> GeometryParamPoly3::DistanceTo(const geom::Location &) const {
//No analytical expression (Newton-Raphson?/point search)
//throw_exception(std::runtime_error("not implemented"));
return {_start_position.x,_start_position.y};
// No analytical expression (Newton-Raphson?/point search)
// throw_exception(std::runtime_error("not implemented"));
return {_start_position.x, _start_position.y};
}
} // namespace element
} // namespace road

130
LibCarla/source/carla/road/element/Geometry.h
View File

@ -8,6 +8,8 @@
#include "carla/geom/Location.h"
#include "carla/geom/Math.h"
#include "carla/geom/CubicPolynomial.h"
#include "carla/geom/Rtree.h"
namespace carla {
namespace road {
@ -73,17 +75,16 @@ namespace element {
protected:
Geometry(
GeometryType type,
double start_offset,
double length,
double heading,
const geom::Location &start_pos)
GeometryType type,
double start_offset,
double length,
double heading,
const geom::Location &start_pos)
: _type(type),
_length(length),
_start_position_offset(start_offset),
_heading(heading),
_start_position(start_pos)
{}
_start_position(start_pos) {}
protected:
@ -100,10 +101,10 @@ namespace element {
public:
GeometryLine(
double start_offset,
double length,
double heading,
const geom::Location &start_pos)
double start_offset,
double length,
double heading,
const geom::Location &start_pos)
: Geometry(GeometryType::LINE, start_offset, length, heading, start_pos) {}
DirectedPoint PosFromDist(double dist) const override;
@ -116,9 +117,9 @@ namespace element {
/// @param p point to calculate the distance
std::pair<float, float> DistanceTo(const geom::Location &p) const override {
return geom::Math::DistanceSegmentToPoint(
p,
_start_position,
PosFromDist(_length).location);
p,
_start_position,
PosFromDist(_length).location);
}
};
@ -127,11 +128,11 @@ namespace element {
public:
GeometryArc(
double start_offset,
double length,
double heading,
const geom::Location &start_pos,
double curv)
double start_offset,
double length,
double heading,
const geom::Location &start_pos,
double curv)
: Geometry(GeometryType::ARC, start_offset, length, heading, start_pos),
_curvature(curv) {}
@ -144,11 +145,11 @@ namespace element {
/// @param p point to calculate the distance
std::pair<float, float> DistanceTo(const geom::Location &p) const override {
return geom::Math::DistanceArcToPoint(
p,
_start_position,
static_cast<float>(_length),
static_cast<float>(_heading),
static_cast<float>(_curvature));
p,
_start_position,
static_cast<float>(_length),
static_cast<float>(_heading),
static_cast<float>(_curvature));
}
double GetCurvature() const {
@ -164,12 +165,12 @@ namespace element {
public:
GeometrySpiral(
double start_offset,
double length,
double heading,
const geom::Location &start_pos,
double curv_s,
double curv_e)
double start_offset,
double length,
double heading,
const geom::Location &start_pos,
double curv_s,
double curv_e)
: Geometry(GeometryType::SPIRAL, start_offset, length, heading, start_pos),
_curve_start(curv_s),
_curve_end(curv_e) {}
@ -194,20 +195,24 @@ namespace element {
class GeometryPoly3 final : public Geometry {
public:
GeometryPoly3(
double start_offset,
double length,
double heading,
const geom::Location &start_pos,
double a,
double b,
double c,
double d)
: Geometry(GeometryType::POLY3, start_offset, length, heading, start_pos),
double start_offset,
double length,
double heading,
const geom::Location &start_pos,
double a,
double b,
double c,
double d)
: Geometry(GeometryType::POLY3, start_offset, length, heading, start_pos),
_a(a),
_b(b),
_c(c),
_d(d){}
_d(d) {
_poly.Set(a, b, c, d);
PreComputeSpline();
}
double Geta() const {
return _a;
@ -227,30 +232,44 @@ namespace element {
std::pair<float, float> DistanceTo(const geom::Location &) const override;
private:
geom::CubicPolynomial _poly;
double _a;
double _b;
double _c;
double _d;
struct RtreeValue {
double u = 0;
double v = 0;
double s = 0;
double t = 0;
};
using Rtree = geom::SegmentCloudRtree<RtreeValue>;
using TreeElement = Rtree::TreeElement;
Rtree _rtree;
void PreComputeSpline();
};
class GeometryParamPoly3 final : public Geometry {
public:
GeometryParamPoly3(
double start_offset,
double length,
double heading,
const geom::Location &start_pos,
double aU,
double bU,
double cU,
double dU,
double aV,
double bV,
double cV,
double dV,
bool arcLength)
: Geometry(GeometryType::POLY3PARAM, start_offset, length, heading, start_pos),
double start_offset,
double length,
double heading,
const geom::Location &start_pos,
double aU,
double bU,
double cU,
double dU,
double aV,
double bV,
double cV,
double dV,
bool arcLength)
: Geometry(GeometryType::POLY3PARAM, start_offset, length, heading, start_pos),
_aU(aU),
_bU(bU),
_cU(cU),
@ -259,7 +278,7 @@ namespace element {
_bV(bV),
_cV(cV),
_dV(dV),
_arcLength(arcLength){}
_arcLength(arcLength) {}
double GetaU() const {
return _aU;
@ -291,6 +310,7 @@ namespace element {
std::pair<float, float> DistanceTo(const geom::Location &) const override;
private:
double _aU;
double _bU;
double _cU;

120
PythonAPI/util/map_viewer.py
View File

@ -7,13 +7,16 @@
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.
# ==============================================================================
# -- find carla module ---------------------------------------------------------
# ==============================================================================
import glob
import os
import sys
import argparse
import math
import pygame
# ==============================================================================
# -- find carla module ---------------------------------------------------------
# ==============================================================================
try:
sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
@ -29,36 +32,40 @@ except IndexError:
import carla
import argparse
import logging
import datetime
import weakref
import math
import random
import hashlib
import pygame
import time
# ==============================================================================
# -- Constants -----------------------------------------------------------------
# ==============================================================================
COLOR_BLACK = pygame.Color(0, 0, 0)
COLOR_GREEN = pygame.Color(0, 255, 0)
COLOR_RED = pygame.Color(255, 0, 0)
COLOR_BLUE = pygame.Color(0, 0, 255)
COLOR_WHITE = pygame.Color(255, 255, 255)
COLOR_PINK = pygame.Color(255, 0, 255)
def world_to_pixel(location, pixels_per_meter, scale, world_offset, offset=(0,0)):
def world_to_pixel(location, pixels_per_meter, scale, world_offset, offset=(0, 0)):
"""Converts the world coordinates to pixel coordinates"""
x = scale * pixels_per_meter * (location.x - world_offset[0])
y = scale * pixels_per_meter * (location.y - world_offset[1])
return [int(x - offset[0]), int(y - offset[1])]
pixel_x = scale * pixels_per_meter * (location.x - world_offset[0])
pixel_y = scale * pixels_per_meter * (location.y - world_offset[1])
return [int(pixel_x - offset[0]), int(pixel_y - offset[1])]
def pixel_to_world(pixel_x, pixel_y, pixels_per_meter, scale, world_offset, offset=(0, 0)):
"""Converts the pixel coordinates to world coordinates"""
location_x = float(pixel_x + offset[0]) / (scale * pixels_per_meter) + world_offset[0]
location_y = float(pixel_y + offset[1]) / (scale * pixels_per_meter) + world_offset[1]
return carla.Location(location_x, location_y, 0)
# ==============================================================================
# -- Main --------------------------------------------------------------------
# ==============================================================================
HEIGHT = 1024
WIDTH = 1024
def main():
"""Runs the 2D map viewer"""
argparser = argparse.ArgumentParser()
argparser.add_argument(
'-f', '--file',
@ -66,19 +73,12 @@ def main():
default="",
type=str,
help='Path to the OpenDRIVE file')
argparser.add_argument(
'-o', '--output',
metavar='O',
default="map_viewer.png",
type=str,
help='Output file for the map image')
args = argparser.parse_args()
pygame.init()
height = 1024
width = 1024
display = pygame.display.set_mode(
(width, height),
(WIDTH, HEIGHT),
pygame.HWSURFACE | pygame.DOUBLEBUF)
# Place a title to game window
@ -87,16 +87,18 @@ def main():
filename = args.file
f_odr = open(filename, "r")
opendrive = f_odr.read()
map = carla.Map("TestParser", str(opendrive))
waypoints = map.generate_waypoints(10)
f_odr.close()
map = carla.Map("MapViewer", str(opendrive))
waypoints = map.generate_waypoints(5)
points = []
x_list = []
y_list = []
for w in waypoints:
transf = w.transform
for waypoint in waypoints:
transf = waypoint.transform
if math.isnan(transf.location.x) | math.isnan(transf.location.y):
print("nan here: lane id " + str(w.lane_id) + " road id " + str(w.road_id))
print("nan here: lane id " + str(waypoint.lane_id) +
" road id " + str(waypoint.road_id))
else:
x_list.append(transf.location.x)
y_list.append(transf.location.y)
@ -108,41 +110,41 @@ def main():
road_width = x_max - x_min
road_height = y_max - y_min
road_mid = (0.5*(x_max + x_min), 0.5*(y_max + y_min))
scale = 1
pixels_per_meter = width/road_width
road_mid = (0.5 * (x_max + x_min), 0.5 * (y_max + y_min))
scale = 0.99
pixels_per_meter = min(WIDTH / road_width, HEIGHT / road_height)
world_offset = road_mid
print("Num points: " + str(len(waypoints)))
print("road width: " + str(road_width))
print("road height: " + str(road_height))
print("road mid: " + str(road_mid))
print("scale: " + str(scale))
for point in points:
pygame.draw.circle(
display, COLOR_GREEN,
world_to_pixel(point.location, pixels_per_meter, scale, world_offset, (-width / 2, -height / 2)), 0)
world_to_pixel(point.location, pixels_per_meter, scale, world_offset,
(-WIDTH / 2, -HEIGHT / 2)), 0)
"""waypoints = map.get_topology()
topopoints = []
for w in waypoints:
pos = w[0].transform.location
topopoints.append(pos)
pos = w[1].transform.location
topopoints.append(pos)
while True:
event = pygame.event.poll()
if event.type == pygame.QUIT:
break
display.fill(COLOR_BLACK)
for point in topopoints:
pygame.draw.circle(
display, COLOR_BLUE,
world_to_pixel(point, pixels_per_meter, scale, world_offset, (-width / 2, -height / 2)), 3)
"""
pygame.display.flip()
if args.output != "":
pygame.image.save(display, args.output)
time.sleep(100000)
for point in points:
pygame.draw.circle(
display, COLOR_GREEN,
world_to_pixel(point.location, pixels_per_meter, scale, world_offset,
(-WIDTH / 2, -HEIGHT / 2)), 0)
mouse = pygame.mouse.get_pos()
mouse_position = pixel_to_world(mouse[0], mouse[1], pixels_per_meter,
scale, world_offset, (-WIDTH / 2, -HEIGHT / 2))
mouse_waypoint = map.get_waypoint(mouse_position)
waypoint_position = world_to_pixel(mouse_waypoint.transform.location,
pixels_per_meter, scale, world_offset,
(-WIDTH / 2, -HEIGHT / 2))
pygame.draw.line(display, COLOR_WHITE, mouse, waypoint_position, 1)
pygame.display.flip()
pygame.quit()
if __name__ == '__main__':
main()
main()

2
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp
View File

@ -10,9 +10,9 @@
#include <compiler/disable-ue4-macros.h>
#include <carla/rpc/WeatherParameters.h>
#include "carla/opendrive/OpenDriveParser.h"
#include <compiler/enable-ue4-macros.h>
#include "carla/opendrive/OpenDriveParser.h"
#include "Carla/OpenDrive/OpenDrive.h"
ACarlaGameModeBase::ACarlaGameModeBase(const FObjectInitializer& ObjectInitializer)