fix FoV filtering

Signed-off-by: a-maumau <[email protected]>

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/camera_projection.hpp

@@ -62,7 +62,8 @@ class CameraProjection
 
   sensor_msgs::msg::CameraInfo camera_info_;
   uint32_t image_h_, image_w_;
-  double tan_h_x_, tan_h_y_;
+  double tan_h_x_left_, tan_h_x_right_;
+  double tan_h_y_top_, tan_h_y_bottom_;
 
   uint32_t cache_size_;
   float grid_w_size_;

perception/autoware_image_projection_based_fusion/src/camera_projection.cpp

@@ -51,11 +51,22 @@ CameraProjection::CameraProjection(
   grid_y_num_ = static_cast<uint32_t>(std::ceil(image_h_ / grid_h_size_));
   cache_size_ = grid_x_num_*grid_y_num_;
 
-  // for checking the views
-  // cx/fx
-  tan_h_x_ = camera_info.k.at(2)/camera_info.k.at(0);
-  // cy/fy
-  tan_h_y_ = camera_info.k.at(5)/camera_info.k.at(4);
+  // for checking the FoV
+  // x0/fx
+  const double fx = camera_info.k.at(0);
+  const double x0 = camera_info.k.at(2);
+  tan_h_x_left_ = -x0 / fx;
+  tan_h_x_right_ = (-x0 + image_w_) / fx;
+  //tan_h_x_left_ = (x0-image_w_*0.5) / fx;
+  //tan_h_x_right_ = (x0+image_w_) / fx;
+  // y0/fy
+  const double fy = camera_info.k.at(4);
+  const double y0 = camera_info.k.at(5);
+  tan_h_y_bottom_ = (-y0+image_h_*0.7) / fy; // actually top
+  tan_h_y_top_ = (-y0 + image_h_*0.9) / fy; // actually bottom
+  // for in this case
+  // if (py < tan_h_y_bottom_ * pz) return true;
+  // if (py > tan_h_y_top_ * pz) return true;
 }
 
 void CameraProjection::initialize(){
@@ -185,12 +196,22 @@ sensor_msgs::msg::CameraInfo CameraProjection::getCameraInfo(){
 
 bool CameraProjection::isOutsideHorizontalView(const float px, const float pz){
   // assuming the points' origin is centered on the camera
-  return pz <= 0.0 || px > tan_h_x_ * pz || px < -tan_h_x_ * pz;
+  if (pz <= 0.0) return true;
+  if (px < tan_h_x_left_ * pz) return true;
+  if (px > tan_h_x_right_ * pz) return true;
+
+  // in the view
+  return false;
 }
 
 bool CameraProjection::isOutsideVerticalView(const float py, const float pz){
   // assuming the points' origin is centered on the camera
-  return pz <= 0.0 || py > tan_h_y_ * pz || py < -tan_h_y_ * pz;
+  if (pz <= 0.0) return true;
+  if (py < tan_h_y_bottom_ * pz) return true;
+  if (py > tan_h_y_top_ * pz) return true;
+
+  // in the view
+  return false;
 }
 
 }  // namespace autoware::image_projection_based_fusion

perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -332,14 +332,26 @@ dc   | dc dc dc  dc ||zc|
     p_y = point_camera.y();
     p_z = point_camera.z();
 
-    if (camera_projectors_[image_id].isOutsideHorizontalView(p_x, p_z)) {
+    //if (camera_projectors_[image_id].isOutsideHorizontalView(p_x, p_z)) {
+    //  continue;
+    //}
+
+    //if (camera_projectors_[image_id].isOutsideVerticalView(p_y, p_z)) {
+    //  continue;
+    //}
+
+    if (p_z <= 0.0) {
       continue;
     }
 
     // project
     Eigen::Vector2d projected_point;
     if (camera_projectors_[image_id].calcImageProjectedPoint(
           cv::Point3d(p_x, p_y, p_z), projected_point)) {
+      //if (camera_projectors_[image_id].isOutsideHorizontalView(projected_point.x(), projected_point.y())) {
+      //  continue;
+      //}
+
       // iterate 2d bbox
       for (const auto & feature_object : objects) {
         sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi;

perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp

@@ -132,6 +132,8 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs(
   }
   const auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec);
   const sensor_msgs::msg::CameraInfo & camera_info = camera_projectors_[image_id].getCameraInfo();
+  const double image_width = static_cast<double>(camera_info.width);
+  const double image_height = static_cast<double>(camera_info.height);
 
   for (std::size_t obj_i = 0; obj_i < input_object_msg.objects.size(); ++obj_i) {
     std::vector<Eigen::Vector3d> vertices_camera_coord;
@@ -152,7 +154,7 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs(
       transformPoints(vertices, object2camera_affine, vertices_camera_coord);
     }
 
-    double min_x(camera_info.width), min_y(camera_info.height), max_x(0.0), max_y(0.0);
+    double min_x(image_width), min_y(image_height), max_x(0.0), max_y(0.0);
     std::size_t point_on_image_cnt = 0;
     for (const auto & point : vertices_camera_coord) {
       if (point.z() <= 0.0) {