Inconsistency in different implementation of rs2_project_point_to_pixel and rs2_deproject_pixel_to_point

[oceanusxiv]

• Before opening a new issue, we wanted to provide you with some useful suggestions (Click "Preview" above for a better view):

  • Consider checking out SDK examples.
  • Have you looked in our documentations?
  • Is you question a frequently asked one?
  • Try searching our GitHub Issues (open and closed) for a similar issue.

• All users are welcomed to report bugs, ask questions, suggest or request enhancements and generally feel free to open new issue, even if they haven't followed any of the suggestions above :)

---

Required Info
Camera Model	{ D455 }
Firmware Version	(Open RealSense Viewer --> Click info)
Operating System & Version	Ubuntu 20
Kernel Version (Linux Only)	(e.g. 4.14.13)
Platform	Any
SDK Version	mater
Language	C++/CUDA
Segment	{Robot/Smartphone/VR/AR/others }

Issue Description

Some time ago there were questions in #7335 about the distortion model of the D455, and it was confirmed that it was inverse brown conrady. At the end of that issue, it was mentioned that there was an implementation of rs2_deproject_pixel_to_point for the inverse brown conrady. That code looks like

    if (intrin->model == RS2_DISTORTION_INVERSE_BROWN_CONRADY)
    {
        // need to loop until convergence 
        // 10 iterations determined empirically
        for (int i = 0; i < 10; i++)
        {
            float r2 = x * x + y * y;
            float icdist = (float)1 / (float)(1 + ((intrin->coeffs[4] * r2 + intrin->coeffs[1]) * r2 + intrin->coeffs[0]) * r2);
            float xq = x / icdist;
            float yq = y / icdist;
            float delta_x = 2 * intrin->coeffs[2] * xq * yq + intrin->coeffs[3] * (r2 + 2 * xq * xq);
            float delta_y = 2 * intrin->coeffs[3] * xq * yq + intrin->coeffs[2] * (r2 + 2 * yq * yq);
            x = (xo - delta_x) * icdist;
            y = (yo - delta_y) * icdist;
        }
    }
    if (intrin->model == RS2_DISTORTION_BROWN_CONRADY)
    {
        // need to loop until convergence 
        // 10 iterations determined empirically
        for (int i = 0; i < 10; i++)
        {
            float r2 = x * x + y * y;
            float icdist = (float)1 / (float)(1 + ((intrin->coeffs[4] * r2 + intrin->coeffs[1]) * r2 + intrin->coeffs[0]) * r2);
            float delta_x = 2 * intrin->coeffs[2] * x * y + intrin->coeffs[3] * (r2 + 2 * x * x);
            float delta_y = 2 * intrin->coeffs[3] * x * y + intrin->coeffs[2] * (r2 + 2 * y * y);
            x = (xo - delta_x) * icdist;
            y = (yo - delta_y) * icdist;
        }

    }

Note that the both involves iteration, this seems incorrect, as according to the documentation here https://dev.intelrealsense.com/docs/projection-in-intel-realsense-sdk-20#section-distortion-models, for inverse conrady

This model provides a closed-form formula to map from distorted points to undistored points, while mapping in the other direction requires iteration or lookup tables. 

and for brown conrady

This model provides a closed-form formula to map from undistorted points to distorted points, while mapping in the other direction requires iteration or lookup tables.

Therefore, for the same reprojection code, it cannot be true that both the forward and inverse distortion model would require iteration, one of them must be an incorrect implementation. I think in this case that the inverse brown conrady implementation is incorrect, because in the CUDA implementation of these functions in https://github.com/IntelRealSense/librealsense/blob/master/src/cuda/rscuda_utils.cuh, rs2_deproject_pixel_to_point for inverse brown conrady is

        if (intrin->model == RS2_DISTORTION_INVERSE_BROWN_CONRADY)
        {
            float r2 = x * x + y * y;
            float f = 1 + intrin->coeffs[0] * r2 + intrin->coeffs[1] * r2*r2 + intrin->coeffs[4] * r2*r2*r2;
            float ux = x * f + 2 * intrin->coeffs[2] * x*y + intrin->coeffs[3] * (r2 + 2 * x*x);
            float uy = y * f + 2 * intrin->coeffs[3] * x*y + intrin->coeffs[2] * (r2 + 2 * y*y);
            x = ux;
            y = uy;
        }

which is completely different from the deprojection code in the C++ implementation, and closed form, which matches the documentation.

This same inconsistency can be observed in the rs2_project_point_to_pixel code too, where the code for brown conrady and inverse brown conrady projection is functionally identical, which again violates what the documentation states. Given this, I'm think the projection and deprojection code for the inverse brown conrady distortion model in the C++ implementation is exactly flipped, the section for deprojection should be in projection, and vice versa.

[MartyG-RealSense]

Hi @oceanusxiv As mentioned in your other case at IntelRealSense/realsense-ros#2458 (comment) the D455 is known for its unusual coefficients compared to other RealSense 400 Series camera models and the nature of its distortion model has been previously discussed.

I will consult my Intel RealSense colleagues about your concerns.

[MartyG-RealSense]

Hi again @oceanusxiv I just wanted to update you that discussion of this subject with my Intel RealSense colleagues continues to be ongoing. Thanks for your patience!

[MartyG-RealSense]

Whilst Intel discussions are continuing, it may take some time before there is progress to report. I have therefore added an Enhancement label to this case as a reminder to keep it open whilst those internal discussions are ongoing.

[levasmol]

Hello! Any news about this issue? I plan to switch from D435 to D455 because of RGB sensor larger FOV needed for my object color segmentation and then I reconstruct the pointcloud based on undistorted aligned images of depth and color streams. I would like to use the undistorted RGB images to be aligned with Depth images. Will that be possible to do with D455?

[MartyG-RealSense]

Hi @levasmol I checked the outcome of the discussions that my Intel RealSense colleagues had about this subject. It was acknowledged that there was the possibility of a discrepancy between the documentation and how the distortion model was actually implemented. They thought that it was unlikely though that there is a fundamental flaw in the implementation, such as using the reversed version of the model from what is needed.

If you are aligning with the SDK's align_to instruction then the SDK's 'align processing block' will automatically make adjustments for differences between the depth and RGB streams, no matter which camera model is used.

[MartyG-RealSense]

Hi @levasmol Bearing in mind the response in the above comment, do you require further assistance with this case, please? Thanks!

[MartyG-RealSense]

Case closed due to no further comments received.