Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(image_projection_based_fusion): handle projection errors in image fusion nodes #7747

Merged
merged 5 commits into from
Aug 19, 2024
Merged

fix(image_projection_based_fusion): handle projection errors in image fusion nodes #7747

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
72d894d
fix: add check for camera distortion model
technolojin Aug 14, 2024
15d3a7d
feat(utils): add const qualifier to local variables in checkCameraInf…
technolojin Aug 15, 2024
eda16f9
style(pre-commit): autofix
pre-commit-ci[bot] Aug 15, 2024
b96c902
chore(utils): update checkCameraInfo function to use RCLCPP_ERROR_STR…
technolojin Aug 15, 2024
7d28fed
Merge branch 'main' into fix/image_point_projection_safely
technolojin Aug 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions ...ge_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@

#include "autoware_perception_msgs/msg/shape.hpp"
#include "tier4_perception_msgs/msg/detected_object_with_feature.hpp"
#include <sensor_msgs/msg/camera_info.hpp>

#include <image_geometry/pinhole_camera_model.h>
#include <pcl/point_cloud.h>
Expand All @@ -62,6 +63,8 @@ struct PointData
size_t orig_index;
};

bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info);

Eigen::Vector2d calcRawImageProjectedPoint(
const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d);

Expand Down
2 changes: 2 additions & 0 deletions perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -271,6 +271,8 @@ void PointPaintingFusionNode::fuseOnSingleImage(
return;
}

if (!checkCameraInfo(camera_info)) return;

std::vector<sensor_msgs::msg::RegionOfInterest> debug_image_rois;
std::vector<Eigen::Vector2d> debug_image_points;

Expand Down
2 changes: 2 additions & 0 deletions perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,8 @@
const DetectedObjectsWithFeature & input_roi_msg,
const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg)
{
if (!checkCameraInfo(camera_info)) return;

Check warning on line 87 in perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

RoiClusterFusionNode::fuseOnSingleImage increases in cyclomatic complexity from 42 to 43, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
image_geometry::PinholeCameraModel pinhole_camera_model;
pinhole_camera_model.fromCameraInfo(camera_info);

Expand Down
2 changes: 2 additions & 0 deletions perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage(
const sensor_msgs::msg::CameraInfo & camera_info,
DetectedObjects & output_object_msg __attribute__((unused)))
{
if (!checkCameraInfo(camera_info)) return;

Eigen::Affine3d object2camera_affine;
{
const auto transform_stamped_optional = getTransformStamped(
Expand Down
2 changes: 2 additions & 0 deletions perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,8 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
if (input_pointcloud_msg.data.empty()) {
return;
}
if (!checkCameraInfo(camera_info)) return;

std::vector<DetectedObjectWithFeature> output_objs;
// select ROIs for fusion
for (const auto & feature_obj : input_roi_msg.feature_objects) {
Expand Down
2 changes: 2 additions & 0 deletions ...eption/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,8 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
if (input_pointcloud_msg.data.empty()) {
return;
}
if (!checkCameraInfo(camera_info)) return;

cv_bridge::CvImagePtr in_image_ptr;
try {
in_image_ptr = cv_bridge::toCvCopy(
Expand Down
23 changes: 23 additions & 0 deletions perception/autoware_image_projection_based_fusion/src/utils/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2022 TIER IV, Inc.

Check warning on line 1 in perception/autoware_image_projection_based_fusion/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Overall Code Complexity 

This module has a mean cyclomatic complexity of 4.10 across 10 functions. The mean complexity threshold is 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -14,8 +14,31 @@

#include "autoware/image_projection_based_fusion/utils/utils.hpp"

#include <sensor_msgs/distortion_models.hpp>

namespace autoware::image_projection_based_fusion
{
bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info)

Check warning on line 21 in perception/autoware_image_projection_based_fusion/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

perception/autoware_image_projection_based_fusion/src/utils/utils.cpp#L21 

Added line #L21 was not covered by tests
{
bool is_supported_model =
(camera_info.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
camera_info.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL);
if (!is_supported_model) {
RCLCPP_WARN_STREAM(
rclcpp::get_logger("image_projection_based_fusion"),
"checkCameraInfo: Unsupported distortion model: " << camera_info.distortion_model);
return false;

Check warning on line 30 in perception/autoware_image_projection_based_fusion/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

perception/autoware_image_projection_based_fusion/src/utils/utils.cpp#L30 

Added line #L30 was not covered by tests
}
bool is_supported_distortion_param = (camera_info.d.size() == 5 || camera_info.d.size() == 8);
if (!is_supported_distortion_param) {
RCLCPP_WARN_STREAM(
rclcpp::get_logger("image_projection_based_fusion"),
"checkCameraInfo: Unsupported distortion coefficients size: " << camera_info.d.size());
return false;

Check warning on line 37 in perception/autoware_image_projection_based_fusion/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

perception/autoware_image_projection_based_fusion/src/utils/utils.cpp#L37 

Added line #L37 was not covered by tests
}
return true;
}

Eigen::Vector2d calcRawImageProjectedPoint(
const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d)
{
Expand Down
Loading