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/match/AdMapMatching.hpp"
10 : :
11 : : #include <algorithm>
12 : : #include <cmath>
13 : : #include <functional>
14 : : #include "ad/map/access/Logging.hpp"
15 : : #include "ad/map/access/Operation.hpp"
16 : : #include "ad/map/lane/LaneOperation.hpp"
17 : : #include "ad/map/match/MapMatchedOperation.hpp"
18 : : #include "ad/map/point/Transform.hpp"
19 : : #include "ad/physics/RangeOperation.hpp"
20 : :
21 : : namespace ad {
22 : : namespace map {
23 : : namespace match {
24 : :
25 : : match::MapMatchedPositionConfidenceList
26 : 21576 : AdMapMatching::findLanesInputChecked(std::vector<lane::Lane::ConstPtr> const &relevantLanes,
27 : : point::ECEFPoint const &ecefPoint,
28 : : physics::Distance const &distance)
29 : : {
30 : 21576 : match::MapMatchedPositionConfidenceList mapMatchingResults;
31 : 21576 : physics::Probability probabilitySum(0.);
32 [ + + ]: 490589 : for (auto lane : relevantLanes)
33 : : {
34 [ + - ]: 469013 : MapMatchedPosition mmpt;
35 [ + - + - ]: 469013 : if (lane::findNearestPointOnLane(*lane, ecefPoint, mmpt))
36 : : {
37 [ + - + + ]: 469013 : if (mmpt.matchedPointDistance <= distance)
38 : : {
39 [ + - ]: 126201 : mapMatchingResults.push_back(mmpt);
40 [ + - ]: 126201 : probabilitySum += mmpt.probability;
41 : : }
42 : : }
43 : : }
44 : :
45 [ + - ]: 21576 : normalizeResults(mapMatchingResults, probabilitySum);
46 : 43152 : return mapMatchingResults;
47 : : }
48 : :
49 : 21580 : void AdMapMatching::normalizeResults(match::MapMatchedPositionConfidenceList &mapMatchingResults,
50 : : physics::Probability const &probabilitySum)
51 : : {
52 : : // normalize result probabilities
53 [ + - + + ]: 21580 : if (probabilitySum > physics::Probability(0.01))
54 : : {
55 [ + + ]: 147780 : for (auto &mmpt : mapMatchingResults)
56 : : {
57 [ + - ]: 126205 : mmpt.probability = mmpt.probability / static_cast<double>(probabilitySum);
58 : : }
59 : : }
60 : :
61 : : // sort the final results
62 [ + - ]: 21580 : std::sort(std::begin(mapMatchingResults),
63 : : std::end(mapMatchingResults),
64 : 308771 : [](MapMatchedPosition const &left, MapMatchedPosition const &right) {
65 : 308771 : return left.probability > right.probability;
66 : : });
67 : 21580 : }
68 : :
69 : : match::MapMatchedPositionConfidenceList
70 : 4 : AdMapMatching::findLanesInputCheckedAltitudeUnknown(point::GeoPoint const &geo_point,
71 : : physics::Distance const &distance,
72 : : ::ad::map::lane::LaneIdSet const &relevantLaneSet)
73 : : {
74 : 4 : match::MapMatchedPositionConfidenceList map_matching_results;
75 : 4 : physics::Probability probability_sum(0.);
76 : 8 : std::vector<lane::Lane::ConstPtr> relevantLanes;
77 [ - + ]: 4 : if (!relevantLaneSet.empty())
78 : : {
79 [ # # ]: 0 : for (auto laneId : relevantLaneSet)
80 : : {
81 [ # # # # ]: 0 : auto lane = access::getStore().getLanePtr(laneId);
82 [ # # ]: 0 : if (lane)
83 : : {
84 [ # # ]: 0 : relevantLanes.push_back(lane);
85 : : }
86 : : }
87 : : }
88 : : else
89 : : {
90 [ + - + - : 568 : for (auto laneId : access::getStore().getLanes())
+ + ]
91 : : {
92 [ + - + - ]: 1128 : auto lane = access::getStore().getLanePtr(laneId);
93 [ + - ]: 564 : if (lane)
94 : : {
95 [ + - ]: 564 : relevantLanes.push_back(lane);
96 : : }
97 : : }
98 : : }
99 [ + + ]: 568 : for (auto lane : relevantLanes)
100 : : {
101 [ + - ]: 564 : auto const altitude_range = calcLaneAltitudeRange(*lane);
102 : 564 : auto geo_point_check = geo_point;
103 [ + - + - ]: 564 : auto const altitude_delta_2 = (altitude_range.maximum - altitude_range.minimum) / 2.;
104 [ + - ]: 564 : geo_point_check.altitude = altitude_range.minimum + altitude_delta_2;
105 [ + - ]: 564 : point::BoundingSphere matchingSphere;
106 [ + - ]: 564 : matchingSphere.center = point::toECEF(geo_point_check);
107 [ + - ]: 564 : matchingSphere.radius = distance + physics::Distance(static_cast<double>(altitude_delta_2));
108 [ + - + + ]: 564 : if (lane::isNear(*lane, matchingSphere))
109 : : {
110 [ + - ]: 16 : MapMatchedPosition mmpt;
111 [ + - + - ]: 16 : if (lane::findNearestPointOnLane(*lane, matchingSphere.center, mmpt))
112 : : {
113 [ + - + + ]: 16 : if (mmpt.matchedPointDistance <= matchingSphere.radius)
114 : : {
115 : : // now correct query point and assume the matched point's altitude to perform final local decision
116 [ + - ]: 4 : geo_point_check.altitude = point::toGeo(mmpt.matchedPoint).altitude;
117 [ + - ]: 4 : matchingSphere.center = point::toECEF(geo_point_check);
118 [ + - + - ]: 4 : if (lane::findNearestPointOnLane(*lane, matchingSphere.center, mmpt))
119 : : {
120 [ + - + - ]: 4 : if (mmpt.matchedPointDistance <= distance)
121 : : {
122 [ + - ]: 4 : map_matching_results.push_back(mmpt);
123 [ + - ]: 4 : probability_sum += mmpt.probability;
124 : : }
125 : : }
126 : : }
127 : : }
128 : : }
129 : : }
130 [ + - ]: 4 : normalizeResults(map_matching_results, probability_sum);
131 : 8 : return map_matching_results;
132 : : }
133 : :
134 : : std::vector<lane::Lane::ConstPtr>
135 : 21275 : AdMapMatching::getRelevantLanesInputChecked(point::ECEFPoint const &ecefPoint,
136 : : physics::Distance const &distance,
137 : : ::ad::map::lane::LaneIdSet const &relevantLaneSet)
138 : : {
139 : 21275 : std::vector<lane::Lane::ConstPtr> relevantLanes;
140 [ + + ]: 21275 : if (!relevantLaneSet.empty())
141 : : {
142 [ + + ]: 485248 : for (auto laneId : relevantLaneSet)
143 : : {
144 [ + - + - ]: 929696 : auto lane = access::getStore().getLanePtr(laneId);
145 [ + - ]: 464848 : if (lane)
146 : : {
147 [ + - ]: 464848 : relevantLanes.push_back(lane);
148 : : }
149 : : }
150 : : }
151 : : else
152 : : {
153 [ + - ]: 875 : point::BoundingSphere matchingSphere;
154 : 875 : matchingSphere.center = ecefPoint;
155 : 875 : matchingSphere.radius = distance;
156 [ + - + - : 129503 : for (auto laneId : access::getStore().getLanes())
+ + ]
157 : : {
158 [ + - + - ]: 257256 : auto lane = access::getStore().getLanePtr(laneId);
159 : :
160 [ + - ]: 128628 : if (lane)
161 : : {
162 [ + - + + ]: 128628 : if (lane::isNear(*lane, matchingSphere))
163 : : {
164 [ + - ]: 2419 : relevantLanes.push_back(lane);
165 : : }
166 : : }
167 : : }
168 : : }
169 : :
170 : 21275 : return relevantLanes;
171 : : }
172 : :
173 : : match::MapMatchedPositionConfidenceList
174 : 21258 : AdMapMatching::findLanesInputChecked(point::ECEFPoint const &ecefPoint,
175 : : physics::Distance const &distance,
176 : : ::ad::map::lane::LaneIdSet const &relevantLaneSet)
177 : : {
178 [ + - ]: 42516 : return findLanesInputChecked(getRelevantLanesInputChecked(ecefPoint, distance, relevantLaneSet), ecefPoint, distance);
179 : : }
180 : :
181 : 21259 : match::MapMatchedPositionConfidenceList AdMapMatching::findLanes(point::GeoPoint const &geoPoint,
182 : : physics::Distance const &distance,
183 : : ::ad::map::lane::LaneIdSet const &relevantLaneSet)
184 : : {
185 [ - + ]: 21259 : if (!distance.isValid())
186 : : {
187 [ # # ]: 0 : access::getLogger()->error("Invalid radius passed to AdMapMatching::findLanes(): {}", distance);
188 : 0 : return MapMatchedPositionConfidenceList();
189 : : }
190 [ + + ]: 21259 : if (geoPoint.altitude == point::AltitudeUnknown)
191 : : {
192 [ + - - + : 4 : if (!withinValidInputRange(geoPoint.latitude) || !withinValidInputRange(geoPoint.longitude))
- + ]
193 : : {
194 [ # # ]: 0 : access::getLogger()->error(
195 : : "Invalid Latitude/Longitude of Geo Point passed to AdMapMatching::findLanes() with AltitudeUnknown: {}",
196 : : geoPoint);
197 : 0 : return MapMatchedPositionConfidenceList();
198 : : }
199 : 4 : return findLanesInputCheckedAltitudeUnknown(geoPoint, distance, relevantLaneSet);
200 : : }
201 : : else
202 : : {
203 [ - + ]: 21255 : if (!isValid(geoPoint))
204 : : {
205 [ # # ]: 0 : access::getLogger()->error("Invalid Geo Point passed to AdMapMatching::findLanes(): {}", geoPoint);
206 : 0 : return MapMatchedPositionConfidenceList();
207 : : }
208 [ + - ]: 42510 : return findLanesInputChecked(point::toECEF(geoPoint), distance, relevantLaneSet);
209 : : }
210 : : }
211 : :
212 : 5 : match::MapMatchedPositionConfidenceList AdMapMatching::findLanes(point::ECEFPoint const &ecefPoint,
213 : : physics::Distance const &distance,
214 : : ::ad::map::lane::LaneIdSet const &relevantLaneSet)
215 : : {
216 [ + + ]: 5 : if (!isValid(ecefPoint))
217 : : {
218 [ + - ]: 2 : access::getLogger()->error("Invalid ECEF Point passed to AdMapMatching::findLanes(): {}", ecefPoint);
219 : 1 : return MapMatchedPositionConfidenceList();
220 : : }
221 [ + + ]: 4 : if (!distance.isValid())
222 : : {
223 [ + - ]: 2 : access::getLogger()->error("Invalid radius passed to AdMapMatching::findLanes(): {}", distance);
224 : 1 : return MapMatchedPositionConfidenceList();
225 : : }
226 : 3 : return findLanesInputChecked(ecefPoint, distance, relevantLaneSet);
227 : : }
228 : :
229 : 1990 : AdMapMatching::AdMapMatching()
230 : : {
231 : 1990 : }
232 : :
233 : 6 : point::ENUHeading AdMapMatching::getLaneENUHeading(MapMatchedPosition const &mapMatchedPosition) const
234 : : {
235 : 6 : return lane::getLaneENUHeading(mapMatchedPosition);
236 : : }
237 : :
238 : 125728 : bool AdMapMatching::isLanePartOfRouteHints(lane::LaneId const &laneId) const
239 : : {
240 [ + + ]: 125983 : for (auto const &route : mRouteHints)
241 : : {
242 [ + + ]: 294 : for (auto const &roadSegment : route.roadSegments)
243 : : {
244 [ + + ]: 72 : for (auto const &laneSegment : roadSegment.drivableLaneSegments)
245 : : {
246 [ + - + + ]: 39 : if (laneSegment.laneInterval.laneId == laneId)
247 : : {
248 : 6 : return true;
249 : : }
250 : : }
251 : : }
252 : : }
253 : 125722 : return false;
254 : : }
255 : :
256 : 125726 : double AdMapMatching::getHeadingFactor(MapMatchedPosition const &matchedPosition) const
257 : : {
258 : 125726 : double headingFactor = 1.0;
259 : :
260 [ + + ]: 125726 : if (mHeadingHints.size() > 0u)
261 : : {
262 [ + - ]: 123857 : point::ECEFHeading const laneDrivingDirection = lane::getLaneECEFHeading(matchedPosition);
263 [ + + ]: 511266 : for (auto const &headingHint : mHeadingHints)
264 : : {
265 : : // -1 <= dot product <= 1 (cosine between the two directional vectors)
266 [ + - ]: 387409 : auto dotProduct = headingHint * laneDrivingDirection;
267 : :
268 : : // pushes the 1. <= headingFactor <= mHeadingHintFactor
269 : 387409 : double newHeadingFactor = 1. + (dotProduct + 1.) / 2. * mHeadingHintFactor;
270 : 387409 : headingFactor = std::max(headingFactor, newHeadingFactor);
271 : : }
272 : :
273 [ + - + - ]: 247714 : access::getLogger()->trace("getHeadingFactor {}, {}", matchedPosition.lanePoint.paraPoint.laneId, headingFactor);
274 : : }
275 : 125726 : return headingFactor;
276 : : }
277 : :
278 : 21259 : MapMatchedPositionConfidenceList AdMapMatching::getMapMatchedPositions(point::GeoPoint const &geoPoint,
279 : : physics::Distance const &distance,
280 : : physics::Probability const &minProbability) const
281 : : {
282 : 21259 : auto mapMatchingResult = findLanes(geoPoint, distance, mRelevantLanes);
283 [ + - ]: 21259 : mapMatchingResult = considerMapMatchingHints(mapMatchingResult, minProbability);
284 [ + - + - ]: 42518 : access::getLogger()->trace("MapMatching result {}", mapMatchingResult);
285 : 21259 : return mapMatchingResult;
286 : : }
287 : :
288 : 20404 : MapMatchedPositionConfidenceList AdMapMatching::getMapMatchedPositions(point::ENUPoint const &enuPoint,
289 : : physics::Distance const &distance,
290 : : physics::Probability const &minProbability) const
291 : : {
292 [ + - ]: 40808 : return getMapMatchedPositions(point::toGeo(enuPoint), distance, minProbability);
293 : : }
294 : :
295 : 2 : MapMatchedPositionConfidenceList AdMapMatching::getMapMatchedPositions(point::ENUPoint const &enuPoint,
296 : : point::GeoPoint const &enuReferencePoint,
297 : : physics::Distance const &distance,
298 : : physics::Probability const &minProbability) const
299 : : {
300 [ + - ]: 4 : return getMapMatchedPositions(point::toGeo(enuPoint, enuReferencePoint), distance, minProbability);
301 : : }
302 : :
303 : 2 : MapMatchedPositionConfidenceList AdMapMatching::getMapMatchedPositions(ENUObjectPosition const &enuObjectPosition,
304 : : physics::Distance const &distance,
305 : : physics::Probability const &minProbability)
306 : : {
307 : 2 : addHeadingHint(enuObjectPosition.heading, enuObjectPosition.enuReferencePoint);
308 : : auto mapMatchedPositions = getMapMatchedPositions(
309 : 2 : enuObjectPosition.centerPoint, enuObjectPosition.enuReferencePoint, distance, minProbability);
310 : 2 : clearHeadingHints();
311 : 2 : return mapMatchedPositions;
312 : : }
313 : :
314 : : MapMatchedPositionConfidenceList
315 : 21259 : AdMapMatching::considerMapMatchingHints(MapMatchedPositionConfidenceList const &mapMatchedPositions,
316 : : physics::Probability const &minProbability) const
317 : : {
318 [ + - + - ]: 42518 : access::getLogger()->trace("considerMapMatchingHints {}", mapMatchedPositions);
319 : :
320 : : // revised probabilities can be > 1. while calculating
321 : 21259 : physics::Probability revisedProbabilitySum(0.);
322 : 21259 : MapMatchedPositionConfidenceList mapMatchingResults;
323 [ + - ]: 21259 : mapMatchingResults.reserve(mapMatchedPositions.size());
324 [ + + ]: 146985 : for (auto const &mapMatchingResult : mapMatchedPositions)
325 : : {
326 : 125726 : auto revisedProbability = mapMatchingResult.probability;
327 [ + - ]: 125726 : if (match::isLaneType(mapMatchingResult.type))
328 : : {
329 : : // probability of lanes on current route are multiplied by a certain factor
330 [ + - + + ]: 125726 : if (isLanePartOfRouteHints(mapMatchingResult.lanePoint.paraPoint.laneId))
331 : : {
332 [ + - + - ]: 10 : access::getLogger()->trace("AdMapMatching::considerMapMatchingHints {} is on current route",
333 : 5 : mapMatchingResult.lanePoint.paraPoint.laneId);
334 [ + - ]: 5 : revisedProbability = revisedProbability * getRouteHintFactor();
335 : : }
336 : :
337 : : // add heading hint factor
338 [ + - + - ]: 125726 : revisedProbability = revisedProbability * getHeadingFactor(mapMatchingResult);
339 : : }
340 : :
341 [ + - ]: 125726 : revisedProbabilitySum = revisedProbabilitySum + revisedProbability;
342 [ + - ]: 125726 : mapMatchingResults.push_back(mapMatchingResult);
343 : 125726 : mapMatchingResults.back().probability = revisedProbability;
344 : : }
345 : :
346 : : // post processing
347 : 1 : do
348 : : {
349 : 21260 : auto const probabilityDivisor = revisedProbabilitySum;
350 [ + - + + ]: 21260 : if (probabilityDivisor == physics::Probability(0.))
351 : : {
352 : 5 : break;
353 : : }
354 : 21255 : revisedProbabilitySum = physics::Probability(1.);
355 : 21255 : size_t writeIndex = 0u;
356 [ + + ]: 146982 : for (size_t readIndex = 0u; readIndex < mapMatchingResults.size(); ++readIndex)
357 : : {
358 [ - + ]: 125727 : if (writeIndex != readIndex)
359 : : {
360 : 0 : mapMatchingResults[writeIndex] = mapMatchingResults[readIndex];
361 : : }
362 : 125727 : mapMatchingResults[writeIndex].probability
363 [ + - ]: 125727 : = mapMatchingResults[writeIndex].probability / static_cast<double>(probabilityDivisor);
364 [ + - + + ]: 125727 : if (mapMatchingResults[writeIndex].probability >= minProbability)
365 : : {
366 : : // write index only incremented if new probability is above minProbability
367 : 125726 : writeIndex++;
368 : : }
369 : : else
370 : : {
371 : : // otherwise the entry is dropped so the probabilities of the others have to be adapted next loop
372 [ + - ]: 1 : revisedProbabilitySum = revisedProbabilitySum - mapMatchingResults[writeIndex].probability;
373 : : }
374 : : }
375 [ + - ]: 21255 : mapMatchingResults.resize(writeIndex);
376 [ + - + + ]: 21255 : } while (revisedProbabilitySum < physics::Probability(1.));
377 : :
378 : : // sort the final results
379 [ + - ]: 21259 : std::sort(std::begin(mapMatchingResults),
380 : : std::end(mapMatchingResults),
381 : 249409 : [](MapMatchedPosition const &left, MapMatchedPosition const &right) {
382 : 249409 : return left.probability > right.probability;
383 : : });
384 : 42518 : return mapMatchingResults;
385 : : }
386 : :
387 : 17 : MapMatchedObjectBoundingBox AdMapMatching::getMapMatchedBoundingBox(ENUObjectPosition const &enuObjectPosition,
388 : : physics::Distance const &samplingDistance) const
389 : : {
390 [ + - ]: 17 : MapMatchedObjectBoundingBox mapMatchedObjectBoundingBox;
391 : 17 : mapMatchedObjectBoundingBox.samplingDistance = samplingDistance;
392 : : mapMatchedObjectBoundingBox.matchRadius
393 [ + - + - : 17 : = samplingDistance + (0.5 * enuObjectPosition.dimension.length) + (0.5 * enuObjectPosition.dimension.width);
+ - + - ]
394 : :
395 : : // if the vehicle covers multiple lanes
396 : : // the occupied regions usually don't span up to the borders
397 : : // To ensure that longitudinally and laterally we don't miss parts of the lane in
398 : : // between the the sampling points, we have to ensure: point matching distance >= sampling distance
399 : :
400 : : // directly calculate in ECEF to get rid of unnecessary coordinate transformations
401 [ + - ]: 17 : point::ENUPoint directionalVectorENU;
402 [ + - ]: 17 : point::ENUPoint orthogonalVectorENU;
403 [ + - ]: 17 : point::getDirectionVectorsZPlane(enuObjectPosition.heading, directionalVectorENU, orthogonalVectorENU);
404 : :
405 [ + - ]: 17 : point::ECEFPoint const start = toECEF(enuObjectPosition.enuReferencePoint);
406 [ + - ]: 17 : point::ECEFPoint const directionalVectorEnd = toECEF(directionalVectorENU, enuObjectPosition.enuReferencePoint);
407 [ + - + - ]: 17 : point::ECEFPoint const directionalVector = vectorNorm(directionalVectorEnd - start);
408 [ + - ]: 17 : point::ECEFPoint const orthogonalVectorEnd = toECEF(orthogonalVectorENU, enuObjectPosition.enuReferencePoint);
409 [ + - + - ]: 17 : point::ECEFPoint const orthogonalVector = vectorNorm(orthogonalVectorEnd - start);
410 : :
411 [ + - + - ]: 17 : point::ECEFPoint directionalLength = directionalVector * (0.5 * enuObjectPosition.dimension.length);
412 [ + - + - ]: 17 : point::ECEFPoint directionalWidth = orthogonalVector * (0.5 * enuObjectPosition.dimension.width);
413 : :
414 [ + + + - ]: 102 : point::ECEFPoint referencePoints[int32_t(ObjectReferencePoints::NumPoints)];
415 : :
416 : : referencePoints[int32_t(ObjectReferencePoints::Center)]
417 [ + - ]: 17 : = toECEF(enuObjectPosition.centerPoint, enuObjectPosition.enuReferencePoint);
418 : :
419 : : referencePoints[int32_t(ObjectReferencePoints::FrontLeft)]
420 [ + - + - ]: 17 : = (referencePoints[int32_t(ObjectReferencePoints::Center)] + directionalLength) + directionalWidth;
421 : : referencePoints[int32_t(ObjectReferencePoints::FrontRight)]
422 [ + - + - ]: 17 : = (referencePoints[int32_t(ObjectReferencePoints::Center)] + directionalLength) - directionalWidth;
423 : : referencePoints[int32_t(ObjectReferencePoints::RearLeft)]
424 [ + - + - ]: 17 : = (referencePoints[int32_t(ObjectReferencePoints::Center)] - directionalLength) + directionalWidth;
425 : : referencePoints[int32_t(ObjectReferencePoints::RearRight)]
426 [ + - + - ]: 17 : = (referencePoints[int32_t(ObjectReferencePoints::Center)] - directionalLength) - directionalWidth;
427 : :
428 [ - + ]: 17 : if (!samplingDistance.isValid())
429 : : {
430 [ # # # # ]: 0 : access::getLogger()->error("Invalid sampling distance passed to AdMapMatching::getMapMatchedBoundingBox(): {}",
431 : : samplingDistance);
432 : 0 : return mapMatchedObjectBoundingBox;
433 : : }
434 [ + + ]: 102 : for (size_t i = 0; i < size_t(ObjectReferencePoints::NumPoints); i++)
435 : : {
436 [ + - - + ]: 85 : if (!isValid(referencePoints[i]))
437 : : {
438 [ # # # # ]: 0 : access::getLogger()->error("Invalid reference point within AdMapMatching::getMapMatchedBoundingBox(): {}",
439 : 0 : referencePoints[i]);
440 : 0 : return mapMatchedObjectBoundingBox;
441 : : }
442 : : }
443 : :
444 : : // filter lanes on large scale first
445 : : auto const relevantLanes = getRelevantLanesInputChecked(
446 [ + - ]: 34 : referencePoints[int32_t(ObjectReferencePoints::Center)], mapMatchedObjectBoundingBox.matchRadius, mRelevantLanes);
447 : :
448 [ + - ]: 17 : mapMatchedObjectBoundingBox.referencePointPositions.resize(size_t(ObjectReferencePoints::NumPoints));
449 : :
450 : : // we have to ensure that the confidence list used for the calculation of the lane regions
451 : : // covers ALL matching samples at once!
452 : : // Otherwise the fill-up to inner borders of the region is not working properly
453 : 34 : MapMatchedPositionConfidenceList regionConfidenceList;
454 [ + + ]: 102 : for (size_t i = 0; i < size_t(ObjectReferencePoints::NumPoints); i++)
455 : : {
456 : 85 : mapMatchedObjectBoundingBox.referencePointPositions[i]
457 [ + - ]: 170 : = findLanesInputChecked(relevantLanes, referencePoints[i], samplingDistance);
458 : 85 : regionConfidenceList.insert(regionConfidenceList.end(),
459 : 85 : mapMatchedObjectBoundingBox.referencePointPositions[i].begin(),
460 [ + - ]: 255 : mapMatchedObjectBoundingBox.referencePointPositions[i].end());
461 : : }
462 : :
463 : : // calculate actual sampling stride
464 : : // stride below 10cm doesn't actually make sense
465 [ + - ]: 17 : physics::Distance const stride = std::max(samplingDistance, physics::Distance(0.1));
466 : :
467 [ + - ]: 17 : auto const lengthStrideCount = std::ceil(enuObjectPosition.dimension.length / stride);
468 : 17 : auto const lengthStrideCountUInt = static_cast<uint32_t>(lengthStrideCount);
469 [ + - ]: 17 : auto const lengthStride = enuObjectPosition.dimension.length / lengthStrideCount;
470 [ + - ]: 17 : point::ECEFPoint const lengthStrideVector = directionalVector * lengthStride;
471 : :
472 [ + - ]: 17 : auto const widthStrideCount = std::ceil(enuObjectPosition.dimension.width / stride);
473 : 17 : auto const widthStrideCountUInt = static_cast<uint32_t>(widthStrideCount);
474 [ + - ]: 17 : auto const widthStride = enuObjectPosition.dimension.width / widthStrideCount;
475 [ + - ]: 17 : point::ECEFPoint const widthStrideVector = orthogonalVector * widthStride;
476 : :
477 : 17 : point::ECEFPoint widthStartPos = referencePoints[int32_t(ObjectReferencePoints::FrontLeft)];
478 : :
479 [ + + ]: 65 : for (uint32_t widthStrideNum = 0u; widthStrideNum <= widthStrideCountUInt; widthStrideNum++)
480 : : {
481 : 48 : point::ECEFPoint currentPoint = widthStartPos;
482 [ + + ]: 281 : for (uint32_t lengthStrideNum = 0u; lengthStrideNum <= lengthStrideCountUInt; lengthStrideNum++)
483 : : {
484 : : MapMatchedPositionConfidenceList mapMatchedPositions
485 [ + - ]: 466 : = findLanesInputChecked(relevantLanes, currentPoint, samplingDistance);
486 [ + - ]: 233 : regionConfidenceList.insert(regionConfidenceList.end(), mapMatchedPositions.begin(), mapMatchedPositions.end());
487 : :
488 [ + - ]: 233 : currentPoint = currentPoint - lengthStrideVector;
489 : : }
490 [ + - ]: 48 : widthStartPos = widthStartPos - widthStrideVector;
491 : : }
492 : :
493 [ + - ]: 17 : addLaneRegions(mapMatchedObjectBoundingBox.laneOccupiedRegions, regionConfidenceList);
494 : :
495 [ + - + - ]: 34 : access::getLogger()->trace("getMapMatchedBoundingBox result {}", mapMatchedObjectBoundingBox);
496 : :
497 : 17 : return mapMatchedObjectBoundingBox;
498 : : }
499 : :
500 : 1 : LaneOccupiedRegionList AdMapMatching::getLaneOccupiedRegions(std::vector<ENUObjectPosition> enuObjectPositions,
501 : : physics::Distance const &samplingDistance) const
502 : : {
503 : 1 : LaneOccupiedRegionList laneOccupiedRegions;
504 : :
505 [ + + ]: 2 : for (auto const &objectPosition : enuObjectPositions)
506 : : {
507 [ + - ]: 2 : auto const mapMatchedBoundingBox = getMapMatchedBoundingBox(objectPosition, samplingDistance);
508 [ + - ]: 1 : addLaneRegions(laneOccupiedRegions, mapMatchedBoundingBox.laneOccupiedRegions);
509 : : }
510 : 1 : return laneOccupiedRegions;
511 : : }
512 : :
513 : 944 : inline bool isLateralInLaneMatch(const MapMatchedPosition &mapMatchedPosition)
514 : : {
515 [ + - ]: 944 : if ((mapMatchedPosition.lanePoint.lateralT > physics::RatioValue(1.0))
516 [ + + + - : 944 : || (mapMatchedPosition.lanePoint.lateralT < physics::RatioValue(0.0)))
+ + + + ]
517 : : {
518 : 250 : return false;
519 : : }
520 : 694 : return true;
521 : : }
522 : :
523 : 694 : inline bool isLongitudinalInLaneMatch(const MapMatchedPosition &mapMatchedPosition)
524 : : {
525 : : // filter out longitudinal out-of-lane matches, indicated by border points being > 5cm away
526 [ + - ]: 694 : if ((mapMatchedPosition.lanePoint.paraPoint.parametricOffset == physics::ParametricValue(0.))
527 [ + + + - : 694 : || (mapMatchedPosition.lanePoint.paraPoint.parametricOffset == physics::ParametricValue(1.)))
+ + + + ]
528 : : {
529 [ + - + - ]: 54 : if (mapMatchedPosition.matchedPointDistance > physics::Distance(0.05))
530 : : {
531 : 54 : return false;
532 : : }
533 : : }
534 : 640 : return true;
535 : : }
536 : :
537 : 472 : inline bool isActualWithinLaneMatch(const MapMatchedPosition &mapMatchedPosition)
538 : : {
539 [ + + + + ]: 472 : return isLateralInLaneMatch(mapMatchedPosition) && isLongitudinalInLaneMatch(mapMatchedPosition);
540 : : }
541 : :
542 : 17 : void AdMapMatching::addLaneRegions(LaneOccupiedRegionList &laneOccupiedRegions,
543 : : MapMatchedPositionConfidenceList const &mapMatchedPositions) const
544 : : {
545 : : // Basic algorithm:
546 : : // First, collect all actual matches within the respective lane (lateral AND longitudinal)
547 : : // Second, ensure all inner-regions are expanded to the borders (lateral AND longitudinal)
548 : :
549 : : // 1. collect matches within the lane
550 : 34 : std::map<lane::LaneId, physics::ParametricValue> longitudinalBorderMatches;
551 [ + + ]: 489 : for (auto const ¤tPosition : mapMatchedPositions)
552 : : {
553 : : // only consider actual lateral in lane matches
554 [ + - + + ]: 472 : if (isLateralInLaneMatch(currentPosition))
555 : : {
556 : 347 : physics::ParametricValue const currentLateralOffset(static_cast<double>(currentPosition.lanePoint.lateralT));
557 : :
558 [ + - + + ]: 347 : if (isLongitudinalInLaneMatch(currentPosition))
559 : : {
560 : : // 1.a longitudinal in lane
561 : : auto search = std::find_if(std::begin(laneOccupiedRegions),
562 : : std::end(laneOccupiedRegions),
563 : 345 : [currentPosition](LaneOccupiedRegion const ®ion) {
564 : 345 : return region.laneId == currentPosition.lanePoint.paraPoint.laneId;
565 [ + - ]: 320 : });
566 : :
567 [ + + ]: 320 : if (search != laneOccupiedRegions.end())
568 : : {
569 [ + - ]: 299 : unionRangeWith(search->longitudinalRange, currentPosition.lanePoint.paraPoint.parametricOffset);
570 [ + - ]: 299 : unionRangeWith(search->lateralRange, currentLateralOffset);
571 : : }
572 : : else
573 : : {
574 [ + - ]: 21 : LaneOccupiedRegion laneRegion;
575 : 21 : laneRegion.laneId = currentPosition.lanePoint.paraPoint.laneId;
576 : 21 : laneRegion.longitudinalRange.maximum = currentPosition.lanePoint.paraPoint.parametricOffset;
577 : 21 : laneRegion.longitudinalRange.minimum = currentPosition.lanePoint.paraPoint.parametricOffset;
578 : 21 : laneRegion.lateralRange.maximum = currentLateralOffset;
579 : 21 : laneRegion.lateralRange.minimum = currentLateralOffset;
580 [ + - ]: 21 : laneOccupiedRegions.push_back(laneRegion);
581 : : }
582 : : }
583 [ + - ]: 27 : else if ((currentPosition.lanePoint.paraPoint.parametricOffset == physics::ParametricValue(0.))
584 [ + + + - : 27 : || (currentPosition.lanePoint.paraPoint.parametricOffset == physics::ParametricValue(1.)))
+ - + - ]
585 : : {
586 : : // 1.b handle lane segments that are shorter than the sampling distance
587 : : // If we have short lane segment the sampling could jump over it
588 : : // Jumping over would be indicated by out of lane matches before (0.0) AND after (1.0)
589 : : auto insertResult = longitudinalBorderMatches.insert(
590 [ + - ]: 27 : {currentPosition.lanePoint.paraPoint.laneId, currentPosition.lanePoint.paraPoint.parametricOffset});
591 [ + + ]: 27 : if (!insertResult.second)
592 : : {
593 [ + - + + ]: 22 : if (insertResult.first->second != currentPosition.lanePoint.paraPoint.parametricOffset)
594 : : {
595 : : // found a short lane segment to be added as a whole
596 : : auto search = std::find_if(std::begin(laneOccupiedRegions),
597 : : std::end(laneOccupiedRegions),
598 : 12 : [currentPosition](LaneOccupiedRegion const ®ion) {
599 : 12 : return region.laneId == currentPosition.lanePoint.paraPoint.laneId;
600 [ + - ]: 4 : });
601 : :
602 [ + - ]: 4 : if (search != laneOccupiedRegions.end())
603 : : {
604 : : // adapt the borders
605 : 4 : search->longitudinalRange.maximum = physics::ParametricValue(1.);
606 : 4 : search->longitudinalRange.minimum = physics::ParametricValue(0.);
607 : : }
608 : : else
609 : : {
610 : : // add the region
611 [ # # ]: 0 : LaneOccupiedRegion laneRegion;
612 : 0 : laneRegion.laneId = currentPosition.lanePoint.paraPoint.laneId;
613 : 0 : laneRegion.longitudinalRange.maximum = physics::ParametricValue(1.);
614 : 0 : laneRegion.longitudinalRange.minimum = physics::ParametricValue(0.);
615 : 0 : laneRegion.lateralRange.maximum = currentLateralOffset;
616 : 0 : laneRegion.lateralRange.minimum = currentLateralOffset;
617 [ # # ]: 0 : laneOccupiedRegions.push_back(laneRegion);
618 : : }
619 : : }
620 : : }
621 : : }
622 : : }
623 : : }
624 : : // 2. If the vehicle covers multiple lanes the occupied regions usually don't span up to the borders
625 : : // since in the above loop only collects actual matches within the lane
626 : : // Since the likelihood for a match exactly at the border is very low, we have to enlarge
627 : : // the inner regions to cover also the space in between the map matching points
628 : : // This can be achieved by taking the out-of-lane matches into account.
629 : : // (Remember: we ensured that the matching distance >= sampling distance,
630 : : // so that respective out of lane matches are present!)
631 [ + + ]: 489 : for (auto const ¤tPosition : mapMatchedPositions)
632 : : {
633 [ + - + + ]: 472 : if (!isActualWithinLaneMatch(currentPosition))
634 : : {
635 : : auto search = std::find_if(std::begin(laneOccupiedRegions),
636 : : std::end(laneOccupiedRegions),
637 : 204 : [currentPosition](LaneOccupiedRegion const ®ion) {
638 : 204 : return region.laneId == currentPosition.lanePoint.paraPoint.laneId;
639 [ + - ]: 152 : });
640 : :
641 [ + + ]: 152 : if (search != laneOccupiedRegions.end())
642 : : {
643 : : // 2.a in longitudinal direction just perform union (either in-lane and present in each case, or out-of-lane and
644 : : // therefore exactly 1.0 or 0.0)
645 [ + - ]: 30 : unionRangeWith(search->longitudinalRange, currentPosition.lanePoint.paraPoint.parametricOffset);
646 : :
647 : : // 2.b in lateral direction map out-of-lane to in lane
648 [ + - + + ]: 30 : if (currentPosition.lanePoint.lateralT >= physics::RatioValue(1.0))
649 : : {
650 [ + - ]: 10 : unionRangeWith(search->lateralRange, physics::ParametricValue(1.0));
651 : : }
652 [ + - + + ]: 20 : else if (currentPosition.lanePoint.lateralT <= physics::RatioValue(0.0))
653 : : {
654 [ + - ]: 4 : unionRangeWith(search->lateralRange, physics::ParametricValue(0.0));
655 : : }
656 : : }
657 : : }
658 : : }
659 : 17 : }
660 : :
661 : 3 : void AdMapMatching::addLaneRegions(LaneOccupiedRegionList &laneOccupiedRegions,
662 : : LaneOccupiedRegionList const &otherLaneOccupiedRegions) const
663 : : {
664 [ + + ]: 6 : for (auto const &otherRegion : otherLaneOccupiedRegions)
665 : : {
666 : : auto search
667 : : = std::find_if(std::begin(laneOccupiedRegions),
668 : : std::end(laneOccupiedRegions),
669 [ + - ]: 4 : [otherRegion](LaneOccupiedRegion const ®ion) { return region.laneId == otherRegion.laneId; });
670 : :
671 [ + + ]: 3 : if (search != laneOccupiedRegions.end())
672 : : {
673 [ + - ]: 1 : unionRangeWith(search->longitudinalRange, otherRegion.longitudinalRange);
674 [ + - ]: 1 : unionRangeWith(search->lateralRange, otherRegion.lateralRange);
675 : : }
676 : : else
677 : : {
678 [ + - ]: 2 : laneOccupiedRegions.push_back(otherRegion);
679 : : }
680 : : }
681 : 3 : }
682 : :
683 : 0 : MapMatchedPositionConfidenceList AdMapMatching::findRouteLanes(point::ECEFPoint const &ecefPoint,
684 : : route::FullRoute const &route)
685 : : {
686 [ # # # # ]: 0 : if (!isValid(ecefPoint))
687 : : {
688 [ # # # # ]: 0 : access::getLogger()->error("Invalid ECEF Point passed to AdMapMatching::findLanes(): {}", ecefPoint);
689 : 0 : return MapMatchedPositionConfidenceList();
690 : : }
691 : 0 : match::MapMatchedPositionConfidenceList mapMatchingResults;
692 : 0 : physics::Distance distanceSum(0.);
693 [ # # ]: 0 : for (auto const &roadSegment : route.roadSegments)
694 : : {
695 [ # # ]: 0 : for (auto const &laneSegment : roadSegment.drivableLaneSegments)
696 : : {
697 [ # # ]: 0 : MapMatchedPosition mmpt;
698 [ # # # # ]: 0 : if (lane::findNearestPointOnLaneInterval(laneSegment.laneInterval, ecefPoint, mmpt))
699 : : {
700 [ # # ]: 0 : mapMatchingResults.push_back(mmpt);
701 [ # # ]: 0 : distanceSum += mmpt.matchedPointDistance;
702 : : }
703 : : }
704 : : }
705 : :
706 : : // set the result probabilities in respect to matched point distances
707 [ # # # # ]: 0 : if (distanceSum > physics::Distance(0.01))
708 : : {
709 [ # # ]: 0 : for (auto &mmpt : mapMatchingResults)
710 : : {
711 [ # # # # ]: 0 : mmpt.probability = physics::Probability(1.) - physics::Probability(mmpt.matchedPointDistance / distanceSum);
712 : : }
713 : : }
714 : :
715 : : // sort the final results
716 [ # # ]: 0 : std::sort(std::begin(mapMatchingResults),
717 : : std::end(mapMatchingResults),
718 : 0 : [](MapMatchedPosition const &left, MapMatchedPosition const &right) {
719 : 0 : return left.probability > right.probability;
720 : : });
721 : :
722 : 0 : return mapMatchingResults;
723 : : }
724 : :
725 : : } // namespace match
726 : : } // namespace map
727 : : } // namespace ad
|
