From 5250a7d3e8c6604e16fd45d896065057c5b5f6ae Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Mon, 10 Jun 2024 08:42:41 -0400
Subject: [PATCH] [jazzy] image_publisher: add field of view parameter
 (backport #985) (#992)

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`.
<hr>This is an automatic backport of pull request #985 done by
[Mergify](https://mergify.com).

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 image_publisher/doc/components.rst             |  1 +
 .../image_publisher/image_publisher.hpp        |  1 +
 image_publisher/src/image_publisher.cpp        | 18 ++++++++++++++++--
 3 files changed, 18 insertions(+), 2 deletions(-)

diff --git a/image_publisher/doc/components.rst b/image_publisher/doc/components.rst
index afd7fcf5e..e381edfc2 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: 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/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..dc6649e39 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 <cmath>
 #include <chrono>
+#include <limits>
 #include <string>
 #include <thread>
 #include <vector>
@@ -60,6 +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<double>(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"));
@@ -79,6 +82,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 +226,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<float>(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<double>::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<float>(camera_info_.width / 2), 0, f_approx,
     static_cast<float>(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<float>(camera_info_.width / 2), 0, 0, 1,
+  camera_info_.p = {f_approx, 0, static_cast<float>(camera_info_.width / 2), 0, 0, f_approx,
     static_cast<float>(camera_info_.height / 2), 0, 0, 0, 1, 0};
 
   timer_ = this->create_wall_timer(