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/access/Operation.hpp>
10 : : #include <ad/map/lane/LaneOperation.hpp>
11 : : #include <ad/map/match/AdMapMatching.hpp>
12 : : #include <ad/map/point/Operation.hpp>
13 : : #include <algorithm>
14 : : #include <gtest/gtest.h>
15 : : #include "ad/map/match/MapMatchedOperation.hpp"
16 : :
17 : : using namespace ::ad;
18 : : using namespace ::ad::map;
19 : : using namespace ::ad::map::match;
20 : :
21 : : struct AdMapBoundingBoxMapMatchingTest : ::testing::Test
22 : : {
23 : 4 : AdMapBoundingBoxMapMatchingTest()
24 : 4 : {
25 : 4 : }
26 : :
27 : 4 : virtual void SetUp()
28 : : {
29 [ + - ]: 4 : access::cleanup();
30 [ + - + - ]: 4 : access::init("test_files/TPK.adm.txt");
31 : :
32 : 4 : mMinProbabilty = physics::Probability(0.05);
33 : 4 : mSamplingDistance = physics::Distance(1.);
34 : :
35 : 4 : config::PointOfInterest poi;
36 [ + - + - : 4 : ASSERT_TRUE(access::getPointOfInterest("T1", poi));
- + - - -
- - - -
- ]
37 [ + - ]: 4 : mObjectPosition.enuReferencePoint = access::getENUReferencePoint();
38 [ + - ]: 4 : mObjectPosition.centerPoint = point::toENU(poi.geoPoint);
39 [ + - ]: 4 : mObjectPosition.heading = point::createENUHeading(M_PI_2);
40 : 4 : mObjectPosition.dimension.width = physics::Distance(0.7);
41 : 4 : mObjectPosition.dimension.length = physics::Distance(4.);
42 : 4 : mObjectPosition.dimension.height = physics::Distance(0.);
43 : : }
44 : :
45 : 4 : virtual void TearDown()
46 : : {
47 : : // make sure that we leave the singleton "clean"
48 : 4 : access::cleanup();
49 : 4 : }
50 : :
51 : : physics::Probability mMinProbabilty;
52 : : physics::Distance mSamplingDistance;
53 : : match::ENUObjectPosition mObjectPosition;
54 : : };
55 : :
56 : 2 : TEST_F(AdMapBoundingBoxMapMatchingTest, box_within_single_lane)
57 : : {
58 [ + - ]: 1 : match::AdMapMatching mapMatching;
59 [ + - ]: 1 : auto centerMapMatched = mapMatching.getMapMatchedPositions(mObjectPosition, mSamplingDistance, mMinProbabilty);
60 : :
61 [ + - ]: 1 : point::ParaPointList para = getParaPoints(centerMapMatched);
62 [ + - - + : 1 : ASSERT_EQ(para.size(), 2u);
- - - - -
- ]
63 : :
64 [ - + - - : 1 : ASSERT_FALSE(centerMapMatched.empty());
- - - - -
- ]
65 [ + - ]: 1 : auto heading = mapMatching.getLaneENUHeading(centerMapMatched.front());
66 : :
67 : 1 : mObjectPosition.heading = heading;
68 : 1 : mObjectPosition.dimension.width = physics::Distance(1.5);
69 : 1 : mObjectPosition.dimension.length = physics::Distance(3);
70 : :
71 [ + - ]: 1 : auto result = mapMatching.getMapMatchedBoundingBox(mObjectPosition, mSamplingDistance);
72 : :
73 [ + - - + : 1 : ASSERT_EQ(result.laneOccupiedRegions.size(), 1u);
- - - - -
- ]
74 [ + + ]: 3 : for (auto centerMatch : centerMapMatched)
75 : : {
76 [ + + ]: 2 : if (centerMatch.type == match::MapMatchedPositionType::LANE_IN)
77 : : {
78 [ + - - + : 1 : ASSERT_EQ(centerMatch.lanePoint.paraPoint.laneId, result.laneOccupiedRegions.front().laneId);
- - - - -
- ]
79 [ + - - + : 1 : ASSERT_LE(result.laneOccupiedRegions.front().lateralRange.minimum,
- - - - -
- ]
80 : : result.laneOccupiedRegions.front().lateralRange.maximum);
81 [ + - - + : 1 : ASSERT_LE(result.laneOccupiedRegions.front().longitudinalRange.minimum,
- - - - -
- ]
82 : : result.laneOccupiedRegions.front().longitudinalRange.maximum);
83 : : }
84 : : }
85 : :
86 [ + - + - : 1 : ASSERT_EQ(getObjectENUHeading(result), heading);
- + - - -
- - - ]
87 [ + - ]: 1 : match::MapMatchedObjectBoundingBox saveResult = result;
88 : 1 : result.referencePointPositions[int32_t(match::ObjectReferencePoints::RearLeft)].clear();
89 : 1 : result.referencePointPositions[int32_t(match::ObjectReferencePoints::RearRight)].clear();
90 : 1 : result.referencePointPositions[int32_t(match::ObjectReferencePoints::FrontLeft)].clear();
91 : 1 : result.referencePointPositions[int32_t(match::ObjectReferencePoints::FrontRight)].clear();
92 [ + - + - : 2 : EXPECT_THROW(getObjectENUHeading(result), std::runtime_error);
+ - - + -
+ - - - -
- - - - ]
93 [ + - ]: 1 : result = saveResult;
94 : 1 : result.referencePointPositions[int32_t(match::ObjectReferencePoints::RearRight)].clear();
95 : 1 : result.referencePointPositions[int32_t(match::ObjectReferencePoints::FrontRight)].clear();
96 [ + - + - : 1 : ASSERT_EQ(getObjectENUHeading(result), heading);
- + - - -
- - - ]
97 [ + - ]: 1 : result = saveResult;
98 : 1 : result.referencePointPositions[int32_t(match::ObjectReferencePoints::RearLeft)].clear();
99 : 1 : result.referencePointPositions[int32_t(match::ObjectReferencePoints::FrontLeft)].clear();
100 [ + - + - : 1 : ASSERT_EQ(getObjectENUHeading(result), heading);
- + - - -
- - - ]
101 [ + - ]: 1 : result = saveResult;
102 : 1 : result.referencePointPositions[int32_t(match::ObjectReferencePoints::RearLeft)].clear();
103 : 1 : result.referencePointPositions[int32_t(match::ObjectReferencePoints::RearRight)].clear();
104 [ + - + - : 1 : ASSERT_EQ(getObjectENUHeading(result), heading);
- + - - -
- - - ]
105 [ + - ]: 1 : result = saveResult;
106 : 1 : result.referencePointPositions[int32_t(match::ObjectReferencePoints::FrontLeft)].clear();
107 : 1 : result.referencePointPositions[int32_t(match::ObjectReferencePoints::FrontRight)].clear();
108 [ + - + - : 1 : ASSERT_EQ(getObjectENUHeading(result), heading);
- + - - -
- - - ]
109 [ + - ]: 1 : result = saveResult;
110 : :
111 : 1 : std::vector<ENUObjectPosition> enuObjs;
112 [ + - ]: 1 : enuObjs.push_back(mObjectPosition);
113 [ + - + - ]: 1 : LaneOccupiedRegionList occRegion = mapMatching.getLaneOccupiedRegions(enuObjs, mSamplingDistance);
114 : :
115 : 1 : lane::LaneId x11;
116 [ + - - + : 1 : ASSERT_EQ(occRegion.size(), 1u);
- - - - -
- ]
117 [ + + ]: 3 : for (auto centerMatch : centerMapMatched)
118 : : {
119 [ + + ]: 2 : if (centerMatch.type == match::MapMatchedPositionType::LANE_IN)
120 : : {
121 : 1 : x11 = centerMatch.lanePoint.paraPoint.laneId;
122 [ + - - + : 1 : ASSERT_EQ(centerMatch.lanePoint.paraPoint.laneId, occRegion.front().laneId);
- - - - -
- ]
123 : : }
124 : : }
125 : :
126 [ + - ]: 1 : physics::Distance dis = signedDistanceToLane(occRegion.front().laneId, centerMapMatched);
127 [ + - - + : 1 : ASSERT_EQ(dis, physics::Distance(0));
- - - - -
- ]
128 : :
129 : 1 : lane::LaneId x(100);
130 : 1 : MapMatchedPositionConfidenceList matchList;
131 : 1 : MapMatchedPosition matchPos;
132 : 1 : matchPos.lanePoint.laneWidth = physics::Distance(1.0);
133 : 1 : matchPos.lanePoint.paraPoint.laneId = x;
134 : :
135 : 1 : matchPos.type = match::MapMatchedPositionType::LANE_LEFT;
136 : 1 : matchPos.lanePoint.lateralT = physics::RatioValue(1.0);
137 : 1 : matchList.clear();
138 [ + - ]: 1 : matchList.push_back(matchPos);
139 [ + - + - : 2 : EXPECT_THROW(signedDistanceToLane(x, matchList), std::runtime_error);
+ - - + -
+ - - - -
- - - - ]
140 : :
141 : 1 : matchPos.type = match::MapMatchedPositionType::LANE_LEFT;
142 : 1 : matchPos.lanePoint.lateralT = physics::RatioValue(-1.0);
143 : 1 : matchList.clear();
144 [ + - ]: 1 : matchList.push_back(matchPos);
145 [ + - + - : 1 : ASSERT_EQ(signedDistanceToLane(x, matchList), physics::Distance(-1.0));
- + - - -
- - - ]
146 : :
147 : 1 : matchPos.type = match::MapMatchedPositionType::LANE_RIGHT;
148 : 1 : matchPos.lanePoint.lateralT = physics::RatioValue(0.8);
149 : 1 : matchList.clear();
150 [ + - ]: 1 : matchList.push_back(matchPos);
151 [ + - + - : 2 : EXPECT_THROW(signedDistanceToLane(x, matchList), std::runtime_error);
+ - - + -
+ - - - -
- - - - ]
152 : :
153 : 1 : matchPos.type = match::MapMatchedPositionType::LANE_RIGHT;
154 : 1 : matchPos.lanePoint.lateralT = physics::RatioValue(1.5);
155 : 1 : matchList.clear();
156 [ + - ]: 1 : matchList.push_back(matchPos);
157 [ + - + - : 1 : ASSERT_EQ(signedDistanceToLane(x, matchList), physics::Distance(0.5));
- + - - -
- - - ]
158 : :
159 : 1 : matchPos.type = match::MapMatchedPositionType::INVALID;
160 : 1 : matchPos.lanePoint.lateralT = physics::RatioValue(1.0);
161 : 1 : matchList.clear();
162 [ + - ]: 1 : matchList.push_back(matchPos);
163 [ + - + - : 2 : EXPECT_THROW(signedDistanceToLane(x, matchList), std::runtime_error);
+ - - + -
+ - - - -
- - - - ]
164 : :
165 : 1 : Object obj;
166 : 1 : obj.enuPosition = mObjectPosition;
167 [ + - ]: 1 : obj.mapMatchedBoundingBox = result;
168 [ + - ]: 1 : dis = getDistanceToLane(x11, obj);
169 [ + - - + : 1 : ASSERT_EQ(dis, physics::Distance(0));
- - - - -
- ]
170 [ + - ]: 1 : dis = getDistanceToLane(para[0].laneId, obj);
171 [ + - - + : 1 : ASSERT_NEAR((double)dis, 1.7429, 0.0001);
- - - - -
- ]
172 : : }
173 : :
174 : 2 : TEST_F(AdMapBoundingBoxMapMatchingTest, rotated_box_within_two_lateral_lanes)
175 : : {
176 [ + - ]: 1 : match::AdMapMatching mapMatching;
177 [ + - ]: 1 : auto centerMapMatched = mapMatching.getMapMatchedPositions(mObjectPosition, mSamplingDistance, mMinProbabilty);
178 : :
179 [ + - - + : 1 : ASSERT_EQ(centerMapMatched.size(), 2u);
- - - - -
- ]
180 [ + - ]: 1 : auto heading = mapMatching.getLaneENUHeading(centerMapMatched.front());
181 : :
182 : 1 : mObjectPosition.heading = heading;
183 : 1 : mObjectPosition.dimension.width = physics::Distance(4.);
184 : 1 : mObjectPosition.dimension.length = physics::Distance(1.5);
185 : :
186 [ + - ]: 1 : auto result = mapMatching.getMapMatchedBoundingBox(mObjectPosition, mSamplingDistance);
187 : :
188 [ + - - + : 1 : ASSERT_EQ(result.laneOccupiedRegions.size(), 2u);
- - - - -
- ]
189 : :
190 : : auto searchFront = std::find_if(result.laneOccupiedRegions.begin(),
191 : : result.laneOccupiedRegions.end(),
192 : 2 : [¢erMapMatched](match::LaneOccupiedRegion const &other) {
193 : 2 : return other.laneId == centerMapMatched.front().lanePoint.paraPoint.laneId;
194 [ + - ]: 1 : });
195 [ - + - - : 1 : ASSERT_TRUE(searchFront != std::end(result.laneOccupiedRegions));
- - - - -
- ]
196 : :
197 : : auto searchBack = std::find_if(result.laneOccupiedRegions.begin(),
198 : : result.laneOccupiedRegions.end(),
199 : 1 : [¢erMapMatched](match::LaneOccupiedRegion const &other) {
200 : 1 : return other.laneId == centerMapMatched.back().lanePoint.paraPoint.laneId;
201 [ + - ]: 1 : });
202 [ - + - - : 1 : ASSERT_TRUE(searchBack != std::end(result.laneOccupiedRegions));
- - - - -
- ]
203 : :
204 : : // ensure the whole lane region up the the lane borders is covered
205 [ + - ]: 1 : auto laneContactRelation = lane::getDirectNeighborhoodRelation(centerMapMatched.front().lanePoint.paraPoint.laneId,
206 : 1 : centerMapMatched.back().lanePoint.paraPoint.laneId);
207 [ + - ]: 1 : if (laneContactRelation == lane::ContactLocation::LEFT)
208 : : {
209 [ + - - + : 1 : ASSERT_EQ(physics::ParametricValue(0.), searchFront->lateralRange.minimum);
- - - - -
- ]
210 [ + - - + : 1 : ASSERT_LT(physics::ParametricValue(0.), searchFront->lateralRange.maximum);
- - - - -
- ]
211 [ + - - + : 1 : ASSERT_EQ(physics::ParametricValue(1.), searchBack->lateralRange.maximum);
- - - - -
- ]
212 [ + - - + : 1 : ASSERT_GT(physics::ParametricValue(1.), searchFront->lateralRange.minimum);
- - - - -
- ]
213 : : }
214 [ # # ]: 0 : else if (laneContactRelation == lane::ContactLocation::RIGHT)
215 : : {
216 [ # # # # : 0 : ASSERT_EQ(physics::ParametricValue(0.), searchBack->lateralRange.minimum);
# # # # #
# ]
217 [ # # # # : 0 : ASSERT_LT(physics::ParametricValue(0.), searchBack->lateralRange.maximum);
# # # # #
# ]
218 [ # # # # : 0 : ASSERT_EQ(physics::ParametricValue(1.), searchFront->lateralRange.maximum);
# # # # #
# ]
219 [ # # # # : 0 : ASSERT_LT(physics::ParametricValue(1.), searchFront->lateralRange.minimum);
# # # # #
# ]
220 : : }
221 : : else
222 : : {
223 [ # # # # : 0 : ASSERT_TRUE(false);
# # # # #
# ]
224 : : }
225 : :
226 : : // reconstruct length and width of vehicle
227 : 1 : physics::Distance vehicleWidth(0.);
228 [ + + ]: 3 : for (auto const &occupiedRegion : result.laneOccupiedRegions)
229 : : {
230 [ + - ]: 2 : auto const vehicleLength = lane::getLane(occupiedRegion.laneId).length
231 [ + - + - ]: 4 : * (occupiedRegion.longitudinalRange.maximum - occupiedRegion.longitudinalRange.minimum);
232 [ + - + - : 2 : ASSERT_LE(vehicleLength * 0.9, mObjectPosition.dimension.length);
- + - - -
- - - ]
233 [ + - + - : 2 : ASSERT_GE(vehicleLength * 1.1, mObjectPosition.dimension.length);
- + - - -
- - - ]
234 : :
235 [ + - ]: 2 : vehicleWidth += lane::getLane(occupiedRegion.laneId).width
236 [ + - + - : 4 : * (occupiedRegion.lateralRange.maximum - occupiedRegion.lateralRange.minimum);
+ - ]
237 : : }
238 : :
239 [ + - + - : 1 : ASSERT_LE(vehicleWidth * 0.9, mObjectPosition.dimension.width);
- + - - -
- - - ]
240 [ + - + - : 1 : ASSERT_GE(vehicleWidth * 1.1, mObjectPosition.dimension.width);
- + - - -
- - - ]
241 : : }
242 : :
243 : 2 : TEST_F(AdMapBoundingBoxMapMatchingTest, box_covering_three_lanes_longitudinal)
244 : : {
245 : : auto objectCenter = point::toENU(
246 [ + - ]: 1 : point::createGeoPoint(point::Longitude(8.439497056), point::Latitude(49.018312553), point::Altitude(0.)));
247 : :
248 [ + - ]: 1 : match::AdMapMatching mapMatching;
249 [ + - ]: 1 : auto centerMapMatched = mapMatching.getMapMatchedPositions(objectCenter, mSamplingDistance, mMinProbabilty);
250 : :
251 [ + - - + : 1 : ASSERT_EQ(centerMapMatched.size(), 1u);
- - - - -
- ]
252 : :
253 : 1 : auto const centerLaneId = centerMapMatched.front().lanePoint.paraPoint.laneId;
254 : 1 : auto objectPosition = mObjectPosition;
255 [ + - ]: 1 : auto heading = mapMatching.getLaneENUHeading(centerMapMatched.front());
256 : :
257 [ + - ]: 1 : objectPosition.heading = point::createENUHeading(static_cast<double>(heading));
258 : 1 : objectPosition.centerPoint = objectCenter;
259 : 1 : objectPosition.dimension.width = physics::Distance(2.5);
260 : 1 : objectPosition.dimension.length = physics::Distance(6.);
261 : :
262 [ + - ]: 1 : auto result = mapMatching.getMapMatchedBoundingBox(objectPosition, mSamplingDistance);
263 : :
264 [ + - - + : 1 : ASSERT_EQ(result.laneOccupiedRegions.size(), 3u);
- - - - -
- ]
265 : :
266 : : // reconstruct length and width of vehicle
267 : 1 : physics::Distance vehicleLength(0.);
268 [ + + ]: 4 : for (auto const &occupiedRegion : result.laneOccupiedRegions)
269 : : {
270 : : // longitudinal
271 [ + - + + ]: 3 : if (occupiedRegion.laneId == centerLaneId)
272 : : {
273 : : // center lane fully covered
274 [ + - - + : 1 : ASSERT_EQ(physics::ParametricValue(0.), occupiedRegion.longitudinalRange.minimum);
- - - - -
- ]
275 [ + - - + : 1 : ASSERT_EQ(physics::ParametricValue(1.), occupiedRegion.longitudinalRange.maximum);
- - - - -
- ]
276 : : }
277 [ + - + - ]: 2 : else if (occupiedRegion.longitudinalRange.minimum == physics::ParametricValue(0.))
278 : : {
279 : : // other lane not fully covered, but definitely some parts of it
280 [ + - - + : 2 : ASSERT_NE(physics::ParametricValue(0.), occupiedRegion.longitudinalRange.maximum);
- - - - -
- ]
281 [ + - - + : 2 : ASSERT_NE(physics::ParametricValue(1.), occupiedRegion.longitudinalRange.maximum);
- - - - -
- ]
282 : : }
283 : : else
284 : : {
285 : : // other lane not fully covered, but definitely some parts of it
286 [ # # # # : 0 : ASSERT_EQ(physics::ParametricValue(1.), occupiedRegion.longitudinalRange.maximum);
# # # # #
# ]
287 [ # # # # : 0 : ASSERT_NE(physics::ParametricValue(0.), occupiedRegion.longitudinalRange.minimum);
# # # # #
# ]
288 [ # # # # : 0 : ASSERT_NE(physics::ParametricValue(1.), occupiedRegion.longitudinalRange.minimum);
# # # # #
# ]
289 : : }
290 : :
291 [ + - ]: 3 : vehicleLength += lane::getLane(occupiedRegion.laneId).length
292 [ + - + - : 6 : * (occupiedRegion.longitudinalRange.maximum - occupiedRegion.longitudinalRange.minimum);
+ - ]
293 : :
294 : : // lateral always in between
295 [ + - - + : 3 : ASSERT_NE(physics::ParametricValue(0.), occupiedRegion.lateralRange.minimum);
- - - - -
- ]
296 [ + - - + : 3 : ASSERT_NE(physics::ParametricValue(1.), occupiedRegion.lateralRange.minimum);
- - - - -
- ]
297 [ + - - + : 3 : ASSERT_NE(physics::ParametricValue(0.), occupiedRegion.lateralRange.maximum);
- - - - -
- ]
298 [ + - - + : 3 : ASSERT_NE(physics::ParametricValue(1.), occupiedRegion.lateralRange.maximum);
- - - - -
- ]
299 : :
300 [ + - ]: 3 : auto const vehicleWidth = lane::getLane(occupiedRegion.laneId).width
301 [ + - + - ]: 6 : * (occupiedRegion.lateralRange.maximum - occupiedRegion.lateralRange.minimum);
302 [ + - + - : 3 : ASSERT_LE(vehicleWidth * 0.9, objectPosition.dimension.width);
- + - - -
- - - ]
303 [ + - + - : 3 : ASSERT_GE(vehicleWidth * 1.1, objectPosition.dimension.width);
- + - - -
- - - ]
304 : : }
305 : :
306 [ + - + - : 1 : ASSERT_LE(vehicleLength * 0.9, objectPosition.dimension.length);
- + - - -
- - - ]
307 [ + - + - : 1 : ASSERT_GE(vehicleLength * 1.1, objectPosition.dimension.length);
- + - - -
- - - ]
308 : : }
309 : :
310 : 2 : TEST_F(AdMapBoundingBoxMapMatchingTest, box_not_touching_second_lane_within_sampling_distance)
311 : : {
312 : : auto objectCenter = point::toENU(
313 [ + - ]: 1 : point::createGeoPoint(point::Longitude(8.4394653), point::Latitude(49.0182735), point::Altitude(0.)));
314 : :
315 [ + - ]: 1 : match::AdMapMatching mapMatching;
316 [ + - ]: 1 : auto centerMapMatched = mapMatching.getMapMatchedPositions(objectCenter, mSamplingDistance, mMinProbabilty);
317 : :
318 [ + - - + : 1 : ASSERT_EQ(centerMapMatched.size(), 1u);
- - - - -
- ]
319 : :
320 : 1 : auto objectPosition = mObjectPosition;
321 [ + - ]: 1 : objectPosition.heading = mapMatching.getLaneENUHeading(centerMapMatched.front());
322 : 1 : objectPosition.centerPoint = objectCenter;
323 : 1 : objectPosition.dimension.width = physics::Distance(2.5);
324 : 1 : objectPosition.dimension.length = physics::Distance(6.);
325 : :
326 [ + - ]: 1 : auto result = mapMatching.getMapMatchedBoundingBox(objectPosition, mSamplingDistance);
327 : :
328 [ + - - + : 1 : ASSERT_EQ(result.laneOccupiedRegions.size(), 1u);
- - - - -
- ]
329 : : }
|
