@@ -359,28 +359,6 @@ CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswa
359
359
back_center_point, r_p_back, l_p_back};
360
360
}
361
361
362
- bool withinLanelet (
363
- const TrackedObject & object, const lanelet::ConstLanelet & lanelet,
364
- const bool use_yaw_information = false , const float yaw_threshold = 0.6 )
365
- {
366
- using Point = boost::geometry::model::d2::point_xy<double >;
367
-
368
- const auto & obj_pos = object.kinematics .pose_with_covariance .pose .position ;
369
- const Point p_object{obj_pos.x , obj_pos.y };
370
-
371
- auto polygon = lanelet.polygon2d ().basicPolygon ();
372
- boost::geometry::correct (polygon);
373
- bool with_in_polygon = boost::geometry::within (p_object, polygon);
374
-
375
- if (!use_yaw_information) return with_in_polygon;
376
-
377
- // use yaw angle to compare
378
- const double abs_yaw_diff = calcAbsYawDiffBetweenLaneletAndObject (object, lanelet);
379
- if (abs_yaw_diff < yaw_threshold) return with_in_polygon;
380
-
381
- return false ;
382
- }
383
-
384
362
bool withinRoadLanelet (
385
363
const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
386
364
const bool use_yaw_information = false )
@@ -392,17 +370,26 @@ bool withinRoadLanelet(
392
370
const auto surrounding_lanelets =
393
371
lanelet::geometry::findNearest (lanelet_map_ptr->laneletLayer , search_point, search_radius);
394
372
395
- for (const auto & lanelet : surrounding_lanelets) {
396
- if (lanelet.second .hasAttribute (lanelet::AttributeName::Subtype)) {
397
- lanelet::Attribute attr = lanelet.second .attribute (lanelet::AttributeName::Subtype);
373
+ for (const auto & lanelet_with_dist : surrounding_lanelets) {
374
+ const auto & dist = lanelet_with_dist.first ;
375
+ const auto & lanelet = lanelet_with_dist.second ;
376
+
377
+ if (lanelet.hasAttribute (lanelet::AttributeName::Subtype)) {
378
+ lanelet::Attribute attr = lanelet.attribute (lanelet::AttributeName::Subtype);
398
379
if (
399
380
attr.value () == lanelet::AttributeValueString::Crosswalk ||
400
381
attr.value () == lanelet::AttributeValueString::Walkway) {
401
382
continue ;
402
383
}
403
384
}
404
385
405
- if (withinLanelet (object, lanelet.second , use_yaw_information)) {
386
+ constexpr float yaw_threshold = 0.6 ;
387
+ bool within_lanelet = std::abs (dist) < 1e-5 ;
388
+ if (use_yaw_information) {
389
+ within_lanelet =
390
+ within_lanelet && calcAbsYawDiffBetweenLaneletAndObject (object, lanelet) < yaw_threshold;
391
+ }
392
+ if (within_lanelet) {
406
393
return true ;
407
394
}
408
395
}
@@ -1439,7 +1426,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
1439
1426
attr.value () == lanelet::AttributeValueString::Walkway) {
1440
1427
const auto & crosswalk = lanelet;
1441
1428
surrounding_crosswalks.push_back (crosswalk);
1442
- if (withinLanelet (object, crosswalk) ) {
1429
+ if (std::abs (dist) < 1e-5 ) {
1443
1430
crossing_crosswalk = crosswalk;
1444
1431
}
1445
1432
}
0 commit comments