Realsense-ros: depth_registered pointcloud has incorrect depth scaling

[r0b0cop]

If I run roslaunch realsense2_camera rs_rgbd.launch and examine the pointcloud published on /camera/depth_registered/points in RViz, the points appear to be projected incorrectly. For example, a .3m long object in the scene is rescaled to around 2.2m (as indicated by rviz's measure tool). Changing the fixed frame does not solve this.

Running roslaunch realsense2_camera demo_pointcloud.launch works however. Objects published to the topic /camera/depth/color/points are correctly shown as .3m long. Unfortunately, this topic doesn't align points with the color frame.

The problem could be related to the _depth_scale_meters parameter used in base_realsense_node.cpp, but I haven't been able to find any documentation on that parameter. Am I misunderstanding something about the registered depth cloud, or any suggestions on how to fix this?

Environment info:
SR300 with firmware 3.21.0.0
ROS kinetic

[oka1125]

By solution #350, I found another problem has occurred:

[problem]
- Striped pattern occurs in the published aligned depth image.
- Jagged pattern occurs in the published point cloud 2.

I think that "depth_units / meter_to_mm (decimal value)" multiplied to each (upper/lower) byte is not good,
because decimal point values will be disappeared after calculation in each byte.

So I propose new solution like following:

                        int out_pixel_index = y * other_intrin.width + x;
+                       uint16_t value = depth / meter_to_mm;
                        //Tranfer n-bit pixel to n-bit pixel
                        for (int i = 0; i < from_bytes_per_pixel; i++)
                        {
                            const auto out_offset = out_pixel_index * output_image_bytes_per_pixel + i;
-                           const auto from_offset = from_pixel_index * output_image_bytes_per_pixel + i;
-                           p_out_frame[out_offset] = p_from_frame[from_offset] * (depth_units / meter_to_mm);
+                           p_out_frame[out_offset] = (value >> (8 * i)) & 0x00FF;
                        }

[oka1125]

I noticed that the same problem has been already written in the following topic:
"fix the aligned depth frame unit conversion issue #367 "

[jcgarciaca]

I am facing the same issue, do you have any idea how to solve it?
Just for reference, in my case the scale difference is for XYZ.
Thank you.

[oka1125]

In the latest version (https://github.com/intel-ros/realsense/tree/2.0.4), this issue seems to be solved.

https://github.com/intel-ros/realsense/pull/367

[oka1125]

Are you facing this issue in the latest version (git tag = 2.0.4) ?

[weihaosky]

It is solved in 2.0.4 but appears again in 2.1.1

[oka1125]

I also confirmed that this problem appears again in 2.1.1.
Source code in 2.1.1 has seemed to be changed from 2.0.4, and then this measure has seemed to be missing.

I created pull request to fix this problem for 2.1.4.
Please refer it if you need.

[yayaneath]

I can confirm that PR #605 fixes the problem for me.

[RealSenseCustomerSupport]

[Please Ignore - RealSense system comment]

[doronhi]

fixed in PR#682 that incorporates PR#605