Skip to content

Commit

Permalink
image_publisher: add field of view parameter (backport #985) (#993)
Browse files Browse the repository at this point in the history
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 <[email protected]>
  • Loading branch information
mergify[bot] and Kotochleb authored May 28, 2024
1 parent 5b6dec1 commit 38133fa
Show file tree
Hide file tree
Showing 3 changed files with 40 additions and 2 deletions.
23 changes: 23 additions & 0 deletions image_publisher/doc/components.rst
Original file line number Diff line number Diff line change
@@ -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.
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down
18 changes: 16 additions & 2 deletions image_publisher/src/image_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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<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"));
Expand All @@ -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_);
Expand Down Expand Up @@ -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<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(
Expand Down

0 comments on commit 38133fa

Please sign in to comment.