Added the RangeFinder class to represent a depth camera

[astumpf]

Add the rangefinder class that reads in a depth camera node from URDF and translates it as a rangefinder node into webots proto.

[BenjaminHug8]

This PR should also target develop instead of master.

[astumpf]

Still have some minor issues with the rangefinder, which I hope you can figure out quickly.

First, the resulting proto:

RangeFinder {
        name "d455_depth_camera"
        fieldOfView 1.518436
        width 1280
        height 720
        near 0.520000
        minRange 0.520000
        maxRange 4.000000
      }

Here's the result in RViz:

There are some projection and warping issues. The RGBD image in RViz (greyscaled, top left) is identical to the depth image displayed by Webots itself (see below). Therefore, I would argue that this issue happens already in Webots.

[astumpf]

Still have some minor issues with the rangefinder, which I hope you can figure out quickly. ...

Solved the transformation issue. The problem was the missing exports of the optical frames in our URDF, so this mistake was on my side. But it requires still another bugfix (see #164).

However, warping remains a serious issue:

[BenjaminHug8]

I will take a look at this.

[BenjaminHug8]

May I ask you your URDF used to generate this PROTO RangeFinder?

[astumpf]

May I ask you your URDF used to generate this PROTO RangeFinder?

I will send you some files the next days.

[BenjaminHug8]

After some tests and according with the documentation on the RangeFinder, I think the near value is too high in your case. This value should be just big enough to exclude any geometry of the RangeFinder from the frustum.

Could you try to reduce it and report here the effect?

[astumpf]

After some tests and according with the documentation on the RangeFinder, I think the near value is too high in your case. This value should be just big enough to exclude any geometry of the RangeFinder from the frustum.

Could you try to reduce it and report here the effect?

Set near (and in a second run additionally minRange) to 0.01. Both tests result in the following:

Curvature is removed, but plane is tilted. I'm just wondering that at max range definite values are reported. According to your documentation +inf should be returned for those pixels, which also matches the ROS REP117. Therefore, we should not be even able to "see" the far plane, which is btw. not tilted.

[BenjaminHug8]

Curvature is removed,

Nice!

but plane is tilted.

I will investigate that.

+inf should be returned for those pixels

That's something we recently fixed. Are you using Webots R2022a or a nightly build?

[astumpf]

Curvature is removed,

Nice!

but plane is tilted.

I will investigate that.

+inf should be returned for those pixels

That's something we recently fixed. Are you using Webots R2022a or a nightly build?

Webots R2022a

[BenjaminHug8]

Webots R2022a

Ok then trying one of the latest nightly build should resolve the max range values.

I will get back to you once I have understand the tilt of the plane.

[BenjaminHug8]

I made a quick test with the rangeFinder and the result shows a bending for horizontal planes (see underneath, the wall on the right is not the max range but a physical wall). It seems to be affected by the near plane and is probably due to some mapping of the frustum of the camera, I am currently working on it.

The range finder used is the one you gave but with a near plane of 0.1:

RangeFinder {
        name "d455_depth_camera"
        fieldOfView 1.518436
        width 1280
        height 720
        near 0.1
        minRange 0.1
        maxRange 4.000000
      }

But otherwise I am not able to reproduce the tilt of the plane.

[astumpf]

I made a quick test with the rangeFinder and the result shows a bending for horizontal planes (see underneath, the wall on the right is not the max range but a physical wall). It seems to be affected by the near plane and is probably due to some mapping of the frustum of the camera, I am currently working on it.

The range finder used is the one you gave but with a near plane of 0.1:

RangeFinder {
        name "d455_depth_camera"
        fieldOfView 1.518436
        width 1280
        height 720
        near 0.1
        minRange 0.1
        maxRange 4.000000
      }

But otherwise I am not able to reproduce the tilt of the plane.

It seems your model has also some tilted coordinate systems. Are you sure you are not rendering the depth image from one of these that could accidently compensate the tilt of the plane? However, I've send you our proto file a few days ago.

[BenjaminHug8]

It seems your model has also some tilted coordinate systems. Are you sure you are not rendering the depth image from one of these that could accidently compensate the tilt of the plane?

I can check this.

However, I've send you our proto file a few days ago.

I ll assume that I just have to replace the lidar by a rangefinder in the PROTO file you sent.

[astumpf]

I ll assume that I just have to replace the lidar by a rangefinder in the PROTO file you sent.

The proto is ready to run out of the box :-). No changes required.

[BenjaminHug8]

The proto is ready to run out of the box :-). No changes required.

I am a bit confused, we are talking about rangeFinder in this PR and I received a Lidar.box.proto with a lidar device in it.

Were you talking about a lidar all this time or did I not have the correct file? (The visual of the PROTO is the same, but I just surprised to found a lidar and not a rangeFinder, I will test with the lidar until you see this message).

[astumpf]

The proto is ready to run out of the box :-). No changes required.

I am a bit confused, we are talking about rangeFinder in this PR and I received a Lidar.box.proto with a lidar device in it.

Were you talking about a lidar all this time or did I not have the correct file? (The visual of the PROTO is the same, but I just surprised to found a lidar and not a rangeFinder, I will test with the lidar until you see this message).

I'm sorry for any confusion. This proto is indeed the "Lidarbox" model I'm using for the tests above. It includes both, Lidar and Rangefinder. Lidar works already fine for us.

This thing actually exists and contains a heterogeneous sensor suite.

[BenjaminHug8]

I'm sorry for any confusion. This proto is indeed the "Lidarbox" model I'm using for the tests above. It includes both, Lidar and Rangefinder. Lidar works already fine for us.

Ah ok, I understand better now. I made a quick test with the rangeFinder and I do not see a tilt of the plane.

However if I rotate the device in Webots it appears that the visualization in RViz is not simply rotated, e.g. if I have a horizontal plane with a vertical wall, after a rotation in of the device in Webots I can see (of course) a rotation in Rviz but the plane and the wall are no longer perpendicular and I almost get a horizontal plane tilted but the wall is still vertical.

So I am wondering if you publish your tf tree from your URDF or if you let the webots_ros2_driver do it for you (here is an example on how to set this option)? If you use your own URDF I suspect the converter tool to maybe not correctly handle rotations.

[BenjaminHug8]

I made a quick test with the rangeFinder and the result shows a bending for horizontal planes (see underneath, the wall on the right is not the max range but a physical wall). It seems to be affected by the near plane and is probably due to some mapping of the frustum of the camera, I am currently working on it.

This effect has been fixed in https://github.com/cyberbotics/webots/pull/4309.

[ad-daniel]

The requirements for this PR appear to have been done, is there anything else preventing the merge of this PR?

[omichel]

It seems there is still one unresolved conversation. Not sure if it was addressed.

[astumpf]

To give this topic new input. I'm currently experimenting with the latest changes introduced with the 2022b release.

This issue here unfortunately remains. Here is a new example when using a range finder:

RangeFinder {
  name "vision_d455_depth_camera"
  fieldOfView 1.518436
  width 1280
  height 720
  near 0.520000
  minRange 0.520000
  maxRange 4.000000
}

Webots world:

Produced depth image (converted to point cloud using depth_image_proc, but the rangefinder image is confirmed to contain the same data):

Now, a big concern which is hidden in this example: The color image is perfectly projected into the point cloud. This is an indication that the color image may be heavily warped as well. This would explain the strange behavior that AR marker detectors (e.g. Aruco) have a hard time to detect anything in camera images rendered by Webots.

[astumpf]

Nevermind, this projection issue was on our side due to a wrongly published camera info. After sorting out this issue, everything works perfectly. This PR is ready to merge (finally!) :+1: