diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/camera_projection.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/camera_projection.hpp index cc66dcd15ce62..e6661b627ffc9 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/camera_projection.hpp +++ b/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_; diff --git a/perception/autoware_image_projection_based_fusion/src/camera_projection.cpp b/perception/autoware_image_projection_based_fusion/src/camera_projection.cpp index af17d85989d35..6869f19364e99 100644 --- a/perception/autoware_image_projection_based_fusion/src/camera_projection.cpp +++ b/perception/autoware_image_projection_based_fusion/src/camera_projection.cpp @@ -51,11 +51,22 @@ CameraProjection::CameraProjection( grid_y_num_ = static_cast(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 diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index b51391bee1581..b6f74ec719a83 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -345,7 +345,15 @@ 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; } @@ -353,6 +361,10 @@ dc | dc dc dc dc ||zc| 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);