Add proper output of equidistant distortion model.

[helenol]

(Excuse the whitespace changes, my editor automatically strips whitespace at the end of lines).

This changes the output distortion model for the T265 from plumb_bob (which it is not) to equidistant (which it is).
Kannala-Brandt is frequently referred to as equidistant distortion.
Paper: http://www.ee.oulu.fi/~jkannala/publications/tpami2006.pdf
Source for being referred to as equidistant in kalibr: ethz-asl/kalibr#17

Fixing the label of the output to the ROS-supported equidistant model allows simple undistortion of the T265 images using packages like image_undistort (image_proc which is part of ROS only supports radtan/plumb_bob).
Example undistorted image:

Example launch file for undistortion: https://github.com/ethz-asl/image_undistort/blob/master/launch/undistort_t265.launch

[wang-yu-yang]

Thank you very much for your code. I encountered the following problem while using the lanch file you gave. I don’t quite understand how to solve it. Can you help me?

---

Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://localhost:35987/

SUMMARY

CLEAR PARAMETERS

• /camera/image_undistort_node/

PARAMETERS

• /camera/image_undistort_node/output_camera_info_source: match_input
• /camera/image_undistort_node/scale: 1.0
• /rosdistro: kinetic
• /rosversion: 1.12.14

NODES
/camera/
image_undistort_node (image_undistort/image_undistort_node)

ROS_MASTER_URI=http://localhost:11311

process[camera/image_undistort_node-1]: started with pid [23504]
[ INFO] [1591674481.318116695]: Initializing nodelet with 4 worker threads.
[ INFO] [1591674481.320355621]: Started /camera/image_undistort_node nodelet.
[ERROR] [1591674481.352851622]: Cannot set output to same values as input, as input is not currently set

---

[helenol]

Hi @wang-yu-yang , thanks for trying it! The error is simply because for the first frame received, it does not have yet the distortion parameters as it gets them from ROS. It can be safely ignored.

You should get the undistorted image if you run:
rqt_image_view
and select /camera/fisheye1_rect/image_raw as in the example above.

Hope that helps!

[wang-yu-yang]

t265.txt

Thanks for your help, I completed the image correction of a single camera, but for realsense T265, I need to synchronously correct both cameras. The launch file I completed is as follows: Is this correct?

[helenol]

You can look into doing stereo_undistort: https://github.com/ethz-asl/image_undistort#stereo_undistort_node which will do both together in one node. If you need both unrectified for stereo, then you can also just do stereo directly: https://github.com/ethz-asl/image_undistort#dense_stereo_node

[wang-yu-yang]

I'm sorry to bother you again, I used the Stereo_image_undistrot node for testing today, but I ran into trouble, the error is as follows: I will give my launch file and T265 camera calibration file
camera_calibration.txt
my_stereo_undistort.txt

Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://localhost:41233/

SUMMARY

PARAMETERS

• /camera/stereo_undistort/first_camera_namespace: fisheye1
• /camera/stereo_undistort/input_camera_info_from_ros_params: False
• /camera/stereo_undistort/process_every_nth_frame: 2
• /camera/stereo_undistort/scale: 1.0
• /camera/stereo_undistort/second_camera_namespace: fisheye2
• /rosdistro: kinetic
• /rosversion: 1.12.14

NODES
/camera/
stereo_undistort (image_undistort/stereo_undistort_node)

ROS_MASTER_URI=http://localhost:11311

process[camera/stereo_undistort-1]: started with pid [32212]
[ERROR] [1591787456.715495586]: Stereo rectification cannot be performed on cameras with a baseline of zero
[ERROR] [1591787456.715546841]: Camera parameters invalid, undistortion failed
[ERROR] [1591787456.750929470]: Stereo rectification cannot be performed on cameras with a baseline of zero
[ERROR] [1591787456.750975209]: Stereo rectification cannot be performed on cameras with a baseline of zero
[ERROR] [1591787456.751003439]: Camera parameters invalid, undistortion failed
[ERROR] [1591787456.781296645]: Stereo rectification cannot be performed on cameras with a baseline of zero

[mindThomas]

@wang-yu-yang The problem is that the realsense node doesn't expose the stereo extrinsincs (including the baseline that the stereo rectification node is complaining about).
This would have to be included in the projection matrix, P.
I have tried to address that in my PR here: #1242
But I am still struggling with getting it to undistort correctly with the stereo_undistort_node. Currently my issue is that it flips the right image upside down.

[crankler]

@helenol @wang-yu-yang
thank you for your answer, but when I just input:

roslaunch realsense2_camera rs_t265.launch
roslaunch image_undistort undistort_t265.launch

there show these msg:

[ERROR] [1592193058.165513488]: Cannot set output to same values as input, as input is not currently set
[ WARN] [1592193060.213132061]: TF to MSG: Quaternion Not Properly Normalized

then I open the rqt and only check out /camera/fisheye1_rect/image,
but the picture can't be undistort, it's also a fisheye picture
can you tell me how to rectify? thank you!

[helenol]

@crankler did you check out this branch/merge these changes?
Because this PR isn't merged yet (no one has approved it) and without the changes in it, it doesn't work. ;)

[crankler]

ok, i've get the goog result, thank you for your help!

[doronhi]

Hi @helenol
I checked the PR using the example your suggested and it works as expected.
Personally, I have no doubts.
However, since the testing package you suggested, image_undistort, seems related to the same organization as the one suggesting the modification, I wish to also try it on a 3rd party ROS package. Having no experience in that, I chose the package image_proc (sudo apt-get install ros-kinetic-image-proc)

After running
roslaunch realsense2_camera rs_t265.launch
I tried testing it from a different terminal with:
ROS_NAMESPACE=camera/fisheye1 rosrun image_proc image_proc
However, running:
rosrun image_view image_view image:=/camera/fisheye1/image_rect
Made it crash with the following exception:

terminate called after throwing an instance of 'image_geometry::Exception'
  what():  Cannot call rectifyImage when distortion is unknown.

Can you help me figure out why image_proc crash? Can it means there is something else wrong with the distortions realsense2_camera provides? It is possible, of course, that there is something wrong with "image_proc" itself. Can you then suggest another 3rd party package I can test your modifications with?
(Without your PR image_proc does nothing, so it is another indication that the PR is important. I wish, however, to have a working unrelated example before we go on)

[helenol]

Hi @doronhi! Thanks for checking the code & how it runs. Unfortunately image_proc does not support equidistant distortion/undistortion (edit: supported in OpenCV in fisheye module: https://docs.opencv.org/3.4/db/d58/group__calib3d__fisheye.html ), even though it's supported in the sensor_msgs format. Here's the undisortion code from image_proc/image_geometry:
https://github.com/ros-perception/vision_opencv/blob/noetic/image_geometry/src/pinhole_camera_model.cpp#L134-L148

Which is why we unfortunately have to use a third-party package for the undistortion.

[doronhi]

I restored the redundant spaces to make it easier to follow the change.
Thanks.

[mindThomas]

@helenol

as it's not natively supported in opencv

What is this claim based on? I have had had no issues undistorting the T265 images and even stereo rectifying them with the OpenCV Fisheye module: https://docs.opencv.org/3.4/db/d58/group__calib3d__fisheye.html

cv::Mat lm1, lm2;
cv::fisheye::initUndistortRectifyMap(K_left, D_left, R_left, P_left, rectified_size, CV_32FC1, lm1, lm2);

cv::Mat rm1, rm2;
cv::fisheye::initUndistortRectifyMap(K_right, D_right, R_right, P_right, rectified_size, CV_32FC1, rm1, rm2);

cv::remap(left, left_undistorted, lm1, lm2, cv::INTER_LINEAR);
cv::remap(right, right_undistorted, rm1, rm2, cv::INTER_LINEAR);

[helenol]

@doronhi Thank you! Glad to have this PR in.

@mindThomas Thanks! I wasn't aware. I knew about the fisheye module but didn't realize it was equidistant distortion that was implemented there. Thanks a lot for this, it's very helpful! Nonetheless, image_proc in ROS doesn't support this model. :)