Skip to content

Commit

Permalink
perf(autoware_map_based_prediction): enhance speed by removing unnece…
Browse files Browse the repository at this point in the history
…ssary calculation

Signed-off-by: Y.Hisaki <[email protected]>
  • Loading branch information
yhisaki committed Aug 14, 2024
1 parent ba0b685 commit 31e2027
Showing 1 changed file with 14 additions and 27 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -358,28 +358,6 @@ CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswa
back_center_point, r_p_back, l_p_back};
}

bool withinLanelet(
const TrackedObject & object, const lanelet::ConstLanelet & lanelet,
const bool use_yaw_information = false, const float yaw_threshold = 0.6)
{
using Point = boost::geometry::model::d2::point_xy<double>;

const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
const Point p_object{obj_pos.x, obj_pos.y};

auto polygon = lanelet.polygon2d().basicPolygon();
boost::geometry::correct(polygon);
bool with_in_polygon = boost::geometry::within(p_object, polygon);

if (!use_yaw_information) return with_in_polygon;

// use yaw angle to compare
const double abs_yaw_diff = calcAbsYawDiffBetweenLaneletAndObject(object, lanelet);
if (abs_yaw_diff < yaw_threshold) return with_in_polygon;

return false;
}

bool withinRoadLanelet(
const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
const bool use_yaw_information = false)
Expand All @@ -391,17 +369,26 @@ bool withinRoadLanelet(
const auto surrounding_lanelets =
lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, search_radius);

for (const auto & lanelet : surrounding_lanelets) {
if (lanelet.second.hasAttribute(lanelet::AttributeName::Subtype)) {
lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype);
for (const auto & lanelet_with_dist : surrounding_lanelets) {
const auto & dist = lanelet_with_dist.first;
const auto & lanelet = lanelet_with_dist.second;

if (lanelet.hasAttribute(lanelet::AttributeName::Subtype)) {
lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype);
if (
attr.value() == lanelet::AttributeValueString::Crosswalk ||
attr.value() == lanelet::AttributeValueString::Walkway) {
continue;
}
}

if (withinLanelet(object, lanelet.second, use_yaw_information)) {
constexpr float yaw_threshold = 0.6;
bool within_lanelet = std::abs(dist) < 1e-5;
if (use_yaw_information) {
within_lanelet =
within_lanelet && calcAbsYawDiffBetweenLaneletAndObject(object, lanelet) < yaw_threshold;
}
if (within_lanelet) {

Check warning on line 391 in perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method

withinRoadLanelet has a cyclomatic complexity of 9, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
return true;
}
}
Expand Down Expand Up @@ -1397,7 +1384,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
attr.value() == lanelet::AttributeValueString::Walkway) {
const auto & crosswalk = lanelet;
surrounding_crosswalks.push_back(crosswalk);
if (withinLanelet(object, crosswalk)) {
if (std::abs(dist) < 1e-5) {
crossing_crosswalk = crosswalk;
}
}
Expand Down

0 comments on commit 31e2027

Please sign in to comment.