added inverse brown conrady

[ste93]

Added inverse brown conrady distortion since the realsense image has this distortion

[traversaro]

Hi @ste93, can you explain why both RS2_DISTORTION_BROWN_CONRADY and RS2_DISTORTION_INVERSE_BROWN_CONRADY are mapped to the same value YARP_PLUM_BOB? Thanks! cc @prashanthr05

[ste93]

i've checked both ros https://github.com/IntelRealSense/realsense-ros/blob/be1bb6cd471449af808dff0e62faad94b058757a/realsense2_camera/src/base_realsense_node.cpp#L1983 and ros2 https://github.com/intel/ros2_intel_realsense/blob/99bbe9197983c31df42ed52cfe6b84b7c41fdb94/realsense_ros2_camera/src/realsense_camera_node.cpp#L756 devices and they map all the distortion to plumb_bob, except only for ros1 that has another distortion not considered by us

[traversaro]

i've checked both ros https://github.com/IntelRealSense/realsense-ros/blob/be1bb6cd471449af808dff0e62faad94b058757a/realsense2_camera/src/base_realsense_node.cpp#L1983 and ros2 https://github.com/intel/ros2_intel_realsense/blob/99bbe9197983c31df42ed52cfe6b84b7c41fdb94/realsense_ros2_camera/src/realsense_camera_node.cpp#L756 devices and they map all the distortion to plumb_bob, except only for ros1 that has another distortion not considered by us

Do you think that this behavior in the ROS driver make sense? As far as I can read from https://intelrealsense.github.io/librealsense/doxygen/rs__types_8h.html#a96a8444bedb23db02c92712e0a0c37ae, it seems that the two distortion models are one the inverse of the other, so mapping both to the same distortion model (without any additional metadata to distinguish the two) seems a bug in the ROS driver (or at least an artifact coming from not supporting RS2_DISTORTION_INVERSE_BROWN_CONRADY in the ROS driver).

Perhaps you can explain a bit what is the goal (problem to solve or enhancement to obtain) of this change so we can find another solution?

[ste93]

The problem was that the realsense returns to us the inverse brown conrady and since the device doesn't supports it, the unsupported model doesn't enable the wrapper to publish the data of the camera_info on ros, and rviz doesn't allow to see the images without those data. I don't know if the distortion is useful since we have already undistorted data as @Nicogene said, I think that if the image is already undistorted maybe it can be mapped to undistorted, but I will check deeply.

[traversaro]

The problem was that the realsense returns to us the inverse brown conrady and since the device doesn't supports it, the unsupported model doesn't enable the wrapper to publish the data of the camera_info on ros, and rviz doesn't allow to see the images without those data.

I may be wrong, but according to REP-104 ( https://www.ros.org/reps/rep-0104.html#calibration-parameters ) an unsupported distortion model, so anything that is not plumb_bob or rational_polynomial in both ROS1 and ROS2 or equidistant only in ROS1 ( https://github.com/ros2/common_interfaces/blob/975f1ce52645a9eabe27a222ceb43bbbba268956/sensor_msgs/include/sensor_msgs/distortion_models.hpp and https://github.com/ros/common_msgs/blob/noetic-devel/sensor_msgs/include/sensor_msgs/distortion_models.h ) should be represented with an empty string. In the YARP ROS device wrapper, are we mapping YarpDistortion::YARP_UNSUPPORTED to an empty string? Is this giving problem in RViz?

If we need a RViz-specific workaround to render in any case YarpDistortion::YARP_UNSUPPORTED distortion, probably it could make more sense to put it in the ROS network server rather then in all the related YARP drivers?

[ste93]

I can check, maybe is a good option to manage it in the wrapper. Actually I have this problem in rviz but I don't know if it can raise other problems.

[traversaro]

@Nicogene by the way, do you know where the semantics of the YARP_PLUM_BOB distortion model is defined? I had a possible idea that could help here, but I do not know how YARP_PLUM_BOB is defined.

[ste93]

I think it is in ./src/libYARP_sig/src/yarp/sig/IntrinsicParams.cpp and ./src/libYARP_sig/src/yarp/sig/IntrinsicParams.h

[traversaro]

I think it is in ./src/libYARP_sig/src/yarp/sig/IntrinsicParams.cpp and ./src/libYARP_sig/src/yarp/sig/IntrinsicParams.h

Thanks @ste93! However, while I can find the C++ definition of the YarpDistortion::YARP_PLUM_BOB, I can't find its semantics (i.e. its meaning) documented there.

[ste93]

following this link https://calib.io/blogs/knowledge-base/camera-models it is associated exactly to the brown_conrady, so maybe you were right that is an error in ROS. Also the model was first developed by Brown and Conrady, and I think the name given later was plumb_bob express the same model.

[ste93]

I found from here https://dev.intelrealsense.com/docs/projection-texture-mapping-and-occlusion-with-intel-realsense-depth-cameras that all the D400 cameras are returning undistorted images, and the plumb bob is used without distortion (i've checked with ros, all the 5 distortion parameters are 0). I think this is a model to return the undistorted image but to support also other cameras (like maybe non d400 serie).

[traversaro]

Cool, by the way that document contains a nice description of the semantics of the distortion models coming from an official source: https://dev.intelrealsense.com/docs/projection-texture-mapping-and-occlusion-with-intel-realsense-depth-cameras#section-6-appendix-1-distortion-models, something that we could incorporate or reference in YARP. In any case, if the use of distortion model is actually not really relevant in this case as the actual images are undistorted, I am ok in not using YARP_UNSUPPORTED, but we should add a check for the distortion parameters to be actually 0.

[ste93]

I've checked at this point https://github.com/robotology/yarp-device-realsense2/blob/0e63a2ca4f9713749e1814ebc080d19607d3c4c8/src/devices/realsense2/realsense2Driver.cpp#L385 and the parameters are all 0.

[traversaro]

I've checked at this point

https://github.com/robotology/yarp-device-realsense2/blob/0e63a2ca4f9713749e1814ebc080d19607d3c4c8/src/devices/realsense2/realsense2Driver.cpp#L385 and the parameters are all 0.

They are all 0 in the configuration you are using, but they may not be zero in different configuration, with "we should add a check" I meant at runtime, that checks the parameters and if they are zero set the distortion to the usual value (as in any case it is not relevant as all the parameter are zero) otherwise set it to unsupported.

[traversaro]

We can also talk on this on Teams if you prefer.

[ste93]

updated the changes requested, the problem will be discussed in another pr to change YARP_PLUM_BOB to YARP_DISTORTION_NONE

[traversaro]

@Nicogene what do you think about this?

[ste93]

Tested today. With d435 works well since it has all parameters to 0, while d455 has an inverse brown conrady with parameters different from zero. So this solution works for d435 cameras, while I think for d455 must be implemented the inverse brown conrady in yarp. However I think that this is a good solution for now to work with d435 until we don't have the inverse brown conrady implemented.

[traversaro]

Probably we need to sort out who the mantainer of the repo is so we know who can merge the PR. @drdanz @randaz81 any idea?

[ste93]

I've tested now with d455 camera, if i return the image with plumb bob distortion instead of inverse brown conrady it is still working and seems not to affect the image. We are testing it on CER04 to see if it will work or not.

[traversaro]

Do you agree on merging @ste93 ?

[ste93]

@traversaro yes I agree on merging

[traversaro]

Thanks!