From 8d74d8ab4f42e0d811885660678a009f276a479e Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Tue, 28 May 2024 11:24:51 +0200 Subject: [PATCH] [rolling] image_publisher: add field of view parameter (#985) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Currently, the default value for focal length when no camera info is provided defaults to `1.0` rendering whole approximate intrinsics and projection matrices useless. Based on [this article](https://learnopencv.com/approximate-focal-length-for-webcams-and-cell-phone-cameras/), I propose a better approximation of the focal length based on the field of view of the camera. For most of the use cases, users will either know the field of view of the camera the used, or they already calibrated it ahead of time. If there is some documentation to fill. please let me know. This PR should be straightforward to port it to `Humble`, `Iron` and `Jazzy`. --------- Co-authored-by: Alejandro Hernández Cordero (cherry picked from commit 78d80f77b75bb80feb29484ed8fc6791a4c1deb9) # Conflicts: # image_publisher/doc/components.rst --- image_publisher/doc/components.rst | 23 +++++++++++++++++++ .../image_publisher/image_publisher.hpp | 1 + image_publisher/src/image_publisher.cpp | 18 +++++++++++++-- 3 files changed, 40 insertions(+), 2 deletions(-) create mode 100644 image_publisher/doc/components.rst diff --git a/image_publisher/doc/components.rst b/image_publisher/doc/components.rst new file mode 100644 index 000000000..e381edfc2 --- /dev/null +++ b/image_publisher/doc/components.rst @@ -0,0 +1,23 @@ +Nodes and Components +==================== + +image_publisher::ImagePublisher +------------------------------- +Component to publish sensor_msgs/Image, requires filename argument. +Also avialable as a ROS 2 node named ``image_publisher``. + +Published Topics +^^^^^^^^^^^^^^^^ + * **image_raw** (sensor_msgs/Image): ROS Image message of your input file. + * **camera_info** (sensor_msgs/CameraInfo): CameraInfo published along with Image. + +Parameters +^^^^^^^^^^ + * **filename** (string, default: ""): Name of image file to be published. + * **field_of_view** (double, default: 0): Camera field of view (deg) used to calculate focal length for camera info topic. + * **flip_horizontal** (bool, default: false): Flip output image horizontally. + * **flip_vertical** (bool, default: false): Flip output image vertically. + * **frame_id** (string, default: "camera") Frame id inserted in published + image and camera_info. + * **publish_rate** (double, default: 10): Rate to publish image (hz). + * **camera_info_uri** (string, default: ""): Path to camera info. diff --git a/image_publisher/include/image_publisher/image_publisher.hpp b/image_publisher/include/image_publisher/image_publisher.hpp index dc76bf890..278d940b6 100644 --- a/image_publisher/include/image_publisher/image_publisher.hpp +++ b/image_publisher/include/image_publisher/image_publisher.hpp @@ -63,6 +63,7 @@ class ImagePublisher : public rclcpp::Node rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_; std::string filename_; + double field_of_view_; bool flip_horizontal_; bool flip_vertical_; bool image_flipped_; diff --git a/image_publisher/src/image_publisher.cpp b/image_publisher/src/image_publisher.cpp index aac9511c5..dd90c6f2a 100644 --- a/image_publisher/src/image_publisher.cpp +++ b/image_publisher/src/image_publisher.cpp @@ -31,7 +31,9 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include #include +#include #include #include #include @@ -54,6 +56,7 @@ ImagePublisher::ImagePublisher(const rclcpp::NodeOptions & options) { pub_ = image_transport::create_camera_publisher(this, "image_raw"); + field_of_view_ = this->declare_parameter("field_of_view", static_cast(0)); flip_horizontal_ = this->declare_parameter("flip_horizontal", false); flip_vertical_ = this->declare_parameter("flip_vertical", false); frame_id_ = this->declare_parameter("frame_id", std::string("camera")); @@ -75,6 +78,11 @@ ImagePublisher::ImagePublisher(const rclcpp::NodeOptions & options) RCLCPP_INFO(get_logger(), "Reset filename as '%s'", filename_.c_str()); ImagePublisher::onInit(); return result; + } else if (parameter.get_name() == "field_of_view") { + field_of_view_ = parameter.as_double(); + RCLCPP_INFO(get_logger(), "Reset field_of_view as '%f'", field_of_view_); + ImagePublisher::onInit(); + return result; } else if (parameter.get_name() == "flip_horizontal") { flip_horizontal_ = parameter.as_bool(); RCLCPP_INFO(get_logger(), "Reset flip_horizontal as '%d'", flip_horizontal_); @@ -214,10 +222,16 @@ void ImagePublisher::onInit() camera_info_.height = image_.rows; camera_info_.distortion_model = "plumb_bob"; camera_info_.d = {0, 0, 0, 0, 0}; - camera_info_.k = {1, 0, static_cast(camera_info_.width / 2), 0, 1, + + double f_approx = 1.0; // FOV equal to 0 disables the approximation + if (std::abs(field_of_view_) > std::numeric_limits::epsilon()) { + // Based on https://learnopencv.com/approximate-focal-length-for-webcams-and-cell-phone-cameras/ + f_approx = (camera_info_.width / 2) / std::tan((field_of_view_ * M_PI / 180) / 2); + } + camera_info_.k = {f_approx, 0, static_cast(camera_info_.width / 2), 0, f_approx, static_cast(camera_info_.height / 2), 0, 0, 1}; camera_info_.r = {1, 0, 0, 0, 1, 0, 0, 0, 1}; - camera_info_.p = {1, 0, static_cast(camera_info_.width / 2), 0, 0, 1, + camera_info_.p = {f_approx, 0, static_cast(camera_info_.width / 2), 0, 0, f_approx, static_cast(camera_info_.height / 2), 0, 0, 0, 1, 0}; timer_ = this->create_wall_timer(