Add field of view parameter

ros-perception · May 27, 2024 · fc04923 · fc04923
1 parent a87b971
commit fc04923
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 2 deletions.
diff --git 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
@@ -31,6 +31,7 @@
 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
+#include <cmath>
 #include <chrono>
 #include <string>
 #include <thread>
@@ -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<double>(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<float>(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<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(