ethz-asl / image_undistort

A compact package for undistorting images directly from kalibr calibration files. Can also perform dense stereo estimation

image_undistort exists to handle all the odd situations image_proc doesn't quite cover. Some examples of this are

• working with images that don't have a camera_info topic
• undistortion of images using equidistant or other less common camera models
• turning a location in a distorted image into a bearing vector

If you have an image undistortion / stereo imaging problem that the library doesn't cover, create an issue and I'll look at adding it. Note that the automatic image size approach used will fail for cameras with a fov greater than 180 degrees.

This repo contains six related ros nodes-

• image_undistort_node: Undistorts and changes images intrinsics and resolution.
• stereo_info_node: Calculates the camera information needed for stereo rectification.
• stereo_undistort_node: Combines the functionality of the above two nodes to perform stereo image rectification.
• depth_node: Converts two undistorted images and their camera information into a disparity image and a pointcloud.
• dense_stereo_node: Performs the full dense stereo estimation (internally this node is just the stereo_undistort nodelet and the depth nodelet).
• point_to_bearing_node: Takes in a 2D image location and transforms it into a bearing vector.

Dependencies

Image undistort depends on ROS, OpenCV and Eigen. The point to bearing node also depends on NLopt (installed with apt install libnlopt-dev) and will only be built if it is found.

Supported Camera and Distortion Models

The only supported output is the pinhole camera model with no distortion. Supported input models:

• Pinhole with no distortion
• Pinhole with radial-tangential distortion
• Pinhole with equidistant distortion
• Omnidirectional with no distortion
• Omindirectional with rad-tan distortion
• FOV
• Unified
• Extended Unified
• Double Sphere

image_undistort_node:

A simple node for undistorting images. Handles plumb bob (aka radial-tangential), fov and equidistant distortion models. It can either use standard ros camera_info topics or load camera models in a form that is compatible with the camchain.yaml files produced by Kalibr. Note this node can also be run as a nodelet named image_undistort/ImageUndistortNodelet.

The node has several possible use cases:

• Undistort images. The default usage of the node, outputting an undistorted version of an input image.
• Modify the image resolution and intrinsics. The node supports projecting from and to any valid projection matrix and resolution.
• Provide a camera_info topic for an image. In this mode ros params are used to build a camera info message that is published in sync with the image messages. This allows the use of ros nodes that require camera info with devices and bags that do not provide it.

Parameters:

• queue size The length of the queues the node uses for topics (default: 10).
• input_camera_info_from_ros_params If false the node will subscribe to a camera_info ros topic named input/camera_info to obtain the input camera parameters. If true the input camera parameters will be loaded from ros parameters. See the parameters format section for further details. (default: false).
• output_camera_info_source The source to use when obtaining the output camera parameters. The possible case-insensitive options are,
  • "auto_generated" The default value. In this mode "good" output parameters are automatically generated based on the input image. focal length is the average of fx and fy of the input, the center point is in the center of the image, R=I and translation is preserved. Resolution is set to the largest area that contains no empty pixels. The size of the output can also be modified with the scale parameter.
  • "match_input" The output projection matrix and resolution, exactly match the inputs.
  • "ros_params" The output camera parameters are loaded from ros parameters. See the parameters format section for further details.
  • "camera_info" The output parameters are found through subscribing to a camera_info ros topic named output/camera_info
• input_camera_namespace If the input camera parameters are loaded from ros parameters this is the namespace that will be searched. This is needed to allow both input and output to be loaded from parameters. (default: "input_camera")
• output_camera_namespace If the output camera parameters are loaded from ros parameters this is the namespace that will be searched. (default: "output_camera").
• process_image True to output a processed image, false if only a camera_info topic should be generated. (default: true).
• undistort_image True to undistort the images, false to keep the distortion. (default: true).
• process_every_nth_frame Used to temporarily down-sample the images, if it is <= 1 every frame will be processed. (default: 1).
• output_image_type Converts the output image to the specified format, set to the empty string "" to preserve the input type. See the cv_bridge tutorial for possible format strings. (default: "").
• scale Only used if output_camera_info_source is set to "auto_generated" or "match_input". The output focal length will be multiplied by this value. If "auto_generated" is set the image size will also be increased by this factor. (default: 1.0).
• publish_tf True to publish the tf between the input and output image. If the undistortion involves changes to the rotation matrix the frame that the image is in will change. This tf gives that change. (default: true)
• output_frame The name of the frame of the output images. (default: "output_camera")
• rename_input_frame If the input frame should be renamed in the published topics and tf tree. (default: false)
• input_frame Only used if rename_input_frame is true. The name of the frame of the input images. (default: "input_camera")
• rename_radtan_plumb_bob If true the radial-tangential distortion model will be called "plumb_bob" in the output camera_info, this is needed by some ros image processing nodes. If false it will be called "radtan". (default: false).

Input/Output Topics

Many of these topics are dependent on the parameters set above and may not appear or may be renamed under some settings.

• input/image input image topic
• input/camera_info input camera info topic
• output/image output image topic
• output/camera_info output camera info topic

Loading Camera Information from ROS Parameters:

Camera information can be loaded from ROS parameters. These parameters are typically set using . The format used by this node is compatible with the camchains generated by Kalibr. The follow steps are used when loading this information.

1. A 3x3 intrinscs matrix named K is searched for. If it is found it is loaded. If it is not found a 1x4 vector named intrinsics is loaded, this contains the parameters (fx, fy, cx, cy). If neither parameters are given the node displays an error and terminates.
2. A 1x2 vector named resolution is loaded which contains the parameters (width, height). Again, if not given the node displays an error and terminates.
3. A 4x4 transformation matrix T_cn_cnm1 is searched for. If it is found it is loaded. Otherwise it is searched for under the name T and if found loaded. If neither are found the node continues.
4. A 4x3 projection matrix P is searched for. If it is found it is loaded. If P was found but T was not, P and K are used to construct T, otherwise T is set to identity. If P was not found it is constructed from K and T.
5. If an output is being loaded, the loading of parameters is completed. For input cameras the distortion properties are now loaded
6. A 1xn vector D is loaded. If it is not found or is less than 5 elements long it is padded with zeros.
7. A string distortion_model is loaded and converted to lower-case. If it is not found it is set to "radtan".

stereo_info_node:

A node that takes in the properties of two cameras and outputs the camera info required to rectify them so that stereo reconstruction can be performed. The rectification is performed such that only x translation is present between the cameras. The focal points are in the image centers, fx=fy and the image resolution is set to be the largest frame that contains no empty pixels. Note this node can also be run as a nodelet named image_undistort/StereoInfoNodelet.

Parameters:

• queue size The length of the queues the node uses for topics (default: 10).
• input_camera_info_from_ros_params If false the node will subscribe to a camera_info ros topic named input/camera_info to obtain the input camera parameters. If true the input camera parameters will be loaded from ros parameters. See the parameters format section for further details. (default: false).
• first_camera_namespace If the first camera parameters are loaded from ros parameters this is the namespace that will be searched. (default: "first_camera")
• second_camera_namespace If the second camera parameters are loaded from ros parameters this is the namespace that will be searched. (default: "second_camera").
• scale Only used if output_camera_info_source is set to "auto_generated". The output focal length will be multiplied by this value. This has the effect of resizing the image by this scale factor. (default: 1.0). rename_radtan_plumb_bob If true the radial-tangential distortion model will be called "plumb_bob" in the output camera_info, this is needed by some ros image processing nodes. If false it will be called "radtan". (default: false).

Input/Output Topics

Many of these topics are dependent on the parameters set above and may not appear or may be renamed under some settings.

• raw/first/image first input image topic, only needed if loading camera parameters from ros params (used for timing information)
• raw/second/image second input image topic, only needed if loading camera parameters from ros params (used for timing information)
• raw/first/camera_info first input camera info topic
• raw/second/camera_info second input camera info topic
• rect/first/camera_info first output camera info topic
• rect/second/camera_info second output camera info topic

stereo_undistort_node:

A node that takes in the images and properties of two cameras and outputs rectified stereo images with their corresponding camera parameters. The rectification is performed such that only x translation is present between the cameras. The focal points are in the image centers, fx=fy and the image resolution is set to be the largest frame that contains no empty pixels. Note this node can also be run as a nodelet named image_undistort/StereoUndistortNodelet.

Parameters:

• queue size The length of the queues the node uses for topics (default: 10).
• input_camera_info_from_ros_params If false the node will subscribe to a camera_info ros topic named input/camera_info to obtain the input camera parameters. If true the input camera parameters will be loaded from ros parameters. See the parameters format section for further details. (default: false).
• first_camera_namespace If the first camera parameters are loaded from ros parameters this is the namespace that will be searched. (default: "first_camera")
• second_camera_namespace If the second camera parameters are loaded from ros parameters this is the namespace that will be searched. (default: "second_camera").
• scale Only used if output_camera_info_source is set to "auto_generated". The output focal length will be multiplied by this value. This has the effect of resizing the image by this scale factor. (default: 1.0).
• process_every_nth_frame Used to temporarily down-sample the images, if it is <= 1 every frame will be processed. (default: 1).
• output_image_type Converts the output images to the specified format, set to the empty string "" to preserve the input type. See the cv_bridge tutorial for possible format strings. (default: "").
• scale The output focal length will be multiplied by this value. This has the effect of resizing the image by this scale factor. (default: 1.0).
• T_invert Only used if loading parameters from ros params. True to invert the given transformations. (default: false)
• publish_tf True to publish the tf between the first input and output image. If the undistortion involves changes to the rotation matrix the frame that the image is in will change. This tf gives that change. (default: true)
• first_output_frame The name of the frame of the first camera output images. (default: "first_camera_rect")
• second_output_frame The name of the frame of the second camera output images. (default: "second_camera_rect")
• rename_input_frame If the input frame should be renamed in the published topics and tf tree. (default: false)
• first_input_frame Only used if rename_input_frame is true. The name of the frame of the first input images. (default: "first_camera")
• second_input_frame Only used if rename_input_frame is true. The name of the frame of the second input images. (default: "second_camera")
• rename_radtan_plumb_bob If true the radial-tangential distortion model will be called "plumb_bob" in the output camera_info, this is needed by some ros image processing nodes. If false it will be called "radtan". (default: false).

Input/Output Topics

Many of these topics are dependent on the parameters set above and may not appear or may be renamed under some settings.

• raw/first/image first input image topic
• raw/second/image second input image topic
• raw/first/camera_info first input camera info topic
• raw/second/camera_info second input camera info topic
• rect/first/image first output image topic
• rect/second/image second output image topic
• rect/first/camera_info first output camera info topic
• rect/second/camera_info second output camera info topic

depth_node:

A node that takes in the rectified images and properties of two cameras and outputs a disparity image and a pointcloud. The node uses the camera_info topics to figure out which camera is the left one and which is the right one. Internally the node makes use of the opencv stereo block matcher to perform the depth estimation. Note this node can also be run as a nodelet named image_undistort/DepthNodelet.

Parameters:

• queue size The length of the queues the node uses for topics. (default: 10)
• pre_filter_type The prefilter type (possible values: 'xsobel', 'normalized_response', default: 'xsobel')
• pre_filter_size The size of the prefilter used in StereoBM. (default: 9)
• pre_filter_cap The upper cap on the prefilter used in StereoBM. (default: 31)
• sad_window_size The window size used when performing the stereo matching, note the efficiency of the implementation reduces if this value is greater than 21 (default: 21)
• min_disparity The minimum disparity checked in StereoBM. (default: 0)
• num_disparities The number of disparities checked in StereoBM. (default: 64)
• texture_threshold Minimum texture a patch requires to be matched in StereoBM. (default: 10)
• uniqueness_ratio Minimum margin by which the best matching disparity must 'win' in StereoBM. (default: 15)
• speckle_range Parameter used for removing speckle in StereoBM. (default: 0)
• speckle_window_size Window size used for speckle removal in StereoBM. (default: 0)
• use_sgbm Use SGBM (Semi-Global Block Matching) instead of BM (Block Matching)? (default: false)
• p1 The first parameter controlling the disparity smoothness, only available in SGBM (default: 120)
• p2 The second parameter controlling the disparity smoothness, only available in SGBM (default: 240)
• disp_12_max_diff Maximum allowed difference (in integer pixel units) in the left-right disparity check, only available in SGBM (default: -1)
• use_mode_HH Run the full-scale two-pass dynamic programming algorithm. It will consume O(WHnumDisparities) bytes, which is large for 640x480 stereo and huge for HD-size pictures. Only available in SGBM (default: false)
• do_median_blur Apply median blur to the final disparity image (default: true)

Input/Output Topics

• rect/first/image first input image topic
• rect/second/image second input image topic
• rect/first/camera_info first input camera info topic
• rect/second/camera_info second input camera info topic
• disparity/image output disparity image
• pointcloud output pointcloud
• freespace_pointcloud output freespace pointcloud

dense_stereo_node:

A node for producing dense stereo images. Internally this node simply combines 2 nodelets.

• image_undistort/StereoUndistortNodelet Used to set up the stereo system and rectify the images.
• image_undistort/DepthNodelet Generates disparity images and pointclouds from the rectified images.

Parameters:

• queue size The length of the queues the node uses for topics (default: 10).
• input_camera_info_from_ros_params If false the node will subscribe to a camera_info ros topic named input/camera_info to obtain the input camera parameters. If true the input camera parameters will be loaded from ros parameters. See the parameters format section for further details. (default: false).
• first_camera_namespace If the first camera parameters are loaded from ros parameters this is the namespace that will be searched. (default: "first_camera")
• second_camera_namespace If the second camera parameters are loaded from ros parameters this is the namespace that will be searched. (default: "second_camera").
• scale Only used if output_camera_info_source is set to "auto_generated". The output focal length will be multiplied by this value. This has the effect of resizing the image by this scale factor. (default: 1.0).
• T_invert Only used if loading parameters from ros params. True to invert the given transformations. (default: false)
• process_every_nth_frame Used to temporarily down-sample the images, if it is <= 1 every frame will be processed. (default: 1).
• output_image_type Converts the output images to the specified format, set to the empty string "" to preserve the input type. See the cv_bridge tutorial for possible format strings. (default: "").
• scale The output focal length will be multiplied by this value. This has the effect of resizing the image by this scale factor. (default: 1.0).
• publish_tf True to publish the tf between the first input and output image. If the undistortion involves changes to the transformation matrix the frame that the image is in will change, this occurs during most image rectifications. This tf gives that change. (default: true)
• output_frame The name of the frame of the output images. (default: "first_camera_rect")
• rename_input_frame If the input frame should be renamed in the published topics and tf tree. (default: false)
• first_input_frame Only used if rename_input_frame is true. The name of the frame of the first input images. (default: "first_camera")
• second_input_frame Only used if rename_input_frame is true. The name of the frame of the second input images. (default: "second_camera") rename_radtan_plumb_bob If true the radial-tangential distortion model will be called "plumb_bob" in the output camera_info, this is needed by some ros image processing nodes. If false it will be called "radtan". (default: false).
• pre_filter_type The prefilter type (possible values: 'xsobel', 'normalized_response', default: 'xsobel')
• pre_filter_size The size of the prefilter used in StereoBM. (default: 9)
• pre_filter_cap The upper cap on the prefilter used in StereoBM. (default: 31)
• sad_window_size The window size used when performing the stereo matching, note the efficiency of the implementation reduces if this value is greater than 21 (default: 21)
• min_disparity The minimum disparity checked in StereoBM. (default: 0)
• num_disparities The number of disparities checked in StereoBM. (default: 64)
• texture_threshold Minimum texture a patch requires to be matched in StereoBM. (default: 10)
• uniqueness_ratio Minimum margin by which the best matching disparity must 'win' in StereoBM. (default: 15)
• speckle_range Parameter used for removing speckle in StereoBM. (default: 0)
• speckle_window_size Window size used for speckle removal in StereoBM. (default: 0)
• use_sgbm Use SGBM (Semi-Global Block Matching) instead of BM (Block Matching)? (default: false)
• p1 The first parameter controlling the disparity smoothness, only available in SGBM (default: 120)
• p2 The second parameter controlling the disparity smoothness, only available in SGBM (default: 240)
• disp_12_max_diff Maximum allowed difference (in integer pixel units) in the left-right disparity check, only available in SGBM (default: -1)
• use_mode_HH Run the full-scale two-pass dynamic programming algorithm. It will consume O(WHnumDisparities) bytes, which is large for 640x480 stereo and huge for HD-size pictures. Only available in SGBM (default: false)
• do_median_blur Apply median blur to the final disparity image (default: true)

Input/Output Topics

Many of these topics are dependent on the parameters set above and may not appear or may be renamed under some settings.

• raw/first/image first input image topic
• raw/second/image second input image topic
• raw/first/camera_info first input camera info topic
• raw/second/camera_info second input camera info topic
• rect/first/image first output rectified image topic
• rect/second/image second output rectified image topic
• rect/first/camera_info first output camera info topic
• rect/second/camera_info second output camera info topic
• disparity output disparity image topic
• pointcloud output pointcloud topic

point_to_bearing_node:

A node for converting a point in a distorted image to a unit bearing vector. Note this node can also be run as a nodelet named image_undistort/PointToBearingNodelet.

Parameters:

• queue size The length of the queues the node uses for topics (default: 10).
• input_camera_info_from_ros_params If false the node will subscribe to a camera_info ros topic named input/camera_info to obtain the input camera parameters. If true the input camera parameters will be loaded from ros parameters. See the parameters format section for further details. (default: false).

Input/Output Topics

Many of these topics are dependent on the parameters set above and may not appear or may be renamed under some settings.

• input/camera_info camera info topic
• image_point input location of the point of interest in the image
• bearing unit bearing to point

image_undistort's Issues

Small bug in function CameraParametersPair::generateCameraInfoMessage

The function CameraParametersPair::generateCameraInfoMessage creates a null vector for the distortion coefficients at line 500, if the system is in the following mode: io == CameraIO::OUTPUT || distortion_processing_ == DistortionProcessing::UNDISTORT

It should be: io == CameraIO::OUTPUT && distortion_processing_ == DistortionProcessing::UNDISTORT

Otherwise, when undistorting images, both the raw camera_info and the rect camera_info have null distortion coefficients, when only the rect camera_info should have them null. With a logical AND, this is fixed.

Please let me know if I am wrong with this.

Regards,

Audren

stereo_undistort not working with T265 camera

I am trying to use the stereo_undistort node together with the Intel Realsense T265 camera.
Although I have corrected the camera_info output to include the baseline as part of the Projection matrix, the initialization of the rectified image intrinsics fails, see: IntelRealSense/realsense-ros#1242

The code appears to be stuck in the for loop within setOptimalOutputCameraParameters since the resolution never converges and just gets bigger and bigger. When it finally quits due to it exceeding the number of iterations the resolution is so be that the undistortion map generation (i.e. remap within OpenCV) crashes.

I have tried to limit the number of iterations in setOptimalOutputCameraParameters to just 1, leading to a reasonable resolution. In that case the undistortion appears to work for both left and right camera, although the right image is flipped upside down?

As a side note, why is the rectification parameters recomputed each time a new image is received (as part of the image callback calling setInputCameraParameters --> generateRectificationParameters --> setOptimalOutputCameraParameters)?

catkin build fatal error: cv.h: No such file or directory #include <cv.h>

Running pkg-config --modversion opencv shows that opencv version is 3.2.0
On Ubuntu 18.04 ROS Melodic I tried to build master branch code and got those error:

.../src/image_undistort/include/image_undistort/undistorter.h:6:10: fatal error: cv.h: No such file or directory
 #include <cv.h>

and

.../src/image_undistort/include/image_undistort/undistorter.h:7:10: fatal error: highgui.h: No such file or directory
 #include <highgui.h>

To fix it I changed includes in undistorter.h from this:

#include <cv.h>
#include <highgui.h>

to this

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>

how to rectify an Omni camera model?

I have a Omni camera, I want to know how to get a rectified image?
Thanks.

Output TF not properly normalized

It seems that the tf output between input and output camera frame does not get properly normalized. When I run the image_undistortion_node, I receive the following warning:

[ WARN] (/cam0_undistorter) [1579531407.189425749]: TF to MSG: Quaternion Not Properly Normalized

Non-standard fisheye calibration for opencv fisheye model

Hi thank you so much for sharing the code.
I am trying to use Kalibr to calibrate a stereo camera with a 170fov and a 90fov lenses.

Then use the stereo_info_node to publish the camera_info which feeds into the opencv fisheye model for further stereo rectification.

I’m wondering which Kalibr model should I use and whether I need to have any setup or modifications in the stereo_info_node?

Thank you in advance.

Chang

Stereo undistort node fails on Could not find K or camera intrinsics vector

I am trying to launch undistort node on t265. I am using this calibration file:
cam_overlaps: [1]
camera_model: omni
distortion_coeffs: [1.3148301845482018, 16.728533383318446, 0.0009754402065953533,
-0.00827831490807931]
distortion_model: radtan
intrinsics: [3.5470291856480194, 1301.661733568433, 1302.5400309591962, 420.94457980247427,
403.1127614174467]
resolution: [848, 800]
rostopic: /fisheye1
cam1:
T_cn_cnm1:

• [0.999984012503518, -0.000328682180858635, 0.005645060263142, -0.06125273206848327]
• [0.000347322861611539, 0.9999944898501197, -0.003301459709764415, 4.184041033043501e-05]
• [-0.005643944027036482, 0.0033033675861735058, 0.9999786166005735, 9.419919499688744e-05]
• [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: omni
  distortion_coeffs: [1.1331594874147088, -2.2769326087777113, 0.004491591400102712,
  0.01019789982050767]
  distortion_model: radtan
  intrinsics: [2.676690389811481, 1037.3105860463947, 1035.775404430769, 424.62864780674187,
  405.2972638769736]
  resolution: [848, 800]
  rostopic: /fisheye2

Undistorting single image works. Can someone help? Thank you

Stereo_image_undistort node

When I use the stereo_image_undistort node, I find that all the parameters of the D matrix in the information displayed in /camera/raw/first/camera_info are 0. I use an external camera calibration file, but the D matrix should have parameters in the camera parameter file.In fact, the final corrected image can be displayed. I don’t understand whether this will affect the resolution of the final image, because the resolution I output becomes 6410×9264, and the resolution of the original image is 848×800.

---

/camera/raw/first/camera_info：
header:
seq: 779
stamp:
secs: 1593573471
nsecs: 668548872
frame_id: "camera_fisheye1_optical_frame"
height: 800
width: 848
distortion_model: "equidistant"
D: [0.0, 0.0, 0.0, 0.0, 0.0] #I think there is a problem
K: [278.6592825735526, 0.0, 424.14787009685375, 0.0, 278.575241659058, 395.63796110397817, 0.0, 0.0, 1.0]
R: [0.9990143337765363, 0.00362112213621329, -0.04424079998708848, -0.0036246710643880547, 0.9999934308582606, 2.7105054312137605e-20, 0.04424050936300271, 0.00016035834757857908, 0.9990208964864059]
P: [297.14913534918276, 1.0770749481661108, 411.40447584207635, 0.0, 16.49348130488335, 278.6368555084925, 395.25059058615005, 0.0, 0.04424050936300271, 0.00016035834757857908, 0.9990208964864059, 0.0]
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False

---

/camera/rect/first/camera_info:
header:
seq: 1184
stamp:
secs: 1593573485
nsecs: 168205748
frame_id: "camera_fisheye1_optical_frame"
height: 6410
width: 9264
distortion_model: "equidistant"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [283.9535322979686, 0.0, 4632.0, 0.0, 283.9535322979686, 3205.0, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [283.9535322979686, 0.0, 4632.0, 0.0, 0.0, 283.9535322979686, 3205.0, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False

---

Can someone help me?

Use kalibr intrinsic yaml,but output camera info difference

sorry,When I use kalibr intrinsic yaml ,but output camera info difference.

My kalibr intrinsic yaml distortion_model use "radtan" ,but camera info topic output "equidistant".

Can someone help me?

Undistortion and rectification flips image upside-down for ibis

@ZacharyTaylor We should take a look at this once you're back.

Unable to use output_camera_info_source in stereo_undistort_node

How to undistort stereo camera with origin input resolution?

Attempted to create undistorter from invalid camera parameters

Hi,
I calibrated my fisheye camera with the kalibr tool (omni- radtan and pinhole-equi). I recorded the image stream to a rosbag for the calibration. Now, I wanted to call this package to undistort the image stream. For that, I played the recorded data from the same rosbag as used for the calibration and loaded the yaml file from kalibr. But I always get this error message:
Attempted to create undistorter from invalid camera parameters

Do you know what the problem could be?

Thanks!
Niji

Time-varying T_cn_cnm1

Hi @ZacharyTaylor,
Do you see a good way to feed in time-varying T_cn_cnm1s, i.e. T_cn_cnm1 is different for every stereo image-pair. Thanks!

Add support for standard ros / rovio calibration formats

Would be useful, or probably adding the python script from Kalibr which does the conversion would do?

Undistortion for MonoCam with omni-radtan extrem slow

Hallo guys,
I've been trying to use Kalibr to calibrate my monocular camera with different camera models (also trying to figure the difference.)

If I deploy the undistortion with pinhole-equidistant or pinhole-radtan, I can get the images processed almost in same frequency as the recorded bag (30Hz, rostopic hz ~30Hz). But If I am using omni-radtan, the undistort process gets extrem slow, only 5 - 8 Hz could be archived.
My Laptop has i9-9900K...
Is this situation normal? Or I've made mistakes?
Could you help me?

Many Thanks!

Best regards from Aachen
Haoming

Error When use Stereo_info_node to show the camera info

When using Stereo_info_node to display the corrected image information, I encountered the following error：

SUMMARY

PARAMETERS

• /camera/stereo_info/cam0/cam_overlaps: [1]
• /camera/stereo_info/cam0/camera_model: pinhole
• /camera/stereo_info/cam0/distortion_coeffs: [0.07307789354996...
• /camera/stereo_info/cam0/distortion_model: equidistant
• /camera/stereo_info/cam0/intrinsics: [264.788182513580...
• /camera/stereo_info/cam0/resolution: [848, 800]
• /camera/stereo_info/cam0/rostopic: /camera/fisheye1/...
• /camera/stereo_info/cam1/T_cn_cnm1: [[0.9999954918913...
• /camera/stereo_info/cam1/cam_overlaps: [0]
• /camera/stereo_info/cam1/camera_model: pinhole
• /camera/stereo_info/cam1/distortion_coeffs: [0.08072112677213...
• /camera/stereo_info/cam1/distortion_model: equidistant
• /camera/stereo_info/cam1/intrinsics: [258.992611823673...
• /camera/stereo_info/cam1/resolution: [848, 800]
• /camera/stereo_info/cam1/rostopic: /camera/fisheye2/...
• /camera/stereo_info/first_camera_namespace: fisheye1
• /camera/stereo_info/input_camera_info_from_ros_params: True
• /camera/stereo_info/process_every_nth_frame: 2
• /camera/stereo_info/scale: 1.0
• /camera/stereo_info/second_camera_namespace: fisheye2
• /rosdistro: kinetic
• /rosversion: 1.12.14

NODES
/camera/
stereo_info (image_undistort/stereo_info_node)

ROS_MASTER_URI=http://localhost:11311

process[camera/stereo_info-1]: started with pid [14432]
[ERROR] [1592563683.939323941]: Could not find K or camera intrinsics vector
[FATAL] [1592563683.939372569]: Loading of input camera parameters failed, exiting
[camera/stereo_info-1] process has died [pid 14432, exit code 1, cmd /home/ubuntu/catkin_ws/devel/lib/image_undistort/stereo_info_node raw/first/image:=fisheye1/image_raw raw/second/image:=fisheye2/image_raw __name:=stereo_info __log:=/home/ubuntu/.ros/log/4978f2ce-b21a-11ea-88e8-34de1af975b3/camera-stereo_info-1.log].
log file: /home/ubuntu/.ros/log/4978f2ce-b21a-11ea-88e8-34de1af975b3/camera-stereo_info-1*.log

Camera internal and external parameters use camera information after calibration by Kalibr tool：

cam0:
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [0.07307789354996369, -0.01624786695454266, 0.0054793688458973306,
-0.00729763759178143]
distortion_model: equidistant
intrinsics: [264.78818251358086, 264.9396383111461, 426.66357941709606, 394.6197469244117]
resolution: [848, 800]
rostopic: /camera/fisheye1/image_raw
cam1:
T_cn_cnm1:

• [0.999995491891387, -0.0018850276292362385, 0.002337277848263454, -0.6313283639924419]
• [0.0019005042088560693, 0.9999761676708998, -0.006637181176720762, -0.01075742393483845]
• [-0.0023247108755902104, 0.0066415932619750245, 0.9999752421726688, -0.01767752105719401]
• [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [0.080721126772137, -0.007771852365492558, -0.005479495403646759,
  -0.0033310908906594118]
  distortion_model: equidistant
  intrinsics: [258.99261182367377, 259.60787747453577, 418.3763242563783, 401.9743401851195]
  resolution: [848, 800]
  rostopic: /camera/fisheye2/image_raw

Does anyone know how to solve it?

Recompiling with different compiler flags requires a restart to ros before disparity images can be generated

TUM datasets not rectified correctly

First of all thank you for publishing this package. In my lab we have been using the stereo_undistort_node in our pipeline for rectifying stereo-pairs calibrated using Kalibr and it has been of great help.

Recently we came across the TUM dataset:
https://vision.in.tum.de/data/datasets/visual-inertial-dataset
And we decided to run some experiments on the dataset. We found our pipeline performing poorly on the TUM dataset and further debugging has narrowed the problem down to rectified images failing the visual inspection test (there is a y-offset between left/right image features; also lines in the rectified right image are not perfectly straight).

Here is an example rectification result for a TUM data:

Here are the corresponding raw images:

The above rectification was generated using the following raw bag:
http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-corridor4_512_16.bag
and the following calibration file:
https://vision.in.tum.de/_media/data/datasets/visual-inertial-dataset/pinhole-equi-512.zip

I have tried different scenes from the TUM dataset with similar rectification issues. I have also tried generating my own Kalibr file using the calibration bag they provide and there is no improvement in the rectification quality (though Kalbr reports sub pixel accuracy in calibrating the dataset). I used the following bag to generate my Kalibr file:
http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-calib-cam4_512_16.bag

There are two possibilities I can think of that might cause this rectification issue:

1. TUM dataset published wrong calibration data that goes along with their dataset
2. There is a bug in the stereo_undistort_node that is exposed by this dataset.

I was hoping someone here (@ZacharyTaylor) might be willing to try stereo_undistort_node's performance on the TUM dataset. If there is a bug in the stereo_undistort_node you folks would be best at debugging it. Thanks.

stereo_undistort_node: rectification fails to generate valid P matrices

Thank you for releasing this useful package. I am seeing some unexpected behavior in stereo_undistort_node, which might be due to improper usage by me or possibly a bug in the code. I am seeing P matrices generated by stereo_undistort_ node with non-zero y and z offsets.

Here is a reproduction test that should help reproduce the issue at your end.
Usage: roslaunch image_undistort rect_test.launch

The folder on google drive contains:

• static.bag: the example dataset that comes with Kailbr
• camchain-static.yaml: generated using a fresh build of Kailbr/master; I have also tried Kalibr-CDE
• rect_test.lauch: a launch file that produces the following error at my end:
  [ERROR] [1538875392.391423846]: Image P matrices must match (excluding x offset)
[ERROR] [1538875392.391476697]: Camera info processing failed, skipping disparity generation

When I observe the rectified images using the following command they appear to be flipped in y-axis: rqt_image_view /test/rect/first/image

Any pointers on debugging this issue would be really helpful. Thanks.

catkin build issues

I cannot catkin build this package. Can anyone help me?

Explicit License Information

Dear all,

the licensing information about your fantastic library/ROS package is not very explicit. The package.xml points to BSD, but it is not clear which one. It would be great if this was made clearer. Ideally:

1. Use a license identifier from https://spdx.org/licenses/ to point to the exact license.
2. Add a LICENSE file as in the kalibr repo: https://github.com/ethz-asl/kalibr/blob/master/LICENSE

We would very much appreciate if you could help us out and clear up these details.

Best regards,
Max

List package's dependency

Hi @ZacharyTaylor ,

for people for have never used this node it might be helpful to know where to download dependencies (for now only "nlopt"). I imagine some people at RSL would spend some time to understand which "nlopt" package they should download. After ARCHE I can add a brief section in the readme file with this info, if you agree.

Cheers,
Marco.

scale parameter reduces speed considerably

I tried setting scale > 2 for some wide view cameras (from TUM VI dataset) and it gets really slow.

Bug in rectification parameters generation leading to wrong P

Hi there,

I have spotted a bug in StereoCameraParameters::generateRectificationParameters() leading to wrong rectification of images (at least with the euroc_camchain config file taken from voxblox).

Starting l.655 in src/camera_parameters.cpp,

  first_.setInputCameraParameters(
      first_.getInputPtr()->resolution(),
      T.inverse() * first_.getInputPtr()->T(), first_.getInputPtr()->K(),
      first_.getInputPtr()->D(), first_.getInputPtr()->distortionModel());
  second_.setInputCameraParameters(
      second_.getInputPtr()->resolution(),
      T.inverse() * second_.getInputPtr()->T(), second_.getInputPtr()->K(),
      second_.getInputPtr()->D(), second_.getInputPtr()->distortionModel());

here T is a 4x4 homogeneous matrix defined as Eigen::Matrix4d whose top-left 3x3 block represents an SO(3) rotation matrix. The inverse of T must therefore be computed as the inverse of an SO(3) matrix :

  Eigen::Matrix4d invT;
  invT.block<3,3>(0,0) = T.block<3,3>(0,0).transpose();

  first_.setInputCameraParameters(
      first_.getInputPtr()->resolution(),
      invT * first_.getInputPtr()->T(), first_.getInputPtr()->K(),
      first_.getInputPtr()->D(), first_.getInputPtr()->distortionModel());
  second_.setInputCameraParameters(
      second_.getInputPtr()->resolution(),
      invT * second_.getInputPtr()->T(), second_.getInputPtr()->K(),
      second_.getInputPtr()->D(), second_.getInputPtr()->distortionModel());

Without this fix, the resulting P's matrices are wrong and create flipped images with non aligned along the y axis.

I am running ubuntu 18.04 (ROS melodic) and encountered this issue with both Eigen 3.3.4 and Eigen 3.3.9.

ROS2 port

Hi,
Is there any plan (fork) to port this package to ROS2?
Thanks in advance,
Rui