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 be14d9a0b0fb3..ac15679bc9c16 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 @@ -339,9 +339,16 @@ dc | dc dc dc dc ||zc| if (p_z <= 0.0 || p_x > (tan_h_.at(image_id) * p_z) || p_x < (-tan_h_.at(image_id) * p_z)) { continue; } - // project - Eigen::Vector2d projected_point = - calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(p_x, p_y, p_z)); + + // project, try and catch. if it fails, return + Eigen::Vector2d projected_point; + try { + projected_point = + calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(p_x, p_y, p_z)); + } catch (const cv::Exception & e) { + RCLCPP_WARN_STREAM(this->get_logger(), "Error in projection: " << e.what()); + continue; + } // iterate 2d bbox for (const auto & feature_object : objects) { diff --git a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index d4d5de3cfd381..40e5b1267b77e 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -131,8 +131,16 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } - Eigen::Vector2d projected_point = - calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z)); + // project, try and catch. if it fails, return + Eigen::Vector2d projected_point; + try { + projected_point = + calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z)); + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM(this->get_logger(), "Error in projection: " << e.what()); + return; + } + if ( 0 <= static_cast(projected_point.x()) && static_cast(projected_point.x()) <= static_cast(camera_info.width) - 1 && diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index e2197c148bcf9..24c98973dfe5f 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -146,8 +146,15 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( continue; } - Eigen::Vector2d proj_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z())); + // project, try and catch. if it fails, return + Eigen::Vector2d proj_point; + try { + proj_point = calcRawImageProjectedPoint( + pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z())); + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM(this->get_logger(), "Error in projection: " << e.what()); + return object_roi_map; + } min_x = std::min(proj_point.x(), min_x); min_y = std::min(proj_point.y(), min_y); diff --git a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 40406d6e420b7..ec9f1a1b33708 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -142,8 +142,17 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( if (transformed_z <= 0.0) { continue; } - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z)); + + // project, try and catch. if it fails, return + Eigen::Vector2d projected_point; + try { + projected_point = calcRawImageProjectedPoint( + pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z)); + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM(this->get_logger(), "Error in projection: " << e.what()); + return; + } + for (std::size_t i = 0; i < output_objs.size(); ++i) { auto & feature_obj = output_objs.at(i); const auto & check_roi = feature_obj.feature.roi; diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 5c93fa6c2fd1b..fef532644ff52 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -120,8 +120,15 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( continue; } - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z)); + // project, try and catch. if it fails, return + Eigen::Vector2d projected_point; + try { + projected_point = calcRawImageProjectedPoint( + pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z)); + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM(this->get_logger(), "Error in projection: " << e.what()); + return; + } bool is_inside_image = projected_point.x() > 0 && projected_point.x() < camera_info.width && projected_point.y() > 0 && projected_point.y() < camera_info.height;