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

@@ -345,14 +345,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;
@@ -369,7 +381,7 @@ dc   | dc dc dc  dc ||zc|
             }
           }
         }
-#if 0
+#if 1
         // Parallelizing loop don't support push_back
         if (debugger_) {
           debug_image_points.push_back(projected_point);