From fc049230e9a9556e0f5a39be54ca128d3358e750 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Sun, 26 May 2024 13:39:39 +0000 Subject: [PATCH 1/6] Add field of view parameter --- .../include/image_publisher/image_publisher.hpp | 1 + image_publisher/src/image_publisher.cpp | 14 ++++++++++++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/image_publisher/include/image_publisher/image_publisher.hpp b/image_publisher/include/image_publisher/image_publisher.hpp index 6c22b0199..ae31210d0 100644 --- a/image_publisher/include/image_publisher/image_publisher.hpp +++ b/image_publisher/include/image_publisher/image_publisher.hpp @@ -65,6 +65,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 retry_; // If enabled will retry loading image from the filename_ diff --git a/image_publisher/src/image_publisher.cpp b/image_publisher/src/image_publisher.cpp index 1f8068de4..ea6b7bbdb 100644 --- a/image_publisher/src/image_publisher.cpp +++ b/image_publisher/src/image_publisher.cpp @@ -31,6 +31,7 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include #include #include #include @@ -60,6 +61,7 @@ ImagePublisher::ImagePublisher( std::string topic_name = node_base->resolve_topic_or_service_name("image_raw", false); pub_ = image_transport::create_camera_publisher(this, topic_name); + field_of_view_ = this->declare_parameter("field_of_view", static_cast(50)); 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")); @@ -79,6 +81,11 @@ ImagePublisher::ImagePublisher( 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_); @@ -218,10 +225,13 @@ 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, + + // Based on https://learnopencv.com/approximate-focal-length-for-webcams-and-cell-phone-cameras/ + double f_approx = (camera_info_.width / 2) / 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( From 4762cac9b1e926facd0aa14b6a19e1b79caceab2 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Mon, 27 May 2024 12:21:45 +0200 Subject: [PATCH 2/6] Use std::tan MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- image_publisher/src/image_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/image_publisher/src/image_publisher.cpp b/image_publisher/src/image_publisher.cpp index ea6b7bbdb..46f0dc7c6 100644 --- a/image_publisher/src/image_publisher.cpp +++ b/image_publisher/src/image_publisher.cpp @@ -227,7 +227,7 @@ void ImagePublisher::onInit() camera_info_.d = {0, 0, 0, 0, 0}; // Based on https://learnopencv.com/approximate-focal-length-for-webcams-and-cell-phone-cameras/ - double f_approx = (camera_info_.width / 2) / tan((field_of_view_ * M_PI / 180) / 2); + double 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}; From 70bc4abdf6b84e7570f5c724d09a1c937ec199fd Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Mon, 27 May 2024 12:45:34 +0200 Subject: [PATCH 3/6] Document parameters --- image_publisher/doc/components.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/image_publisher/doc/components.rst b/image_publisher/doc/components.rst index afd7fcf5e..b9669f883 100644 --- a/image_publisher/doc/components.rst +++ b/image_publisher/doc/components.rst @@ -14,6 +14,7 @@ Published Topics Parameters ^^^^^^^^^^ * **filename** (string, default: ""): Name of image file to be published. + * **field_of_view** (double, default: 50): 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 From 7f81af69827051bce4fcd8261d02bf6e688cae92 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Mon, 27 May 2024 23:07:10 +0200 Subject: [PATCH 4/6] Allow disabling of focal length calculation --- image_publisher/doc/components.rst | 2 +- image_publisher/src/image_publisher.cpp | 11 ++++++++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/image_publisher/doc/components.rst b/image_publisher/doc/components.rst index b9669f883..e381edfc2 100644 --- a/image_publisher/doc/components.rst +++ b/image_publisher/doc/components.rst @@ -14,7 +14,7 @@ Published Topics Parameters ^^^^^^^^^^ * **filename** (string, default: ""): Name of image file to be published. - * **field_of_view** (double, default: 50): Camera field of view (deg) used to calculate focal length for camera info topic. + * **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 diff --git a/image_publisher/src/image_publisher.cpp b/image_publisher/src/image_publisher.cpp index 46f0dc7c6..6b9ca811c 100644 --- a/image_publisher/src/image_publisher.cpp +++ b/image_publisher/src/image_publisher.cpp @@ -33,6 +33,7 @@ #include #include +#include #include #include #include @@ -61,7 +62,7 @@ ImagePublisher::ImagePublisher( std::string topic_name = node_base->resolve_topic_or_service_name("image_raw", false); pub_ = image_transport::create_camera_publisher(this, topic_name); - field_of_view_ = this->declare_parameter("field_of_view", static_cast(50)); + 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")); @@ -226,8 +227,12 @@ void ImagePublisher::onInit() camera_info_.distortion_model = "plumb_bob"; camera_info_.d = {0, 0, 0, 0, 0}; - // Based on https://learnopencv.com/approximate-focal-length-for-webcams-and-cell-phone-cameras/ - double f_approx = (camera_info_.width / 2) / std::tan((field_of_view_ * M_PI / 180) / 2); + 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}; From 861fc8d7b392f507452eba2e340b9437ecd9989a Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Mon, 27 May 2024 23:08:06 +0200 Subject: [PATCH 5/6] Apply formatting --- image_publisher/src/image_publisher.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/image_publisher/src/image_publisher.cpp b/image_publisher/src/image_publisher.cpp index 6b9ca811c..1b401c9c2 100644 --- a/image_publisher/src/image_publisher.cpp +++ b/image_publisher/src/image_publisher.cpp @@ -228,8 +228,7 @@ void ImagePublisher::onInit() camera_info_.d = {0, 0, 0, 0, 0}; double f_approx = 1.0; // FOV equal to 0 disables the approximation - if (std::abs(field_of_view_) > std::numeric_limits::epsilon()) - { + 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); } From 76a1aa87a587b9927c500c7db2193eb634ab210d Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Mon, 27 May 2024 23:26:55 +0200 Subject: [PATCH 6/6] Fix comment --- image_publisher/src/image_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/image_publisher/src/image_publisher.cpp b/image_publisher/src/image_publisher.cpp index 1b401c9c2..dc6649e39 100644 --- a/image_publisher/src/image_publisher.cpp +++ b/image_publisher/src/image_publisher.cpp @@ -227,7 +227,7 @@ void ImagePublisher::onInit() camera_info_.distortion_model = "plumb_bob"; camera_info_.d = {0, 0, 0, 0, 0}; - double f_approx = 1.0; // FOV equal to 0 disables the approximation + 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);