diff --git a/LibCarla/source/carla/geom/CubicPolynomial.h b/LibCarla/source/carla/geom/CubicPolynomial.h index 95db92aae..1e1fb6b9d 100644 --- a/LibCarla/source/carla/geom/CubicPolynomial.h +++ b/LibCarla/source/carla/geom/CubicPolynomial.h @@ -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 ----------------------------------------------------- // ========================================================================= diff --git a/LibCarla/source/carla/geom/Math.cpp b/LibCarla/source/carla/geom/Math.cpp index c0293a791..dbd0a306e 100644 --- a/LibCarla/source/carla/geom/Math.cpp +++ b/LibCarla/source/carla/geom/Math.cpp @@ -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 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 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 diff --git a/LibCarla/source/carla/geom/Math.h b/LibCarla/source/carla/geom/Math.h index d87a5ac05..765bf7841 100644 --- a/LibCarla/source/carla/geom/Math.h +++ b/LibCarla/source/carla/geom/Math.h @@ -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) diff --git a/LibCarla/source/carla/geom/Rtree.h b/LibCarla/source/carla/geom/Rtree.h new file mode 100644 index 000000000..552eced14 --- /dev/null +++ b/LibCarla/source/carla/geom/Rtree.h @@ -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 . + +#pragma once + +#include + +#include +#include +#include +#include + +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 + class PointCloudRtree{ + public: + + typedef boost::geometry::model::point BPoint; + typedef std::pair 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 &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 + std::vector GetNearestNeighboursWithFilter(const BPoint &point, Filter filter, + size_t number_neighbours = 1) const { + std::vector query_result; + _rtree.query(boost::geometry::index::nearest(point, + static_cast(number_neighbours)) && boost::geometry::index::satisfies(filter), + std::back_inserter(query_result)); + return query_result; + } + std::vector GetNearestNeighbours(const BPoint &point, size_t number_neighbours = 1) const { + std::vector query_result; + _rtree.query(boost::geometry::index::nearest(point, static_cast(number_neighbours)), + std::back_inserter(query_result)); + return query_result; + } + size_t GetTreeSize() const { return _rtree.size(); + } + + private: + + boost::geometry::index::rtree> _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 + class SegmentCloudRtree { + public: + + typedef boost::geometry::model::point BPoint; + typedef boost::geometry::model::segment BSegment; + typedef std::pair> 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 &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 + std::vector GetNearestNeighboursWithFilter(const BPoint &point, Filter filter, + size_t number_neighbours = 1) const { + std::vector query_result; + _rtree.query(boost::geometry::index::nearest(point, + static_cast(number_neighbours)) && boost::geometry::index::satisfies(filter), + std::back_inserter(query_result)); + return query_result; + } + std::vector GetNearestNeighbours(const BPoint &point, size_t number_neighbours = 1) const { + std::vector query_result; + _rtree.query(boost::geometry::index::nearest(point, + static_cast(number_neighbours)), + std::back_inserter(query_result)); + return query_result; + } + + size_t GetTreeSize() const { return _rtree.size(); + } + + private: + + boost::geometry::index::rtree> _rtree; + + }; + +} // namespace geom +} // namespace carla diff --git a/LibCarla/source/carla/road/Lane.h b/LibCarla/source/carla/road/Lane.h index f5803dad1..27ce4f228 100644 --- a/LibCarla/source/carla/road/Lane.h +++ b/LibCarla/source/carla/road/Lane.h @@ -79,6 +79,12 @@ namespace road { return _info.GetInfo(s); } + template + std::vector GetInfos() const { + DEBUG_ASSERT(_lane_section != nullptr); + return _info.GetInfos(); + } + const std::vector &GetNextLanes() const { return _next_lanes; } diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp index fdcf8c099..11074d335 100644 --- a/LibCarla/source/carla/road/Map.cpp +++ b/LibCarla/source/carla/road/Map.cpp @@ -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 @@ -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 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(lane.GetType()) & static_cast(Lane::LaneType::Driving)) > 0) { std::forward(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 - 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 - 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(func)); + road.GetId(), + lane_section, + distance, + std::forward(func)); } } @@ -137,9 +138,9 @@ namespace road { /// for an specific Lane given an s and a iterator over lanes template static std::pair 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 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 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(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::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::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::max()) { + if (query_result.size() == 0) { return boost::optional{}; } - 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 Map::GetWaypoint( - const geom::Location &pos, - uint32_t lane_type) const { + const geom::Location &pos, + uint32_t lane_type) const { boost::optional 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(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(computed_width.first); lane_tangent = static_cast(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(computed_width.first); lane_tangent = static_cast(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(waypoint.s); float lane_offset_tangent = 0; - if(lane_offset_info){ + if (lane_offset_info) { lane_offset_tangent = static_cast(lane_offset_info->GetPolynomial().Tangent(waypoint.s)); } @@ -369,9 +322,10 @@ namespace road { lane_tangent *= -1; geom::Rotation rot( - geom::Math::ToDegrees(static_cast(dp.pitch)), - geom::Math::ToDegrees(-static_cast(dp.tangent)), // Unreal's Y axis hack - 0.0f); + geom::Math::ToDegrees(static_cast(dp.pitch)), + geom::Math::ToDegrees(-static_cast(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 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 Map::GetAllCrosswalkZones() const { std::vector result; - for (const auto &pair : _data.GetRoads()) { + for (const auto &pair : _data.GetRoads()) { const auto &road = pair.second; std::vector crosswalks = road.GetObjects(); if (crosswalks.size() > 0) { @@ -477,7 +431,8 @@ namespace road { // move perpendicular ('t') geom::Transform pivot = base; pivot.rotation.yaw -= geom::Math::ToDegrees(static_cast(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(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(corner.u), static_cast(corner.v), static_cast(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(corner.u), static_cast(corner.v), + static_cast(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 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 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() / 20.0; // 9 degrees + + std::vector 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_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(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(); + 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(); + 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); } diff --git a/LibCarla/source/carla/road/Map.h b/LibCarla/source/carla/road/Map.h index f7a726ef2..77bf56b88 100644 --- a/LibCarla/source/carla/road/Map.h +++ b/LibCarla/source/carla/road/Map.h @@ -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 -//#include -//#include #include #include @@ -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; + Rtree _rtree; - /*struct rtreecontent{ - LaneId lid; - RoadId rid; - };*/ - //bgi::rtree rtree; + void CreateRtree(); }; } // namespace road diff --git a/LibCarla/source/carla/road/Road.h b/LibCarla/source/carla/road/Road.h index 6d75373ec..2a723df97 100644 --- a/LibCarla/source/carla/road/Road.h +++ b/LibCarla/source/carla/road/Road.h @@ -115,6 +115,11 @@ namespace road { return _info.GetInfo(s); } + template + std::vector GetInfos() const { + return _info.GetInfos(); + } + template const T *GetObject(const double s) const { return _objects.GetObject(s); diff --git a/LibCarla/source/carla/road/element/Geometry.cpp b/LibCarla/source/carla/road/element/Geometry.cpp index e65e9909b..4e6a94d8a 100644 --- a/LibCarla/source/carla/road/element/Geometry.cpp +++ b/LibCarla/source/carla/road/element/Geometry.cpp @@ -15,7 +15,6 @@ #include #include -//#include #include #include @@ -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(x * cos_a - y * sin_a), - static_cast(y * cos_a + x * sin_a)); + static_cast(x * cos_a - y * sin_a), + static_cast(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 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(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(x); - p.location.y += static_cast(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 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 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(last_s), 0.0f, 0.0f); + Rtree::BPoint p2(static_cast(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{{_aU, _bU, _cU, _dU}}; - auto polyV = boost::array{{_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{{_bU, 2.0 * _cU, 3.0 * _dU, 0.0}}; - auto tangentPolyV = boost::array{{_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(x); - point.location.y += static_cast(y); + point.location.x += pos.x; + point.location.y += pos.y; return point; } std::pair 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 diff --git a/LibCarla/source/carla/road/element/Geometry.h b/LibCarla/source/carla/road/element/Geometry.h index 75fd00411..fed2350ab 100644 --- a/LibCarla/source/carla/road/element/Geometry.h +++ b/LibCarla/source/carla/road/element/Geometry.h @@ -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 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 DistanceTo(const geom::Location &p) const override { return geom::Math::DistanceArcToPoint( - p, - _start_position, - static_cast(_length), - static_cast(_heading), - static_cast(_curvature)); + p, + _start_position, + static_cast(_length), + static_cast(_heading), + static_cast(_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 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; + 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 DistanceTo(const geom::Location &) const override; private: + double _aU; double _bU; double _cU; diff --git a/PythonAPI/util/map_viewer.py b/PythonAPI/util/map_viewer.py index b9c698b14..2b85844f6 100644 --- a/PythonAPI/util/map_viewer.py +++ b/PythonAPI/util/map_viewer.py @@ -7,13 +7,16 @@ # This work is licensed under the terms of the MIT license. # For a copy, see . -# ============================================================================== -# -- 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() \ No newline at end of file + main() diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp index 9f34a45b2..6dd8ec508 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp @@ -10,9 +10,9 @@ #include #include +#include "carla/opendrive/OpenDriveParser.h" #include -#include "carla/opendrive/OpenDriveParser.h" #include "Carla/OpenDrive/OpenDrive.h" ACarlaGameModeBase::ACarlaGameModeBase(const FObjectInitializer& ObjectInitializer)