add_height_limitation_to_filter

Author: Your Name

perception/shape_estimation/CMakeLists.txt

@@ -21,10 +21,11 @@ ament_auto_add_library(shape_estimation_lib SHARED
   lib/model/bounding_box.cpp
   lib/model/convex_hull.cpp
   lib/model/cylinder.cpp
-  lib/filter/car_filter.cpp
-  lib/filter/bus_filter.cpp
-  lib/filter/truck_filter.cpp
-  lib/filter/trailer_filter.cpp
+  # lib/filter/car_filter.cpp
+  # lib/filter/bus_filter.cpp
+  # lib/filter/truck_filter.cpp
+  # lib/filter/trailer_filter.cpp
+  lib/filter/vehicle_filter.cpp
   lib/filter/no_filter.cpp
   lib/filter/utils.cpp
   lib/corrector/no_corrector.cpp

perception/shape_estimation/config/shape_estimation.param.yaml

@@ -6,3 +6,38 @@
       use_vehicle_reference_shape_size: false
       use_boost_bbox_optimizer: false
       fix_filtered_objects_label_to_unknown: true
+      # car config
+      shapes.car.shape.min_width: 1.5
+      shapes.car.shape.max_width: 2.5
+      shapes.car.shape.min_length: 3.0
+      shapes.car.shape.max_length: 5.8
+      shapes.car.shape.min_height: 0.3
+      shapes.car.shape.max_height: 12.0
+      # truck config
+      shapes.truck.shape.min_width: 1.5
+      shapes.truck.shape.max_width: 3.5
+      shapes.truck.shape.min_length: 4.0
+      shapes.truck.shape.max_length: 18.0
+      shapes.truck.shape.min_height: 0.3
+      shapes.truck.shape.max_height: 18.0
+      # bus config
+      shapes.bus.shape.min_width: 2.0
+      shapes.bus.shape.max_width: 3.2
+      shapes.bus.shape.min_length: 5.0
+      shapes.bus.shape.max_length: 17.0
+      shapes.bus.shape.min_height: 0.3
+      shapes.bus.shape.max_height: 17.0
+      # trailer config
+      shapes.trailer.shape.min_width: 1.5
+      shapes.trailer.shape.max_width: 3.2
+      shapes.trailer.shape.min_length: 5.0
+      shapes.trailer.shape.max_length: 25.0
+      shapes.trailer.shape.min_height: 0.3
+      shapes.trailer.shape.max_height: 25.0
+      # bicycle and motorcycle config
+      shapes.bicycle.shape.min_width: 0.3
+      shapes.bicycle.shape.max_width: 1.2
+      shapes.bicycle.shape.min_length: 1.0
+      shapes.bicycle.shape.max_length: 2.8
+      shapes.bicycle.shape.min_height: 0.1
+      shapes.bicycle.shape.max_height: 3.0

perception/shape_estimation/include/shape_estimation/filter/bus_filter.hpp

@@ -15,11 +15,8 @@
 #ifndef SHAPE_ESTIMATION__FILTER__FILTER_HPP_
 #define SHAPE_ESTIMATION__FILTER__FILTER_HPP_
 
-#include "shape_estimation/filter/bus_filter.hpp"
-#include "shape_estimation/filter/car_filter.hpp"
-#include "shape_estimation/filter/filter_interface.hpp"
+
 #include "shape_estimation/filter/no_filter.hpp"
-#include "shape_estimation/filter/trailer_filter.hpp"
-#include "shape_estimation/filter/truck_filter.hpp"
+#include "shape_estimation/filter/vehicle_filter.hpp"
 
 #endif  // SHAPE_ESTIMATION__FILTER__FILTER_HPP_

perception/shape_estimation/include/shape_estimation/filter/car_filter.hpp

@@ -15,6 +15,7 @@
 #ifndef SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_
 #define SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_
 
+#include "shape_estimation/shape_estimator.hpp"
 #include <autoware_auto_perception_msgs/msg/shape.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
@@ -29,7 +30,8 @@ class ShapeEstimationFilterInterface
 
   virtual bool filter(
     const autoware_auto_perception_msgs::msg::Shape & shape,
-    const geometry_msgs::msg::Pose & pose) = 0;
+    const geometry_msgs::msg::Pose & pose,
+    const ShapeParameters & param ) = 0;
 };
 
 #endif  // SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_

perception/shape_estimation/include/shape_estimation/filter/filter.hpp

@@ -26,7 +26,9 @@ class NoFilter : public ShapeEstimationFilterInterface
 
   bool filter(
     const autoware_auto_perception_msgs::msg::Shape & shape,
-    const geometry_msgs::msg::Pose & pose) override;
+    const geometry_msgs::msg::Pose & pose,
+    const ShapeParameters & shape_param
+    ) override;
 };
 
 #endif  // SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_

perception/shape_estimation/include/shape_estimation/filter/filter_interface.hpp

@@ -21,7 +21,7 @@ namespace utils
 {
 bool filterVehicleBoundingBox(
   const autoware_auto_perception_msgs::msg::Shape & shape, const float min_width,
-  const float max_width, const float max_length);
+  const float max_width, const float max_length, const float min_height, const float max_height);
 }  // namespace utils
 
 #endif  // SHAPE_ESTIMATION__FILTER__UTILS_HPP_

perception/shape_estimation/include/shape_estimation/filter/no_filter.hpp

@@ -12,22 +12,23 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_
-#define SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_
+#ifndef SHAPE_ESTIMATION__FILTER__VEHICLE_FILTER_HPP_
+#define SHAPE_ESTIMATION__FILTER__VEHICLE_FILTER_HPP_
 
 #include "shape_estimation/filter/filter_interface.hpp"
 #include "utils.hpp"
 
-class TrailerFilter : public ShapeEstimationFilterInterface
+class VehicleFilter : public ShapeEstimationFilterInterface
 {
 public:
-  TrailerFilter() = default;
+  VehicleFilter() = default;
 
-  ~TrailerFilter() = default;
+  ~VehicleFilter() = default;
 
   bool filter(
     const autoware_auto_perception_msgs::msg::Shape & shape,
-    const geometry_msgs::msg::Pose & pose) override;
+    const geometry_msgs::msg::Pose & pose,
+    const ShapeParameters & shape_param ) override;
 };
 
 #endif  // SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_

perception/shape_estimation/include/shape_estimation/filter/truck_filter.hpp

@@ -27,6 +27,23 @@
 
 #include <string>
 
+struct ShapeLimitations
+  {
+    float min_width;
+    float max_width;
+    float min_length;
+    float max_length;
+    float min_height;
+    float max_height;
+};
+
+struct ShapeParameters
+{
+  std::string name;
+  ShapeLimitations shape_limitations;
+  std::string get_name() const { return name; }
+};
+
 struct ReferenceYawInfo
 {
   float yaw;
@@ -55,18 +72,24 @@ class ShapeEstimator
     geometry_msgs::msg::Pose & pose_output);
   bool applyFilter(
     const uint8_t label, const autoware_auto_perception_msgs::msg::Shape & shape,
-    const geometry_msgs::msg::Pose & pose);
+    const geometry_msgs::msg::Pose & pose, const ShapeParameters & shape_limitation);
   bool applyCorrector(
     const uint8_t label, const bool use_reference_yaw,
     const boost::optional<ReferenceShapeSizeInfo> & ref_shape_size_info,
     autoware_auto_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose);
+  // add labelToString function
+  ShapeParameters getShapeLimitation(const std::string & name);
+  std::string labelToString(const uint8_t label);
+
 
   bool use_corrector_;
   bool use_filter_;
   bool use_boost_bbox_optimizer_;
+  // add shapes_ constructor
+  std::vector<ShapeParameters> shapes_;
 
 public:
-  ShapeEstimator(bool use_corrector, bool use_filter, bool use_boost_bbox_optimizer = false);
+  ShapeEstimator(std::vector<ShapeParameters> & shapes, bool use_corrector, bool use_filter, bool use_boost_bbox_optimizer = false);
 
   virtual ~ShapeEstimator() = default;
 

perception/shape_estimation/include/shape_estimation/filter/utils.hpp

@@ -16,7 +16,8 @@
 
 bool NoFilter::filter(
   [[maybe_unused]] const autoware_auto_perception_msgs::msg::Shape & shape,
-  [[maybe_unused]] const geometry_msgs::msg::Pose & pose)
+  [[maybe_unused]] const geometry_msgs::msg::Pose & pose,
+  [[maybe_unused]] const ShapeParameters & shape_param)
 {
   return true;
 }

perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp perception/shape_estimation/include/shape_estimation/filter/vehicle_filter.hpp

@@ -13,16 +13,18 @@
 // limitations under the License.
 
 #include "shape_estimation/filter/utils.hpp"
+#include <iostream>
 
 namespace utils
 {
 bool filterVehicleBoundingBox(
   const autoware_auto_perception_msgs::msg::Shape & shape, const float min_width,
-  const float max_width, const float max_length)
+  const float max_width, const float max_length, const float min_height, const float max_height)
 {
   const float x = shape.dimensions.x;
   const float y = shape.dimensions.y;
   const float s = x * y;
+  const float z = shape.dimensions.z;
 
   if (shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
     return true;
@@ -41,6 +43,12 @@ bool filterVehicleBoundingBox(
   if (max_length * max_width < s) {
     return false;
   }
+
+  if (max_height < z || z < min_height) {
+    std::cout <<"Filtered (min_height:" << min_height << ", max_height:" << max_height << ")" <<std::endl;
+    std::cout <<"Removed (x:" << x << ", y:" << y << ", z: " << z << ")" <<std::endl;
+    return false;
+  }
   return true;
 }
 }  // namespace utils

perception/shape_estimation/include/shape_estimation/shape_estimator.hpp

@@ -12,14 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "shape_estimation/filter/bus_filter.hpp"
+#include "shape_estimation/filter/vehicle_filter.hpp"
+#include <iostream>
 
-bool BusFilter::filter(
+bool VehicleFilter::filter(
   const autoware_auto_perception_msgs::msg::Shape & shape,
-  [[maybe_unused]] const geometry_msgs::msg::Pose & pose)
+  [[maybe_unused]] const geometry_msgs::msg::Pose & pose,
+  [[maybe_unused]] const ShapeParameters & shape_param)
 {
-  constexpr float min_width = 2.0;
-  constexpr float max_width = 3.2;
-  constexpr float max_length = 17.0;
-  return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length);
+  std::cout << "VehicleFilter::filter with " << shape_param.name << std::endl;
+  float min_width = shape_param.shape_limitations.min_width;
+  float max_width = shape_param.shape_limitations.max_width;
+  float max_length = shape_param.shape_limitations.max_length;
+  float min_height = shape_param.shape_limitations.min_height;
+  float max_height = shape_param.shape_limitations.max_height;
+  return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length, min_height, max_height);
 }

perception/shape_estimation/lib/filter/car_filter.cpp

@@ -23,10 +23,11 @@
 
 using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
-ShapeEstimator::ShapeEstimator(bool use_corrector, bool use_filter, bool use_boost_bbox_optimizer)
+ShapeEstimator::ShapeEstimator(std::vector<ShapeParameters> & shapes, bool use_corrector, bool use_filter,  bool use_boost_bbox_optimizer)
 : use_corrector_(use_corrector),
   use_filter_(use_filter),
-  use_boost_bbox_optimizer_(use_boost_bbox_optimizer)
+  use_boost_bbox_optimizer_(use_boost_bbox_optimizer),
+  shapes_(shapes)
 {
 }
 
@@ -38,6 +39,11 @@ bool ShapeEstimator::estimateShapeAndPose(
 {
   autoware_auto_perception_msgs::msg::Shape shape;
   geometry_msgs::msg::Pose pose;
+
+  //get str name of label
+  std::string label_str = labelToString(label);
+  ShapeParameters shape_limitation = getShapeLimitation(label_str);
+
   // estimate shape
   bool reverse_to_unknown = false;
   if (!estimateOriginalShapeAndPose(label, cluster, ref_yaw_info, shape, pose)) {
@@ -46,7 +52,7 @@ bool ShapeEstimator::estimateShapeAndPose(
 
   // rule based filter
   if (use_filter_) {
-    if (!applyFilter(label, shape, pose)) {
+    if (!applyFilter(label, shape, pose, shape_limitation)) {
       reverse_to_unknown = true;
     }
   }
@@ -91,22 +97,15 @@ bool ShapeEstimator::estimateOriginalShapeAndPose(
 
 bool ShapeEstimator::applyFilter(
   const uint8_t label, const autoware_auto_perception_msgs::msg::Shape & shape,
-  const geometry_msgs::msg::Pose & pose)
+  const geometry_msgs::msg::Pose & pose, const ShapeParameters & shape_limitation)
 {
   std::unique_ptr<ShapeEstimationFilterInterface> filter_ptr;
-  if (label == Label::CAR) {
-    filter_ptr.reset(new CarFilter);
-  } else if (label == Label::BUS) {
-    filter_ptr.reset(new BusFilter);
-  } else if (label == Label::TRUCK) {
-    filter_ptr.reset(new TruckFilter);
-  } else if (label == Label::TRAILER) {
-    filter_ptr.reset(new TrailerFilter);
+  if (label == Label::CAR || label == Label::BUS || label == Label::TRUCK || label == Label::TRAILER) {
+    filter_ptr.reset(new VehicleFilter);
   } else {
     filter_ptr.reset(new NoFilter);
   }
-
-  return filter_ptr->filter(shape, pose);
+  return filter_ptr->filter(shape, pose, shape_limitation);
 }
 
 bool ShapeEstimator::applyCorrector(
@@ -134,3 +133,53 @@ bool ShapeEstimator::applyCorrector(
 
   return corrector_ptr->correct(shape, pose);
 }
+
+// get shape_limitation by name
+ShapeParameters ShapeEstimator::getShapeLimitation(const std::string & name)
+{
+  for (const auto & param : shapes_) {
+    if (param.name == name) {
+      // debug print
+      std::cout << "Shape Limitation in Query: " << param.name << std::endl;
+      // std::cout << "min_width: " << param.shape_limitations.min_width << std::endl;
+      // std::cout << "max_width: " << param.shape_limitations.max_width << std::endl;
+      // std::cout << "min_length: " << param.shape_limitations.min_length << std::endl;
+      // std::cout << "max_length: " << param.shape_limitations.max_length << std::endl;
+      // std::cout << "min_height: " << param.shape_limitations.min_height << std::endl;
+      // std::cout << "max_height: " << param.shape_limitations.max_height << std::endl;
+      return param;
+    }
+  }
+  ShapeParameters shape_limitation;
+  shape_limitation.name = "unknown";
+  shape_limitation.shape_limitations.min_width = 0.0;
+  shape_limitation.shape_limitations.max_width = 99.0;
+  shape_limitation.shape_limitations.min_length = 0.0;
+  shape_limitation.shape_limitations.max_length = 99.0;
+  shape_limitation.shape_limitations.min_height = 0.0;
+  shape_limitation.shape_limitations.max_height = 99.0;
+  return shape_limitation;
+}
+
+
+// convert label(uint8_t) to string
+std::string ShapeEstimator::labelToString(const uint8_t label)
+{
+  if (label == Label::CAR) {
+    return "car";
+  } else if (label == Label::TRUCK) {
+    return "truck";
+  } else if (label == Label::BUS) {
+    return "bus";
+  } else if (label == Label::TRAILER) {
+    return "trailer";
+  } else if (label == Label::BICYCLE) {
+    return "bicycle";
+  } else if (label == Label::MOTORCYCLE) {
+    return "bicycle";
+  } else if (label == Label::PEDESTRIAN) {
+    return "pedestrian";
+  } else {
+    return "unknown";
+  }
+}

perception/shape_estimation/lib/filter/no_filter.cpp

@@ -31,6 +31,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
@@ -50,8 +51,25 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option
   fix_filtered_objects_label_to_unknown_ =
     declare_parameter<bool>("fix_filtered_objects_label_to_unknown");
   RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer);
-  estimator_ =
-    std::make_unique<ShapeEstimator>(use_corrector, use_filter, use_boost_bbox_optimizer);
+
+  // get shape params
+  // set shapes as blank std::vector<ShapeParameters> constructor
+  std::vector<ShapeParameters> shapes;
+  {
+    std::vector<std::string> shape_names = {"bus", "car", "truck", "trailer", "bicycle"};
+    for (const auto & name : shape_names) {
+      ShapeParameters shape_param;
+      shape_param.name = name;
+      shape_param.shape_limitations.min_width = this->declare_parameter<float>("shapes." + name + ".shape.min_width");
+      shape_param.shape_limitations.max_width = this->declare_parameter<float>("shapes." + name + ".shape.max_width");
+      shape_param.shape_limitations.min_length = this->declare_parameter<float>("shapes." + name + ".shape.min_length");
+      shape_param.shape_limitations.max_length = this->declare_parameter<float>("shapes." + name + ".shape.max_length");
+      shape_param.shape_limitations.min_height = this->declare_parameter<float>("shapes." + name + ".shape.min_height");
+      shape_param.shape_limitations.max_height = this->declare_parameter<float>("shapes." + name + ".shape.max_height");
+      shapes.push_back(shape_param);
+    }
+  }
+  estimator_ = std::make_unique<ShapeEstimator>(shapes, use_corrector, use_filter, use_boost_bbox_optimizer);
 
   processing_time_publisher_ =
     std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "shape_estimation");