Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: MIT
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "ad/map/lane/LaneOperation.hpp"
10 : :
11 : : #include <algorithm>
12 : : #include "LaneOperationPrivate.hpp"
13 : : #include "ad/map/access/Operation.hpp"
14 : : #include "ad/map/lane/BorderOperation.hpp"
15 : : #include "ad/map/match/AdMapMatching.hpp"
16 : : #include "ad/map/match/MapMatchedOperation.hpp"
17 : : #include "ad/map/point/Operation.hpp"
18 : : #include "ad/map/route/LaneIntervalOperation.hpp"
19 : : #include "ad/physics/RangeOperation.hpp"
20 : :
21 : : namespace ad {
22 : : namespace map {
23 : : namespace lane {
24 : :
25 : 295655 : Lane::ConstPtr getLanePtr(LaneId const &id)
26 : : {
27 : 295655 : auto const lane = access::getStore().getLanePtr(id);
28 : :
29 [ + + ]: 295655 : if (!bool(lane))
30 : : {
31 [ + - ]: 3 : throw std::invalid_argument("ad::map::lane::getLane: LaneId not found in store");
32 : : }
33 : 295652 : return lane;
34 : : }
35 : :
36 : 202848 : Lane const &getLane(LaneId const &id)
37 : : {
38 : 202848 : return *getLanePtr(id);
39 : : }
40 : :
41 : 19 : LaneIdList getLanes()
42 : : {
43 : 19 : return access::getStore().getLanes();
44 : : }
45 : :
46 : 123907 : void interpolateHeadingParametricPoints(physics::Distance const &length,
47 : : physics::ParametricValue const &headingT,
48 : : physics::ParametricValue &longTStart,
49 : : physics::ParametricValue &longTEnd)
50 : : {
51 : : // we target an area of +- 5cm around the point to determine the heading
52 [ + - - + ]: 123907 : if (length < physics::Distance(0.1))
53 : : {
54 : 0 : longTStart = physics::ParametricValue(0.);
55 : 0 : longTEnd = physics::ParametricValue(1.);
56 : : }
57 : : else
58 : : {
59 [ + - ]: 123907 : physics::ParametricValue deltaOffset(physics::Distance(0.1) / length);
60 [ + - ]: 123907 : physics::ParametricValue deltaOffsetHalf = deltaOffset / 2.;
61 [ + - + - : 123907 : if (headingT > physics::ParametricValue(1.) - deltaOffsetHalf)
+ + ]
62 : : {
63 [ + - ]: 23803 : longTStart = physics::ParametricValue(1.) - deltaOffset;
64 : 23803 : longTEnd = physics::ParametricValue(1.);
65 : : }
66 [ + - + + ]: 100104 : else if (headingT < deltaOffsetHalf)
67 : : {
68 : 20606 : longTStart = physics::ParametricValue(0.);
69 : 20606 : longTEnd = deltaOffset;
70 : : }
71 : : else
72 : : {
73 [ + - ]: 79498 : longTStart = headingT - deltaOffsetHalf;
74 [ + - ]: 79498 : longTEnd = headingT + deltaOffsetHalf;
75 : : }
76 : : }
77 : 123907 : }
78 : :
79 : 123907 : point::ECEFHeading getLaneECEFDirection(Lane const &lane, point::ParaPoint const ¶Point)
80 : : {
81 : 123907 : physics::ParametricValue longTStart;
82 : 123907 : physics::ParametricValue longTEnd;
83 [ + - ]: 123907 : interpolateHeadingParametricPoints(lane.length, paraPoint.parametricOffset, longTStart, longTEnd);
84 : :
85 [ + - ]: 123907 : point::ECEFPoint const laneDirStart = getParametricPoint(lane, longTStart, physics::ParametricValue(.5));
86 [ + - ]: 123907 : point::ECEFPoint const laneDirEnd = getParametricPoint(lane, longTEnd, physics::ParametricValue(.5));
87 [ + - ]: 123907 : point::ECEFHeading const laneDirection = point::createECEFHeading(laneDirStart, laneDirEnd);
88 : 247814 : return laneDirection;
89 : : }
90 : :
91 : 123865 : point::ECEFHeading getLaneECEFHeading(match::MapMatchedPosition const &mapMatchedPosition)
92 : : {
93 : 123865 : return getLaneECEFHeading(mapMatchedPosition.lanePoint.paraPoint);
94 : : }
95 : :
96 : 123907 : point::ECEFHeading getLaneECEFHeading(point::ParaPoint const ¶Point)
97 : : {
98 [ + - + - ]: 247814 : auto lane = getLane(paraPoint.laneId);
99 : :
100 [ + - ]: 123907 : point::ECEFHeading laneDrivingDirection = getLaneECEFDirection(lane, paraPoint);
101 [ + + ]: 123907 : if (!isLaneDirectionPositive(lane))
102 : : {
103 [ + - ]: 90602 : laneDrivingDirection = -1. * laneDrivingDirection;
104 : : }
105 : 247814 : return laneDrivingDirection;
106 : : }
107 : :
108 : 8 : point::ENUHeading getLaneENUHeading(match::MapMatchedPosition const &mapMatchedPosition)
109 : : {
110 [ + - ]: 16 : return point::createENUHeading(getLaneECEFHeading(mapMatchedPosition), mapMatchedPosition.matchedPoint);
111 : : }
112 : :
113 : 42 : point::ENUHeading getLaneENUHeading(point::ParaPoint const ¶Point, point::GeoPoint const &gnssReference)
114 : : {
115 [ + - ]: 84 : return point::createENUHeading(getLaneECEFHeading(paraPoint), gnssReference);
116 : : }
117 : :
118 : 786 : point::ParaPoint uniqueParaPoint(point::GeoPoint const &geoPoint)
119 : : {
120 [ + - ]: 1572 : match::AdMapMatching mapMatching;
121 : : auto mapMatchingResult
122 [ + - ]: 787 : = mapMatching.getMapMatchedPositions(geoPoint, physics::Distance(0.1), physics::Probability(0.5));
123 : :
124 [ + + ]: 786 : if (mapMatchingResult.size() == 0u)
125 : : {
126 [ + - ]: 1 : throw std::runtime_error("uniqueLaneId: position doesn't match any lane within 0.1 meters");
127 : : }
128 [ - + ]: 785 : if (mapMatchingResult.size() != 1u)
129 : : {
130 [ # # ]: 0 : throw std::runtime_error("uniqueLaneId: position matches multiple lanes");
131 : : }
132 : :
133 : 1570 : return mapMatchingResult[0].lanePoint.paraPoint;
134 : : }
135 : :
136 : 786 : LaneId uniqueLaneId(point::GeoPoint const &geoPoint)
137 : : {
138 : 786 : return uniqueParaPoint(geoPoint).laneId;
139 : : }
140 : :
141 : 5 : physics::Distance getWidth(Lane const &lane, physics::ParametricValue const &longitudinalOffset)
142 : : {
143 : 5 : physics::Distance width(0.);
144 [ + - ]: 5 : point::ECEFPoint pointOnLeftEdge;
145 [ + - ]: 5 : point::ECEFPoint pointOnRightEdge;
146 [ + - + - ]: 5 : if (projectParametricPointToEdges(lane, longitudinalOffset, pointOnLeftEdge, pointOnRightEdge))
147 : : {
148 [ + - ]: 5 : width = distance(pointOnLeftEdge, pointOnRightEdge);
149 : : }
150 : 5 : return width;
151 : : }
152 : :
153 : 11309 : restriction::SpeedLimitList getSpeedLimits(Lane const &lane, physics::ParametricRange const &range)
154 : : {
155 : 11309 : restriction::SpeedLimitList speedLimits;
156 [ + + ]: 22618 : for (auto const &speedLimit : lane.speedLimits)
157 : : {
158 [ + - + - ]: 11309 : if (doRangesOverlap(speedLimit.lanePiece, range))
159 : : {
160 [ + - ]: 11309 : speedLimits.push_back(speedLimit);
161 : : }
162 : : }
163 : 11309 : return speedLimits;
164 : : }
165 : :
166 : 9917 : physics::Speed getMaxSpeed(Lane const &lane, physics::ParametricRange const &range)
167 : : {
168 : 9917 : physics::Speed maxSpeed(0.);
169 [ + - + + ]: 19834 : for (auto const &speedLimit : getSpeedLimits(lane, range))
170 : : {
171 [ + - ]: 9917 : maxSpeed = std::max(maxSpeed, speedLimit.speedLimit);
172 : : }
173 [ + - + + ]: 9917 : if (maxSpeed == physics::Speed(0.))
174 : : {
175 : 1 : maxSpeed = std::numeric_limits<physics::Speed>::max();
176 : : }
177 : 9917 : return maxSpeed;
178 : : }
179 : :
180 : 19880 : physics::Duration getDuration(Lane const &lane, physics::ParametricRange const &range)
181 : : {
182 : 19880 : physics::Duration laneMinDuration{0.};
183 : 19880 : physics::Distance coveredDurationDistance{0.};
184 [ + + ]: 39760 : for (auto const &speedLimit : lane.speedLimits)
185 : : {
186 [ + - ]: 19880 : auto const intersectedRange = getIntersectionRange(speedLimit.lanePiece, range);
187 [ + - + - ]: 19880 : if (isRangeValid(intersectedRange))
188 : : {
189 [ + - + - ]: 19880 : physics::Distance const speedLimitDistance = (intersectedRange.maximum - intersectedRange.minimum) * lane.length;
190 : 19880 : physics::Speed speedLimitValue = std::numeric_limits<physics::Speed>::max();
191 [ + - + - ]: 19880 : if (speedLimit.speedLimit > physics::Speed(0.))
192 : : {
193 : 19880 : speedLimitValue = speedLimit.speedLimit;
194 : : }
195 [ + - ]: 19880 : physics::Duration const speedLimitDuration = speedLimitDistance / speedLimitValue;
196 [ + - ]: 19880 : laneMinDuration += speedLimitDuration;
197 [ + - ]: 19880 : coveredDurationDistance += speedLimitDistance;
198 : : }
199 : : }
200 [ + - + - : 19880 : physics::Distance const remainingDistance = (range.maximum - range.minimum) * lane.length - coveredDurationDistance;
+ - ]
201 [ + - - + ]: 19880 : if (remainingDistance > physics::Distance(0.))
202 : : {
203 [ # # # # ]: 0 : physics::Duration const speedLimitDuration = remainingDistance / getMaxSpeed(lane, range);
204 [ # # ]: 0 : laneMinDuration += speedLimitDuration;
205 : : }
206 : 19880 : return laneMinDuration;
207 : : }
208 : :
209 : 141019 : ContactLaneList getContactLanes(Lane const &lane, ContactLocation const &location)
210 : : {
211 [ - + ]: 141019 : if (location == ContactLocation::INVALID)
212 : : {
213 : 0 : std::runtime_error("LaneOperation::getContactLanes() location is INVALID");
214 : : }
215 : 141019 : ContactLaneList contactLanes;
216 [ + + ]: 1179950 : for (auto const &contactLane : lane.contactLanes)
217 : : {
218 [ + + ]: 1038930 : if (contactLane.location == location)
219 : : {
220 [ + - ]: 141424 : contactLanes.push_back(contactLane);
221 : : }
222 : : }
223 : 141019 : return contactLanes;
224 : : }
225 : :
226 : 29601 : ContactLaneList getContactLanes(Lane const &lane, ContactLocationList const &locations)
227 : : {
228 : 29601 : ContactLaneList contactLanes;
229 [ + + ]: 89403 : for (auto const &location : locations)
230 : : {
231 [ + - ]: 59802 : auto const locationContactLanes = getContactLanes(lane, location);
232 [ + - ]: 59802 : contactLanes.insert(std::begin(contactLanes), std::begin(locationContactLanes), std::end(locationContactLanes));
233 : : }
234 : 29601 : return contactLanes;
235 : : }
236 : :
237 : 24376 : ContactLocation getContactLocation(Lane const &lane, LaneId const &to_lane_id)
238 : : {
239 [ + - ]: 24376 : if (isValid(to_lane_id))
240 : : {
241 [ + + ]: 61351 : for (auto const &contact_lane : lane.contactLanes)
242 : : {
243 [ + - + + ]: 61343 : if (contact_lane.toLane == to_lane_id)
244 : : {
245 : 24368 : return contact_lane.location;
246 : : }
247 : : }
248 : : }
249 : 8 : return ContactLocation::INVALID;
250 : : }
251 : :
252 : 412569 : point::ECEFPoint getParametricPoint(Lane const &lane,
253 : : physics::ParametricValue const &longitudinalOffset,
254 : : physics::ParametricValue const &lateralOffset)
255 : : {
256 [ + - ]: 412569 : point::ECEFPoint const pt0 = getParametricPoint(lane.edgeLeft, longitudinalOffset);
257 [ + - + + ]: 412569 : if (isValid(pt0))
258 : : {
259 [ + - ]: 412568 : point::ECEFPoint const pt1 = getParametricPoint(lane.edgeRight, longitudinalOffset);
260 [ + - + - ]: 412568 : if (isValid(pt1))
261 : : {
262 [ + - ]: 412568 : return point::vectorInterpolate(pt0, pt1, lateralOffset);
263 : : }
264 : : }
265 [ + - ]: 1 : return point::ECEFPoint();
266 : : }
267 : :
268 : 5804 : bool projectParametricPointToEdges(Lane const &lane,
269 : : point::ECEFPoint const &referencePoint,
270 : : point::ECEFPoint &pointOnLeftEdge,
271 : : point::ECEFPoint &pointOnRightEdge)
272 : : {
273 [ + - ]: 5804 : if (isValid(referencePoint))
274 : : {
275 [ + - ]: 5804 : auto const longTLeft = findNearestPointOnEdge(lane.edgeLeft, referencePoint);
276 [ + - + - ]: 5804 : if (isRangeValid(longTLeft))
277 : : {
278 [ + - ]: 5804 : auto const longTRight = findNearestPointOnEdge(lane.edgeRight, referencePoint);
279 [ + - + - ]: 5804 : if (isRangeValid(longTRight))
280 : : {
281 [ + - ]: 5804 : pointOnLeftEdge = getParametricPoint(lane.edgeLeft, longTLeft);
282 [ + - ]: 5804 : pointOnRightEdge = getParametricPoint(lane.edgeRight, longTRight);
283 [ + - + - : 5804 : return isValid(pointOnLeftEdge) && isValid(pointOnRightEdge);
+ - + - ]
284 : : }
285 : : }
286 : : }
287 : 0 : return false;
288 : : }
289 : :
290 : 5804 : bool projectParametricPointToEdges(Lane const &lane,
291 : : physics::ParametricValue const &longitudinalOffset,
292 : : point::ECEFPoint &pointOnLeftEdge,
293 : : point::ECEFPoint &pointOnRightEdge)
294 : : {
295 [ + - ]: 5804 : point::ECEFPoint const centerPoint = getParametricPoint(lane, longitudinalOffset, physics::ParametricValue(0.5));
296 [ + - ]: 11608 : return projectParametricPointToEdges(lane, centerPoint, pointOnLeftEdge, pointOnRightEdge);
297 : : }
298 : :
299 : 5797 : point::ECEFPoint getProjectedParametricPoint(Lane const &lane,
300 : : physics::ParametricValue const &longitudinalOffset,
301 : : physics::ParametricValue const &lateralOffset)
302 : : {
303 [ + - ]: 5797 : point::ECEFPoint pt0;
304 [ + - ]: 5797 : point::ECEFPoint pt1;
305 [ + - ]: 5797 : bool projectionResult = projectParametricPointToEdges(lane, longitudinalOffset, pt0, pt1);
306 [ + - ]: 5797 : if (projectionResult)
307 : : {
308 [ + - ]: 5797 : return point::vectorInterpolate(pt0, pt1, lateralOffset);
309 : : }
310 [ # # ]: 0 : return point::ECEFPoint();
311 : : }
312 : :
313 : 6 : bool isPyhsicalSuccessor(Lane const &lane, const Lane &other)
314 : : {
315 [ + + + + : 6 : if (isSuccessor(lane.edgeLeft, other.edgeLeft) && isSuccessor(lane.edgeRight, other.edgeRight))
+ + ]
316 : : {
317 : 1 : return true;
318 : : }
319 [ + + + + : 5 : if (isSuccessor(lane.edgeLeft, other.edgeRight) && isSuccessor(lane.edgeRight, other.edgeLeft))
+ + ]
320 : : {
321 : 1 : return true;
322 : : }
323 [ + + - + : 4 : if (isVanishingLaneStart(lane) || isVanishingLaneEnd(lane))
+ + ]
324 : : {
325 [ + + + - : 2 : if (isSuccessor(lane.edgeLeft, other.edgeLeft) && isSuccessor(lane.edgeRight, other.edgeLeft))
+ + ]
326 : : {
327 : 1 : return true;
328 : : }
329 [ + - + - : 1 : if (isSuccessor(lane.edgeLeft, other.edgeRight) && isSuccessor(lane.edgeRight, other.edgeRight))
+ - ]
330 : : {
331 : 1 : return true;
332 : : }
333 : : }
334 [ + - ]: 2 : if (isVanishingLaneEnd(other))
335 : : {
336 [ + + - + : 2 : if (isSuccessor(lane.edgeLeft, other.edgeRight) || isSuccessor(lane.edgeRight, other.edgeRight))
+ + ]
337 : : {
338 : 1 : return true;
339 : : }
340 : : }
341 : 1 : return false;
342 : : }
343 : :
344 : 5 : bool isPhysicalPredecessor(Lane const &lane, const Lane &other)
345 : : {
346 [ + + + + : 5 : if (isPredecessor(lane.edgeLeft, other.edgeLeft) && isPredecessor(lane.edgeRight, other.edgeRight))
+ + ]
347 : : {
348 : 1 : return true;
349 : : }
350 [ + + + + : 4 : if (isPredecessor(lane.edgeLeft, other.edgeRight) && isPredecessor(lane.edgeRight, other.edgeLeft))
+ + ]
351 : : {
352 : 1 : return true;
353 : : }
354 [ - + - - : 3 : if (isVanishingLaneStart(lane) || isVanishingLaneEnd(lane))
+ - ]
355 : : {
356 [ + + + - : 3 : if (isPredecessor(lane.edgeLeft, other.edgeLeft) && isPredecessor(lane.edgeRight, other.edgeLeft))
+ + ]
357 : : {
358 : 1 : return true;
359 : : }
360 [ + + + - : 2 : if (isPredecessor(lane.edgeLeft, other.edgeRight) && isPredecessor(lane.edgeRight, other.edgeRight))
+ + ]
361 : : {
362 : 1 : return true;
363 : : }
364 : : }
365 [ + - ]: 1 : if (isVanishingLaneStart(other))
366 : : {
367 [ + - - + : 1 : if (isPredecessor(lane.edgeLeft, other.edgeRight) || isPredecessor(lane.edgeRight, other.edgeRight))
- + ]
368 : : {
369 : 0 : return true;
370 : : }
371 : : }
372 : 1 : return false;
373 : : }
374 : :
375 : 9 : bool isVanishingLaneStart(Lane const &lane)
376 : : {
377 : 9 : return haveSameStart(lane.edgeLeft, lane.edgeRight);
378 : : }
379 : :
380 : 5 : bool isVanishingLaneEnd(Lane const &lane)
381 : : {
382 : 5 : return haveSameEnd(lane.edgeLeft, lane.edgeRight);
383 : : }
384 : :
385 : 5 : bool satisfiesFilter(Lane const &lane, std::string const &typeFilter, bool isHov)
386 : : {
387 [ + - ]: 5 : if (isHov == (getHOV(lane) > restriction::PassengerCount(1)))
388 : : {
389 [ + - ]: 5 : if (!typeFilter.empty())
390 : : {
391 [ + - ]: 5 : auto const laneFullTypeString = toString(lane.type);
392 [ + + ]: 5 : if (typeFilter.find(laneFullTypeString) != std::string::npos)
393 : : {
394 : 2 : return true;
395 : : }
396 : 3 : std::size_t found = laneFullTypeString.find_last_of(":");
397 [ + - ]: 3 : auto const laneTypeWithoutPrefix = laneFullTypeString.substr(found + 1);
398 [ + - ]: 3 : if (!laneTypeWithoutPrefix.empty())
399 : : {
400 [ - + ]: 3 : if (typeFilter.find(laneTypeWithoutPrefix) != std::string::npos)
401 : : {
402 : 0 : return true;
403 : : }
404 : : }
405 : : }
406 : : else
407 : : {
408 : 0 : return true;
409 : : }
410 : : }
411 : 3 : return false;
412 : : }
413 : :
414 : 23504 : void updateLaneLengths(Lane &lane)
415 : : {
416 [ + + ]: 23504 : if (isValid(lane.edgeLeft))
417 : : {
418 [ + + ]: 23502 : if (isValid(lane.edgeRight))
419 : : {
420 [ + - ]: 23501 : lane.lengthRange.minimum = std::min(lane.edgeLeft.length, lane.edgeRight.length);
421 [ + - ]: 23501 : lane.lengthRange.maximum = std::max(lane.edgeLeft.length, lane.edgeRight.length);
422 [ + - + - ]: 23501 : lane.length = (lane.edgeLeft.length + lane.edgeRight.length) * 0.5;
423 : :
424 : : auto widthRangeResult = calculateWidthRange(
425 [ + - ]: 23501 : lane.edgeLeft.ecefEdge, lane.edgeLeft.length, lane.edgeRight.ecefEdge, lane.edgeRight.length);
426 : 23501 : lane.widthRange = widthRangeResult.first;
427 : 23501 : lane.width = widthRangeResult.second;
428 : : }
429 : : else
430 : : {
431 : 1 : lane.length = lane.edgeLeft.length;
432 : 1 : lane.lengthRange.minimum = lane.length;
433 : 1 : lane.lengthRange.maximum = lane.length;
434 : 1 : lane.width = physics::Distance(0);
435 : 1 : lane.widthRange.minimum = lane.width;
436 : 1 : lane.widthRange.maximum = lane.width;
437 : : }
438 : : }
439 [ + + ]: 2 : else if (isValid(lane.edgeRight))
440 : : {
441 : 1 : lane.length = lane.edgeRight.length;
442 : 1 : lane.lengthRange.minimum = lane.length;
443 : 1 : lane.lengthRange.maximum = lane.length;
444 : 1 : lane.width = physics::Distance(0);
445 : 1 : lane.widthRange.minimum = lane.width;
446 : 1 : lane.widthRange.maximum = lane.width;
447 : : }
448 : : else
449 : : {
450 : 1 : lane.length = physics::Distance(0);
451 : 1 : lane.lengthRange.minimum = lane.length;
452 : 1 : lane.lengthRange.maximum = lane.length;
453 : 1 : lane.width = physics::Distance(0);
454 : 1 : lane.widthRange.minimum = lane.width;
455 : 1 : lane.widthRange.maximum = lane.width;
456 : : }
457 : 23504 : }
458 : :
459 : 42 : point::ENUHeading getLaneENUHeading(point::ParaPoint const &position)
460 : : {
461 [ + - ]: 42 : const auto gnssReference = access::getENUReferencePoint();
462 [ + - ]: 42 : const auto laneHeading = getLaneENUHeading(position, gnssReference);
463 : 42 : return laneHeading;
464 : : }
465 : :
466 : 23 : bool isHeadingInLaneDirection(point::ParaPoint const &position, point::ENUHeading const &heading)
467 : : {
468 [ + - ]: 23 : const auto laneHeading = getLaneENUHeading(position);
469 : : // enforce normalization of angle difference
470 [ + - + - ]: 23 : const auto headingDifference = point::createENUHeading(static_cast<double>(std::fabs(heading - laneHeading)));
471 : :
472 [ + + ]: 23 : if (static_cast<double>(std::fabs(headingDifference)) > M_PI / 2.)
473 : : {
474 : 3 : return false;
475 : : }
476 : :
477 : 20 : return true;
478 : : }
479 : :
480 : 308 : bool isLaneDirectionPositive(LaneId const &laneId)
481 : : {
482 [ + - + - ]: 616 : auto const lane = getLane(laneId);
483 : 616 : return isLaneDirectionPositive(lane);
484 : : }
485 : :
486 : 2517 : bool isLaneDirectionNegative(LaneId const &laneId)
487 : : {
488 [ + - + - ]: 5034 : auto const lane = getLane(laneId);
489 : 5034 : return isLaneDirectionNegative(lane);
490 : : }
491 : :
492 : 1 : bool projectPositionToLaneInHeadingDirection(point::ParaPoint const &position,
493 : : point::ENUHeading const &heading,
494 : : point::ParaPoint &projectedPosition)
495 : : {
496 : 1 : projectedPosition = position;
497 [ + - + - ]: 1 : if (isHeadingInLaneDirection(position, heading))
498 : : {
499 : 1 : return true;
500 : : }
501 : :
502 [ # # # # ]: 0 : const auto lane = getLane(position.laneId);
503 : 0 : ContactLocationList types;
504 [ # # # # ]: 0 : if (isLaneDirectionPositive(position.laneId))
505 : : {
506 [ # # ]: 0 : types.push_back(ContactLocation::LEFT);
507 [ # # ]: 0 : types.push_back(ContactLocation::RIGHT);
508 : : }
509 : : else
510 : : {
511 [ # # ]: 0 : types.push_back(ContactLocation::RIGHT);
512 [ # # ]: 0 : types.push_back(ContactLocation::LEFT);
513 : : }
514 [ # # ]: 0 : for (auto contactType : types)
515 : : {
516 [ # # ]: 0 : auto contactLanes = getContactLanes(lane, contactType);
517 [ # # ]: 0 : while (contactLanes.size() > 0)
518 : : {
519 : : // check first neighbor
520 : 0 : const auto neighborLaneId = contactLanes[0].toLane;
521 [ # # # # ]: 0 : const auto neighborLane = getLane(neighborLaneId);
522 : :
523 : 0 : projectedPosition.laneId = neighborLaneId;
524 : 0 : projectedPosition.parametricOffset = position.parametricOffset;
525 : :
526 [ # # # # : 0 : if (isRouteable(neighborLane) && isHeadingInLaneDirection(projectedPosition, heading))
# # # # ]
527 : : {
528 : 0 : return true;
529 : : }
530 : : else
531 : : {
532 [ # # ]: 0 : contactLanes = getContactLanes(neighborLane, contactType);
533 : : }
534 : : }
535 : : }
536 : :
537 : 0 : return false;
538 : : }
539 : :
540 : 469036 : match::MapMatchedPosition calcMapMatchedPosition(Lane const &lane,
541 : : physics::ParametricValue const &longTLeft,
542 : : physics::ParametricValue const &longTRight,
543 : : point::ECEFPoint const &pt)
544 : : {
545 [ + - ]: 469036 : match::MapMatchedPosition mmpos;
546 [ + - ]: 469036 : point::ECEFPoint const pt_left = point::getParametricPoint(lane.edgeLeft, longTLeft);
547 [ + - ]: 469036 : point::ECEFPoint const pt_right = point::getParametricPoint(lane.edgeRight, longTRight);
548 : :
549 : 469036 : mmpos.lanePoint.paraPoint.laneId = lane.id;
550 [ + - ]: 469036 : mmpos.lanePoint.lateralT = point::findNearestPointOnEdge(pt, pt_left, pt_right);
551 : 469036 : physics::ParametricValue nearestT;
552 [ + - + + ]: 469036 : if (mmpos.lanePoint.lateralT < physics::RatioValue(0.))
553 : : {
554 : 278281 : nearestT = physics::ParametricValue(0.);
555 : 278281 : mmpos.type = match::MapMatchedPositionType::LANE_LEFT;
556 : 278281 : mmpos.probability = std::max(physics::Probability(0.1),
557 [ + - ]: 556562 : physics::Probability(0.5 + static_cast<double>(mmpos.lanePoint.lateralT) / 10.));
558 : : }
559 [ + - + + ]: 190755 : else if (mmpos.lanePoint.lateralT > physics::RatioValue(1.))
560 : : {
561 : 134846 : nearestT = physics::ParametricValue(1.);
562 : 134846 : mmpos.type = match::MapMatchedPositionType::LANE_RIGHT;
563 : : mmpos.probability
564 : 134846 : = std::max(physics::Probability(0.1),
565 [ + - ]: 269692 : physics::Probability(0.5 - (static_cast<double>(mmpos.lanePoint.lateralT) - 1.) / 10.));
566 : : }
567 : : else
568 : : {
569 : 55909 : nearestT = physics::ParametricValue(static_cast<double>(mmpos.lanePoint.lateralT));
570 : 55909 : mmpos.type = match::MapMatchedPositionType::LANE_IN;
571 : 0 : mmpos.probability = physics::Probability(1.)
572 : 55909 : - std::min(physics::Probability(0.5),
573 [ + - + - ]: 111818 : physics::Probability(fabs(0.5 - static_cast<double>(mmpos.lanePoint.lateralT))));
574 : : }
575 [ + - ]: 469036 : mmpos.matchedPoint = point::vectorInterpolate(pt_left, pt_right, nearestT);
576 : : mmpos.lanePoint.paraPoint.parametricOffset
577 [ + - + - : 469036 : = nearestT * longTLeft + (physics::ParametricValue(1.) - nearestT) * longTRight;
+ - + - ]
578 : 469036 : mmpos.lanePoint.laneLength = lane.length;
579 [ + - ]: 469036 : mmpos.lanePoint.laneWidth = point::distance(pt_left, pt_right);
580 : 469036 : mmpos.queryPoint = pt;
581 [ + - ]: 469036 : mmpos.matchedPointDistance = point::distance(mmpos.matchedPoint, mmpos.queryPoint);
582 : 938072 : return mmpos;
583 : : }
584 : :
585 : 469036 : bool findNearestPointOnLane(Lane const &lane, point::ECEFPoint const &pt, match::MapMatchedPosition &mmpos)
586 : : {
587 [ + - ]: 469036 : auto const longTLeft = findNearestPointOnEdge(lane.edgeLeft, pt);
588 [ + - ]: 469036 : if (longTLeft.isValid())
589 : : {
590 [ + - ]: 469036 : auto const longTRight = findNearestPointOnEdge(lane.edgeRight, pt);
591 [ + - ]: 469036 : if (longTRight.isValid())
592 : : {
593 [ + - ]: 469036 : mmpos = calcMapMatchedPosition(lane, longTLeft, longTRight, pt);
594 : 469036 : return true;
595 : : }
596 : : }
597 : 0 : return false;
598 : : }
599 : :
600 : 0 : bool findNearestPointOnLaneInterval(route::LaneInterval const &laneInterval,
601 : : point::ECEFPoint const &pt,
602 : : match::MapMatchedPosition &mmpos)
603 : : {
604 [ # # ]: 0 : auto const &lane = getLane(laneInterval.laneId);
605 [ # # ]: 0 : auto const laneRange = route::toParametricRange(laneInterval);
606 [ # # ]: 0 : auto longTLeft = findNearestPointOnEdge(lane.edgeLeft, pt);
607 [ # # ]: 0 : if (longTLeft.isValid())
608 : : {
609 [ # # ]: 0 : auto longTRight = findNearestPointOnEdge(lane.edgeRight, pt);
610 [ # # ]: 0 : if (longTRight.isValid())
611 : : {
612 [ # # # # ]: 0 : if (!physics::isWithinRange(laneRange, longTLeft))
613 : : {
614 [ # # # # ]: 0 : if (longTLeft < laneRange.minimum)
615 : : {
616 : 0 : longTLeft = laneRange.minimum;
617 : : }
618 [ # # # # ]: 0 : else if (longTLeft > laneRange.maximum)
619 : : {
620 : 0 : longTLeft = laneRange.maximum;
621 : : }
622 : : }
623 [ # # # # ]: 0 : if (!physics::isWithinRange(laneRange, longTRight))
624 : : {
625 [ # # # # ]: 0 : if (longTRight < laneRange.minimum)
626 : : {
627 : 0 : longTRight = laneRange.minimum;
628 : : }
629 [ # # # # ]: 0 : else if (longTRight > laneRange.maximum)
630 : : {
631 : 0 : longTRight = laneRange.maximum;
632 : : }
633 : : }
634 : :
635 [ # # ]: 0 : mmpos = calcMapMatchedPosition(lane, longTLeft, longTRight, pt);
636 : 0 : return true;
637 : : }
638 : : }
639 : 0 : return false;
640 : : }
641 : :
642 : 2 : physics::Distance getDistanceToLane(LaneId laneId, match::Object const &object)
643 : : {
644 : : // prefer fast checks first
645 [ + + ]: 3 : for (auto const &occupiedLane : object.mapMatchedBoundingBox.laneOccupiedRegions)
646 : : {
647 [ + - + + ]: 2 : if (occupiedLane.laneId == laneId)
648 : : {
649 : 1 : return physics::Distance(0.);
650 : : }
651 : : }
652 : : // already map matching result available, but not within the lane
653 : 1 : physics::Distance distance = std::numeric_limits<physics::Distance>::max();
654 : 1 : bool mapMatchedPointFound = false;
655 : : point::ECEFPoint const objectPointECEF
656 [ + - ]: 1 : = point::toECEF(object.enuPosition.centerPoint, object.enuPosition.enuReferencePoint);
657 : :
658 : 4 : for (auto referencePoint : {match::ObjectReferencePoints::FrontLeft,
659 : : match::ObjectReferencePoints::FrontRight,
660 : : match::ObjectReferencePoints::RearLeft,
661 [ + + ]: 5 : match::ObjectReferencePoints::RearRight})
662 : : {
663 [ + - ]: 8 : auto mapMatchedConfidenceList = object.mapMatchedBoundingBox.referencePointPositions[size_t(referencePoint)];
664 [ + + ]: 10 : for (auto const &mapMatchedPosition : mapMatchedConfidenceList)
665 : : {
666 [ + - + + ]: 6 : if (mapMatchedPosition.lanePoint.paraPoint.laneId == laneId)
667 : : {
668 : 2 : mapMatchedPointFound = true;
669 : : // use the smallest distance if multiple have been matched
670 [ + - + - ]: 2 : distance = std::min(distance, point::distance(mapMatchedPosition.matchedPoint, objectPointECEF));
671 : : }
672 : : }
673 : : }
674 [ - + ]: 1 : if (!mapMatchedPointFound)
675 : : {
676 : : // need to perform map matching on our own
677 [ # # # # ]: 0 : auto lane = getLane(laneId);
678 [ # # ]: 0 : match::MapMatchedPosition mapMatchedPosition;
679 [ # # # # ]: 0 : if (findNearestPointOnLane(lane, objectPointECEF, mapMatchedPosition))
680 : : {
681 [ # # ]: 0 : distance = point::distance(mapMatchedPosition.matchedPoint, objectPointECEF);
682 : : distance
683 [ # # # # : 0 : = std::max(distance - std::max(object.enuPosition.dimension.length, object.enuPosition.dimension.width) / 2.,
# # ]
684 [ # # ]: 0 : physics::Distance(0.));
685 : : }
686 : : }
687 : :
688 : 1 : return distance;
689 : : }
690 : :
691 : 155 : physics::Distance calcLength(LaneId const &laneId)
692 : : {
693 [ + - ]: 155 : const auto lane = getLanePtr(laneId);
694 : 310 : return lane->length;
695 : : }
696 : :
697 : 1 : physics::Distance calcLength(match::LaneOccupiedRegion const &laneOccupiedRegion)
698 : : {
699 : : auto const rangeLength
700 [ + - ]: 1 : = (laneOccupiedRegion.longitudinalRange.maximum - laneOccupiedRegion.longitudinalRange.minimum);
701 [ + - + - ]: 2 : return rangeLength * calcLength(laneOccupiedRegion.laneId);
702 : : }
703 : :
704 : 4 : physics::Distance calcWidth(point::ParaPoint const ¶Point)
705 : : {
706 : 4 : return calcWidth(paraPoint.laneId, paraPoint.parametricOffset);
707 : : }
708 : :
709 : 5 : physics::Distance calcWidth(LaneId const &laneId, physics::ParametricValue const &longOffset)
710 : : {
711 [ + - ]: 10 : const auto lane = getLanePtr(laneId);
712 [ + - ]: 10 : return getWidth(*lane, longOffset);
713 : : }
714 : :
715 : 2 : physics::Distance calcWidth(point::ENUPoint const &enuPoint)
716 : : {
717 [ + - ]: 4 : match::AdMapMatching mapMatching;
718 : : const auto mapMatchedPositions
719 [ + - ]: 4 : = mapMatching.getMapMatchedPositions(enuPoint, physics::Distance(1.0), physics::Probability(0.1));
720 : :
721 [ - + ]: 2 : if (mapMatchedPositions.empty())
722 : : {
723 : 0 : return physics::Distance(-1.0);
724 : : }
725 [ + - ]: 2 : return calcWidth(mapMatchedPositions.front().lanePoint.paraPoint);
726 : : }
727 : :
728 : 1 : physics::Distance calcWidth(match::LaneOccupiedRegion const &laneOccupiedRegion)
729 : : {
730 [ + - ]: 1 : auto const rangeWidth = (laneOccupiedRegion.lateralRange.maximum - laneOccupiedRegion.lateralRange.minimum);
731 [ + - + - : 2 : return rangeWidth * calcWidth(match::getCenterParaPoint(laneOccupiedRegion));
+ - ]
732 : : }
733 : :
734 : 19063 : ContactLocation getDirectNeighborhoodRelation(LaneId const laneId, LaneId const checkLaneId)
735 : : {
736 [ + - + + ]: 19063 : if (laneId == checkLaneId)
737 : : {
738 : 10724 : return ContactLocation::OVERLAP;
739 : : }
740 [ + + + - ]: 16677 : auto lane = getLane(laneId);
741 : 19434 : for (auto contactLocation :
742 [ + + ]: 27772 : {ContactLocation::LEFT, ContactLocation::RIGHT, ContactLocation::SUCCESSOR, ContactLocation::PREDECESSOR})
743 : : {
744 [ + - ]: 27770 : auto contactLanes = getContactLanes(lane, contactLocation);
745 : : auto findResult
746 : 23602 : = std::find_if(std::begin(contactLanes), std::end(contactLanes), [checkLaneId](ContactLane const &contactLane) {
747 : 23602 : return contactLane.toLane == checkLaneId;
748 [ + - ]: 27770 : });
749 [ + + ]: 27770 : if (findResult != std::end(contactLanes))
750 : : {
751 : 8336 : return contactLocation;
752 : : }
753 : : }
754 : 2 : return ContactLocation::INVALID;
755 : : }
756 : :
757 : 1 : bool isSuccessorOrPredecessor(LaneId const laneId, LaneId const checkLaneId)
758 : : {
759 : 1 : auto const neighborhood = getDirectNeighborhoodRelation(laneId, checkLaneId);
760 [ + - - + ]: 1 : return (neighborhood == ContactLocation::SUCCESSOR) || (neighborhood == ContactLocation::PREDECESSOR);
761 : : }
762 : :
763 : 4 : bool isSameOrDirectNeighbor(LaneId const laneId, LaneId const checkLaneId)
764 : : {
765 : 4 : auto const neighborhood = getDirectNeighborhoodRelation(laneId, checkLaneId);
766 [ + + ]: 2 : return (neighborhood == ContactLocation::OVERLAP) || (neighborhood == ContactLocation::LEFT)
767 [ + + + - ]: 5 : || (neighborhood == ContactLocation::RIGHT);
768 : : }
769 : :
770 : 11 : point::ENUPoint getENULanePoint(point::ParaPoint const parametricPoint, physics::ParametricValue const &lateralOffset)
771 : : {
772 : : // perform map matching
773 [ + - + - ]: 22 : auto lane = getLane(parametricPoint.laneId);
774 [ + - ]: 11 : auto ecefPoint = getParametricPoint(lane, parametricPoint.parametricOffset, lateralOffset);
775 [ + - ]: 11 : auto enuPoint = point::toENU(ecefPoint);
776 : 22 : return enuPoint;
777 : : }
778 : :
779 : 564 : LaneAltitudeRange calcLaneAltitudeRange(Lane const &lane)
780 : : {
781 : 564 : LaneAltitudeRange altitude_range;
782 : 564 : bool initialize = true;
783 [ + + ]: 6004 : for (auto const &pt : lane.edgeLeft.ecefEdge)
784 : : {
785 [ + - ]: 5440 : auto const pt_geo = point::toGeo(pt);
786 [ + + ]: 5440 : if (initialize)
787 : : {
788 : 564 : initialize = false;
789 : 564 : altitude_range.minimum = pt_geo.altitude;
790 : 564 : altitude_range.maximum = pt_geo.altitude;
791 : : }
792 : : else
793 : : {
794 [ + - ]: 4876 : altitude_range.minimum = std::min(altitude_range.minimum, pt_geo.altitude);
795 [ + - ]: 4876 : altitude_range.maximum = std::max(altitude_range.maximum, pt_geo.altitude);
796 : : }
797 : : }
798 [ + + ]: 6004 : for (auto const &pt : lane.edgeRight.ecefEdge)
799 : : {
800 [ + - ]: 5440 : auto const pt_geo = point::toGeo(pt);
801 [ + - ]: 5440 : altitude_range.minimum = std::min(altitude_range.minimum, pt_geo.altitude);
802 [ + - ]: 5440 : altitude_range.maximum = std::max(altitude_range.maximum, pt_geo.altitude);
803 : : }
804 : 564 : return altitude_range;
805 : : }
806 : :
807 : 69559 : bool isLaneRelevantForExpansion(lane::LaneId const &laneId, lane::LaneIdSet const &relevantLanes)
808 : : {
809 [ + + ]: 69559 : if (relevantLanes.empty())
810 : : {
811 : 69519 : return true;
812 : : }
813 [ + - ]: 40 : auto const findResult = relevantLanes.find(laneId);
814 [ + + ]: 40 : if (findResult != relevantLanes.end())
815 : : {
816 : 22 : return true;
817 : : }
818 : 18 : return false;
819 : : }
820 : :
821 : : } // namespace lane
822 : : } // namespace map
823 : : } // namespace ad
|
