Skip to content

Commit 5a1d772

Browse files
yhisakixtk8532704
authored andcommitted
perf(autoware_map_based_prediction): enhance speed by removing unnecessary calculation (autowarefoundation#8471)
* fix(autoware_map_based_prediction): use surrounding_crosswalks instead of external_surrounding_crosswalks Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * perf(autoware_map_based_prediction): enhance speed by removing unnecessary calculation Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> --------- Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> Signed-off-by: xtk8532704 <1041084556@qq.com>
1 parent 752c7c7 commit 5a1d772

File tree

1 file changed

+14
-27
lines changed

1 file changed

+14
-27
lines changed

perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

+14-27
Original file line numberDiff line numberDiff line change
@@ -359,28 +359,6 @@ CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswa
359359
back_center_point, r_p_back, l_p_back};
360360
}
361361

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-
384362
bool withinRoadLanelet(
385363
const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
386364
const bool use_yaw_information = false)
@@ -392,17 +370,26 @@ bool withinRoadLanelet(
392370
const auto surrounding_lanelets =
393371
lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, search_radius);
394372

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);
398379
if (
399380
attr.value() == lanelet::AttributeValueString::Crosswalk ||
400381
attr.value() == lanelet::AttributeValueString::Walkway) {
401382
continue;
402383
}
403384
}
404385

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) {
406393
return true;
407394
}
408395
}
@@ -1439,7 +1426,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
14391426
attr.value() == lanelet::AttributeValueString::Walkway) {
14401427
const auto & crosswalk = lanelet;
14411428
surrounding_crosswalks.push_back(crosswalk);
1442-
if (withinLanelet(object, crosswalk)) {
1429+
if (std::abs(dist) < 1e-5) {
14431430
crossing_crosswalk = crosswalk;
14441431
}
14451432
}

0 commit comments

Comments
 (0)