feat(avoidance): check object pose covariance

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -118,6 +118,8 @@
         object_check_return_pose_distance: 20.0         # [m]
         # lost object compensation
         max_compensation_time: 2.0
+        # filtering unstable detection result
+        th_error_eclipse_long_radius: 2.0               # [m]
 
         # detection area generation parameters
         detection_area:

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -42,6 +42,7 @@ enum class ObjectInfo {
   NONE = 0,
   // ignore reasons
   OUT_OF_TARGET_AREA,
+  HUGE_COVARIANCE,
   FURTHER_THAN_THRESHOLD,
   FURTHER_THAN_GOAL,
   IS_NOT_TARGET_OBJECT,
@@ -218,6 +219,9 @@ struct AvoidanceParameters
   // parameters for collision check.
   bool check_all_predicted_path{false};
 
+  // filtering unstable detection result
+  double th_error_eclipse_long_radius{0.0};
+
   // find adjacent lane vehicles
   double safety_check_backward_distance{0.0};
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -119,6 +119,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
       getOrDeclareParameter<double>(*node, ns + "intersection.yaw_deviation");
     p.object_last_seen_threshold =
       getOrDeclareParameter<double>(*node, ns + "max_compensation_time");
+    p.th_error_eclipse_long_radius =
+      getOrDeclareParameter<double>(*node, ns + "th_error_eclipse_long_radius");
   }
 
   {

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp

@@ -41,6 +41,7 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId;
 // ROS 2 general msgs
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::PoseWithCovariance;
 using geometry_msgs::msg::TransformStamped;
 using geometry_msgs::msg::Vector3;
 using std_msgs::msg::ColorRGBA;

planning/behavior_path_avoidance_module/src/debug.cpp

@@ -554,6 +554,7 @@ MarkerArray createDebugMarkerArray(
     addObjects(data.other_objects, ObjectInfo::IS_NOT_TARGET_OBJECT);
     addObjects(data.other_objects, ObjectInfo::FURTHER_THAN_GOAL);
     addObjects(data.other_objects, ObjectInfo::TOO_NEAR_TO_CENTERLINE);
+    addObjects(data.other_objects, ObjectInfo::HUGE_COVARIANCE);
     addObjects(data.other_objects, ObjectInfo::IS_NOT_PARKING_OBJECT);
     addObjects(data.other_objects, ObjectInfo::MOVING_OBJECT);
     addObjects(data.other_objects, ObjectInfo::CROSSWALK_USER);

planning/behavior_path_avoidance_module/src/manager.cpp

@@ -103,6 +103,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
     updateParam<double>(
       parameters, ns + "object_check_return_pose_distance", p->object_check_return_pose_distance);
     updateParam<double>(parameters, ns + "max_compensation_time", p->object_last_seen_threshold);
+    updateParam<double>(
+      parameters, ns + "th_error_eclipse_long_radius", p->th_error_eclipse_long_radius);
   }
 
   {

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -21,6 +21,7 @@
 #include "behavior_path_planner_common/utils/path_utils.hpp"
 #include "behavior_path_planner_common/utils/traffic_light_utils.hpp"
 
+#include <Eigen/Dense>
 #include <lanelet2_extension/utility/message_conversion.hpp>
 
 #include <boost/geometry/algorithms/buffer.hpp>
@@ -337,6 +338,24 @@ bool isWithinIntersection(
     object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon())));
 }
 
+bool hasHugePoseCovariance(
+  const PoseWithCovariance & pose, const std::shared_ptr<AvoidanceParameters> & parameters)
+{
+  // create xy covariance (2x2 matrix)
+  // input geometry_msgs::PoseWithCovariance contain 6x6 matrix
+  Eigen::Matrix2d xy_covariance;
+  const auto cov = pose.covariance;
+  xy_covariance(0, 0) = cov[0 * 6 + 0];
+  xy_covariance(0, 1) = cov[0 * 6 + 1];
+  xy_covariance(1, 0) = cov[1 * 6 + 0];
+  xy_covariance(1, 1) = cov[1 * 6 + 1];
+
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(xy_covariance);
+
+  // eigen values and vectors are sorted in ascending order
+  return std::sqrt(eigensolver.eigenvalues()(1)) > parameters->th_error_eclipse_long_radius;
+}
+
 bool isOnEgoLane(const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
 {
   const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
@@ -737,6 +756,12 @@ bool isSatisfiedWithCommonCondition(
     return false;
   }
 
+  if (filtering_utils::hasHugePoseCovariance(
+        object.object.kinematics.initial_pose_with_covariance, parameters)) {
+    object.info = ObjectInfo::HUGE_COVARIANCE;
+    return false;
+  }
+
   // Step3. filtered by longitudinal distance.
   const auto & ego_pos = planner_data->self_odometry->pose.pose.position;
   fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path_rough, ego_pos, object);