Support the RealSense D405

[xEnVrE]

At the moment the yarp-device-realsense2 does not support the short-range RealSense D405 camera.

The main reason is that this camera only exposes a depth sensor, i.e. in the following for cycle

yarp-device-realsense2/src/devices/realsense2/realsense2Driver.cpp

Lines 599 to 612 in d4da73f

	for (auto & m_sensor : m_sensors)
	{
	if (m_sensor.is<rs2::depth_sensor>())
	{
	m_depth_sensor = &m_sensor;
	if (!getOption(RS2_OPTION_DEPTH_UNITS, m_depth_sensor, m_scale))
	{
	yCError(REALSENSE2) << "Failed to retrieve scale";
	return false;
	}
	}
	else if (m_sensor.get_stream_profiles()[0].stream_type() == RS2_STREAM_COLOR)
	m_color_sensor = &m_sensor;
	}

only the depth sensor pointer m_depth_sensor gets initialized (while for the D415, D435 and D455 also the m_color_sensor gets initialized).

Despite the fact that it only has a depth sensor, in terms of the internal Intel device enumeration, Intel still provides a proper RGB stream - i.e. it is fully supported for this camera.

At the moment, the driver calls the following method eventually:

yarp-device-realsense2/src/devices/realsense2/realsense2Driver.cpp

Line 638 in d4da73f

bool realsense2Driver::setParams()

which fails at this line

yarp-device-realsense2/src/devices/realsense2/realsense2Driver.cpp

Line 757 in d4da73f

if (! setRgbResolution(p1.asInt32(), p2.asInt32()))

because

yarp-device-realsense2/src/devices/realsense2/realsense2Driver.cpp

Lines 865 to 868 in d4da73f

	bool realsense2Driver::setRgbResolution(int width, int height)
	{
	bool fail = true;
	if (m_color_sensor && isSupportedFormat(*m_color_sensor, width, height, m_fps, m_verbose)) {

m_color_sensor is not initialized.

Nonetheless, the code is made in a way such that, if we could make the initialization of the RGB paramters part optional, the initialization completes successfully (as there is no need for setting the resolution parameter for the RGB device, as it does not exist!).

I tried commenting the part - that we should make optional - and I got this:

While the RGB part is ok, the depth is clearly wrong as we get invalid values for small box on the table.

I know that it is wrong also because I have a tiny C++ executable that I use for testing RealSense cameras which produce this totally different result:

which makes more sense as the D405 can produce reasonable depth maps for object as near as 7 cm (although I am not sure about the color being so different!).

We should better investigate what the code does within the YARP device and understand if we are making any configuration that is maybe not compatible with the D405.

cc @Arya07 @fedeceola @randaz81 @Nicogene

[xEnVrE]

After further investigation, I discovered that:

• the D405 uses a different value for scaling the raw depth values to the actual ones (in meters) than that used in D415/D435 cameras
• the standalone C++ executable used for testing was using the wrong scale value (0.001 while it should be 0.0001)

After this change, the output of the test executable is the same obtained using the YARP device, which seems wrong according to the output of the portmonitor.

Then might it just be that the portmonitor is clipping out too near values?

I also made a test with the same scene and compared with the realsense-viewer:

realsense-viewer

YARP device

[xEnVrE]

Then might it just be that the portmonitor is clipping out too near values?

Indeed, it is the case:

At this line https://github.com/robotology/yarp/blob/51292e3a53ebb6a1198913d41ce54d25082accb7/src/portmonitors/depthimage_to_rgb/DepthImage2.cpp#L52
one can find that the minimum value is set to 0.2 meters.

[xEnVrE]

After changing that, we now get the expected depth map in the viewer:

To recap, I think we could get the D405 up and running with the YARP device by making the initialization of the RGB parameters optional, as described in #39 (comment).

[traversaro]

To recap, I think we could get the D405 up and running with the YARP device by making the initialization of the RGB parameters optional, as described in #39 (comment).

Great, feel free to open a PR, thanks!

[xEnVrE]

This morning I had a chat f2f with @Nicogene and we think we have found a good compromise without changing the format of the configuration file at all and making few changes in the code.

I will open a PR next days.

[xEnVrE]

I opened a PR in #40.

[xEnVrE]

Last week @fedeceola made some tests with the D405 mounted on the robot. He used the software with the changes from the above PR.

He noticed that there is something wrong with the RGB/Depth alignment:

We are going to further investigate this and update the PR accordingly.

[xEnVrE]

We also checked the alignment on the images from the realsense-viewer.

This works as expected, as can be seen by superimposing the images:

[Nicogene]

Hi @xEnVrE, this happens one of the two sensor uses a resolution that is not native.

For this reason, there is the parameter alignmentFrame that should fix the problem. I don't remember if changes also the resolution.

[xEnVrE]

Hi @Nicogene, the alignment has been activated using <param name="alignmentFrame">RGB</param> My thought was that it is not working properly with the D405 camera. It might depend on the change that I made in the PR.

[Nicogene]

Hi @Nicogene, the alignment has been activated using RGB My thought was that it is not working properly with the D405 camera. It might depend on the change that I made in the PR.

Maybe it is worth to try also <param name="alignmentFrame">Depth</param>, depending on the resolution and the model the result was different

[xEnVrE]

Thanks for the suggestion @Nicogene.

Yep, we could also try to understand which mode is used inside the realsense-viewer for the D405 when the alignment is enabled.

I always assumed that we wanted to align against the RGB frame as the depth has holes which would result in an aligned RGB images with holes as well.

[xEnVrE]

I finally had time to come back to this.

It seems that the rs2::align is creating a lot of trouble at specific resolutions. See the following video where I was running the camera at 640x480:

test_rs_alignment.mp4

In practice, it seems that the streams from the camera are already aligned. Instead, enabling the alignment from the librealsense SDK causes a strange enlargement of the depth image.

I checked with other resolutions, e.g. 1280x720 and in that case enabling the alignment or not makes a very small and quite unnoticeable difference. This is the default resolution used in the rs-align executable, so the author might not have noticed the issue while testing it with the D405 camera.

Maybe the D405 does the alignment in hardware or, its nature, i.e. the fact the RGB camera is "coaxial" with the depth sensor, implies that it does not require alignment?

I might consider opening an issue on the librealsense repository or checking if there are similar ones. I found IntelRealSense/librealsense#11329 which states that RGB-depth alignment is poor, however is not saying that alignment software-side is not required.

For the time being, we might support completely disabling the alignment within this device, so that the user might configure the device properly when using a D405. What do you think @Nicogene?

cc @Nicogene @fedeceola

[xEnVrE]

I might consider opening an issue on the librealsense

Done in IntelRealSense/librealsense#11784

They stated that "The D405 camera model does not have a separate RGB sensor component and RGB is instead provided by the depth sensor. This means that RGB is already in alignment with the left IR sensor that depth originates from."

[xEnVrE]

we might support completely disabling the alignment within this device

This is already possible by using the option <alignmentFrame>None<\alignmentFrame>. I tested it with this option and now the alignment is as expected:

[Nicogene]

For the time being, we might support completely disabling the alignment within this device, so that the user might configure the device properly when using a D405. What do you think @Nicogene?

You can add a warning if alignmentFrame is defined and the sensor is D405, but I am afraid that that warning will be ignored and it is difficult to maintain all the combinations of options/models out of the one that are officially supported by the sdk.

[xEnVrE]

Given that alignmentFrame=None is already supported in this device, I would just document in the README.md how to configure cameras with special "features" like the D405. What do you think @Nicogene?

[Nicogene]

Given that alignmentFrame=None is already supported in this device, I would just document in the README.md how to configure cameras with special "features" like the D405. What do you think @Nicogene?

Good idea!