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/test_support/NoLogTestMacros.hpp>
13 : : #include <gtest/gtest.h>
14 : :
15 : : using namespace ::ad;
16 : : using namespace ::ad::map;
17 : : using namespace ::ad::map::match;
18 : :
19 : : struct AdMapMatchingTest : ::testing::Test
20 : : {
21 : 5 : AdMapMatchingTest()
22 [ + - + - : 40 : {
+ - + - +
- + - + -
+ - ]
23 : 5 : }
24 : :
25 : 5 : virtual void SetUp()
26 : : {
27 : 5 : access::cleanup();
28 [ + - + - ]: 5 : access::init("test_files/TPK.adm.txt");
29 : 5 : mRouteHint = route::FullRoute();
30 : 5 : mMinProbabilty = physics::Probability(0.05);
31 : 5 : mHeadingHints.clear();
32 [ + - ]: 5 : mMapMatching = new match::AdMapMatching();
33 : 5 : }
34 : :
35 : 5 : virtual void TearDown()
36 : : {
37 [ + - ]: 5 : delete mMapMatching;
38 : 5 : access::cleanup();
39 : 5 : }
40 : :
41 : : void compareMapMatching(int line, point::GeoPoint const &geoPoint, std::size_t const expectedNumberOfMatches);
42 : :
43 : 3 : void addRouteHint()
44 : : {
45 : : // with route hint straigth through Albert-Nestler-Str
46 : : auto hintPoint
47 : 3 : = point::createGeoPoint(point::Longitude(8.4401803), point::Latitude(49.0191987), point::Altitude(0.));
48 [ + - ]: 3 : auto mapMatchingResults = mMapMatching->getMapMatchedPositions(hintPoint, physics::Distance(0.01), mMinProbabilty);
49 [ + - - + : 3 : ASSERT_EQ(mapMatchingResults.size(), 1u);
- - - - -
- ]
50 : 6 : route::RoadSegment roadSegment;
51 : 6 : route::LaneSegment laneSegment;
52 : 3 : laneSegment.laneInterval.laneId = mapMatchingResults.front().lanePoint.paraPoint.laneId;
53 : 3 : laneSegment.laneInterval.start = physics::ParametricValue(0.);
54 : 3 : laneSegment.laneInterval.end = physics::ParametricValue(1.);
55 [ + - ]: 3 : roadSegment.drivableLaneSegments.push_back(laneSegment);
56 [ + - ]: 3 : mRouteHint.roadSegments.push_back(roadSegment);
57 : : }
58 : :
59 : : /* A note on the test setup:
60 : : * Location is the intersection Albert-Nestler-Str crossing Ada-Lovelace-Str
61 : : *
62 : : * Given:
63 : : * | -> Albert-Nestler-Str
64 : : * |
65 : : * -----------+
66 : : * ^ |
67 : : * | | -> Albert-Nestler-Str
68 : : * |
69 : : * Ada-Lovel.
70 : : *
71 : : * All points for matching (mTestPoints) should be along the lane from south to north
72 : : * inside the intersection, e.g.
73 : : * 8.4401468/49.0191790
74 : : * 8.4401701/49.0192037
75 : : * 8.4401889/49.0192335
76 : : * are all along that lane (ordered from south to north)
77 : : */
78 : :
79 : : std::string mTestFile{"test_files/TPK.adm.txt"};
80 : : std::vector<std::pair<point::GeoPoint, size_t>> mTestPoints{
81 : 0 : std::make_pair(point::createGeoPoint(point::Longitude(8.4400665), point::Latitude(49.0192005), point::Altitude(0.)),
82 : 5 : 0u),
83 : 0 : std::make_pair(point::createGeoPoint(point::Longitude(8.4401882), point::Latitude(49.0191939), point::Altitude(0.)),
84 : 5 : 1u),
85 : : std::make_pair(
86 : 5 : point::createGeoPoint(point::Longitude(8.4401882), point::Latitude(49.0191939), point::AltitudeUnknown), 1u),
87 : 0 : std::make_pair(point::createGeoPoint(point::Longitude(8.4401510), point::Latitude(49.0191792), point::Altitude(0.)),
88 : 5 : 2u),
89 : 0 : std::make_pair(point::createGeoPoint(point::Longitude(8.4401742), point::Latitude(49.0192009), point::Altitude(0.)),
90 : 5 : 3u),
91 : : // approaching middle of the intersection
92 : 0 : std::make_pair(point::createGeoPoint(point::Longitude(8.4401540), point::Latitude(49.0192082), point::Altitude(0.)),
93 : 10 : 4u)};
94 : :
95 : : route::FullRoute mRouteHint;
96 : : std::vector<point::ENUHeading> mHeadingHints;
97 : : physics::Probability mMinProbabilty;
98 : : match::AdMapMatching *mMapMatching;
99 : : };
100 : :
101 : 24 : void AdMapMatchingTest::compareMapMatching(int line,
102 : : point::GeoPoint const &geoPoint,
103 : : std::size_t const expectedNumberOfMatches)
104 : : {
105 : 24 : physics::Distance searchDist(1);
106 : :
107 : : struct TestResult
108 : : {
109 : : match::MapMatchedPosition mapMatchedPosition;
110 : : lane::LaneDirection laneDirection;
111 : : };
112 : :
113 : 24 : std::vector<TestResult> testResults;
114 : :
115 [ + - ]: 24 : mMapMatching->addRouteHint(mRouteHint);
116 [ + + ]: 36 : for (auto headingHint : mHeadingHints)
117 : : {
118 [ + - + - ]: 12 : mMapMatching->addHeadingHint(headingHint, ad::map::access::getENUReferencePoint());
119 : : }
120 : :
121 [ + - ]: 24 : auto mapMatchingResults = mMapMatching->getMapMatchedPositions(geoPoint, searchDist, mMinProbabilty);
122 : :
123 [ + + ]: 68 : for (auto mapMatchingResult : mapMatchingResults)
124 : : {
125 : 44 : TestResult result;
126 : 44 : result.mapMatchedPosition = mapMatchingResult;
127 [ + - ]: 44 : result.laneDirection = lane::getLane(result.mapMatchedPosition.lanePoint.paraPoint.laneId).direction;
128 [ + - ]: 44 : testResults.push_back(result);
129 : : }
130 : :
131 [ + - - + : 24 : ASSERT_EQ(expectedNumberOfMatches, testResults.size()) << " compareMapMatching called from " << line << "\n";
- - - - -
- - - - -
- - ]
132 : :
133 [ + + + + : 24 : if ((mRouteHint.roadSegments.size() > 0u) && (testResults.size() > 0u))
+ + ]
134 : : {
135 [ + - - + : 5 : ASSERT_EQ(1u, mRouteHint.roadSegments.size()) << " compareMapMatching called from " << line << "\n";
- - - - -
- - - - -
- - ]
136 [ + - - + : 5 : ASSERT_EQ(1u, mRouteHint.roadSegments[0].drivableLaneSegments.size())
- - - - -
- ]
137 [ # # # # : 0 : << " compareMapMatching called from " << line << "\n";
# # ]
138 [ + - - + : 5 : ASSERT_LT(0u, testResults.size()) << " compareMapMatching called from " << line << "\n";
- - - - -
- - - - -
- - ]
139 [ + - - + : 5 : ASSERT_EQ(mRouteHint.roadSegments[0].drivableLaneSegments[0].laneInterval.laneId,
- - - - -
- ]
140 : : testResults[0].mapMatchedPosition.lanePoint.paraPoint.laneId)
141 [ # # # # : 0 : << " compareMapMatching called from " << line << "\n";
# # ]
142 : : }
143 : : }
144 : :
145 : 2 : TEST_F(AdMapMatchingTest, perform_map_matching_no_route_hints)
146 : : {
147 [ + + ]: 7 : for (auto testElement : mTestPoints)
148 : : {
149 [ + - ]: 6 : compareMapMatching(__LINE__, testElement.first, testElement.second);
150 : : }
151 : 1 : }
152 : :
153 : 2 : TEST_F(AdMapMatchingTest, isLanePartOfRouteHints)
154 : : {
155 : 1 : addRouteHint();
156 : :
157 : 1 : mMapMatching->addRouteHint(mRouteHint);
158 : :
159 [ + - - + : 1 : EXPECT_TRUE(mMapMatching->isLanePartOfRouteHints(
- - - - -
- - - ]
160 : : mRouteHint.roadSegments.front().drivableLaneSegments.front().laneInterval.laneId));
161 [ + - + - : 1 : EXPECT_FALSE(mMapMatching->isLanePartOfRouteHints(
- + - - -
- - - -
- ]
162 : : mRouteHint.roadSegments.front().drivableLaneSegments.front().laneInterval.laneId + lane::LaneId(1)));
163 : 1 : }
164 : :
165 : 2 : TEST_F(AdMapMatchingTest, perform_map_matching_with_route_hints)
166 : : {
167 : 1 : addRouteHint();
168 [ + + ]: 7 : for (auto testElement : mTestPoints)
169 : : {
170 [ + - ]: 6 : compareMapMatching(__LINE__, testElement.first, testElement.second);
171 : : }
172 : 1 : }
173 : :
174 : 2 : TEST_F(AdMapMatchingTest, perform_map_matching_with_heading_hints)
175 : : {
176 : 1 : mMinProbabilty = physics::Probability(0.05);
177 [ + - ]: 1 : mHeadingHints.push_back(point::createENUHeading(point::degree2radians(70)));
178 [ + + ]: 7 : for (auto testElement : mTestPoints)
179 : : {
180 [ + - ]: 6 : compareMapMatching(__LINE__, testElement.first, testElement.second);
181 : : }
182 : :
183 : 1 : mHeadingHints.clear();
184 [ + - ]: 1 : mHeadingHints.push_back(point::createENUHeading(point::degree2radians(-110)));
185 [ + + ]: 7 : for (auto testElement : mTestPoints)
186 : : {
187 [ + - ]: 6 : compareMapMatching(__LINE__, testElement.first, testElement.second);
188 : : }
189 : 1 : }
190 : :
191 : 2 : TEST_F(AdMapMatchingTest, laneOperation)
192 : : {
193 [ + - ]: 1 : addRouteHint();
194 : :
195 [ + - ]: 1 : mMapMatching->addRouteHint(mRouteHint);
196 : 1 : auto hintPoint = point::createGeoPoint(point::Longitude(8.4401803), point::Latitude(49.0191987), point::Altitude(0.));
197 : 1 : MapMatchedPositionConfidenceList mapMatchingResults;
198 [ + - + - : 1 : EXECUTE_NO_LOG(mapMatchingResults = mMapMatching->findLanes(point::ECEFPoint(), physics::Distance(0.01)));
+ - + - +
- ]
199 [ + - - + : 1 : ASSERT_EQ(mapMatchingResults.size(), 0u);
- - - - -
- ]
200 [ + - + - ]: 1 : mapMatchingResults = mMapMatching->findLanes(toECEF(hintPoint), physics::Distance());
201 [ + - - + : 1 : ASSERT_EQ(mapMatchingResults.size(), 0u);
- - - - -
- ]
202 [ + - + - ]: 1 : mapMatchingResults = mMapMatching->findLanes(toECEF(hintPoint), physics::Distance(0.01));
203 [ + - - + : 1 : ASSERT_EQ(mapMatchingResults.size(), 1u);
- - - - -
- ]
204 [ + - + - : 1 : ASSERT_NEAR(double(ad::map::lane::calcWidth(toENU(hintPoint))), 3.0039, 0.0001);
+ - - + -
- - - -
- ]
205 : :
206 : 1 : LaneOccupiedRegionList laneOccupiedRegions;
207 : 1 : LaneOccupiedRegionList otherLaneOccupiedRegions;
208 : :
209 : 1 : LaneOccupiedRegion region1, region2;
210 : 1 : region1.laneId = map::lane::LaneId(10);
211 : 1 : physics::ParametricRange range;
212 : 1 : range.minimum = physics::ParametricValue(0.1);
213 : 1 : range.maximum = physics::ParametricValue(0.2);
214 : 1 : region1.longitudinalRange = range;
215 : 1 : range.maximum = physics::ParametricValue(0.3);
216 : 1 : region1.lateralRange = range;
217 [ + - ]: 1 : otherLaneOccupiedRegions.push_back(region1);
218 [ + - ]: 1 : mMapMatching->addLaneRegions(laneOccupiedRegions, otherLaneOccupiedRegions);
219 [ + - - + : 1 : ASSERT_EQ(laneOccupiedRegions[0].lateralRange, range);
- - - - -
- ]
220 : :
221 : 1 : region2 = region1;
222 : 1 : range.maximum = physics::ParametricValue(0.4);
223 : 1 : region2.lateralRange = range;
224 : 1 : otherLaneOccupiedRegions.clear();
225 [ + - ]: 1 : otherLaneOccupiedRegions.push_back(region2);
226 [ + - ]: 1 : mMapMatching->addLaneRegions(laneOccupiedRegions, otherLaneOccupiedRegions);
227 [ + - - + : 1 : ASSERT_EQ(laneOccupiedRegions[0].lateralRange, range);
- - - - -
- ]
228 : : }
|
