L515 Brown-Conrady distortion plus testing, deproject impl (GitHub #7335)

include/librealsense2/rsutil.h

@@ -87,14 +87,40 @@ static void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrin
 
     float x = (pixel[0] - intrin->ppx) / intrin->fx;
     float y = (pixel[1] - intrin->ppy) / intrin->fy;
+
+    float xo = x;
+    float yo = y;
+
     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;
+        // 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;
+        }
+
     }
     if (intrin->model == RS2_DISTORTION_KANNALA_BRANDT4)
     {

src/cuda/cuda-pointcloud.cu

@@ -17,15 +17,40 @@ void deproject_pixel_to_point_cuda(float points[3], const struct rs2_intrinsics
     //assert(intrin->model != RS2_DISTORTION_BROWN_CONRADY); // Cannot deproject to an brown conrady model
     float x = (pixel[0] - intrin->ppx) / intrin->fx;
     float y = (pixel[1] - intrin->ppy) / intrin->fy;    
-    if(intrin->model == RS2_DISTORTION_INVERSE_BROWN_CONRADY)
+
+    float xo = x;
+    float yo = y;
+
+    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;
-    } 
+        // 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;
+        }
+    }
+    else 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;
+        }
+    }
     points[0] = depth * x;
     points[1] = depth * y;
     points[2] = depth;

src/depth-to-rgb-calibration.cpp

@@ -184,7 +184,8 @@ rs2_calibration_status depth_to_rgb_calibration::optimize(
 
         AC_LOG( DEBUG, "Optimization successful!" );
         _thermal_intr = _algo.get_calibration().get_intrinsics();
-        _thermal_intr.model = RS2_DISTORTION_INVERSE_BROWN_CONRADY;  // restore LRS model
+
+        _thermal_intr.model = l500_distortion;  // restore LRS model
 
         // Override everything in the raw intrinsics except the focal length (fx and fy)
         // TODO: AC is not "supposed" to change focal length, but we shouldn't assume this! The

src/gl/pointcloud-gl.cpp

@@ -53,14 +53,33 @@ static const char* project_fragment_text =
 "    float py = (1.0 - textCoords.y) * height1;\n"
 "    float x = (px - principal1.x) / focal1.x;\n"
 "    float y = (py - principal1.y) / focal1.y;\n"
-"    if(is_bc1 > 0.0)\n"
+"    float xo = x;\n"
+"    float yo = y;\n"
+"    if(is_bc1 == 2.0)\n"
 "    {\n"
-"        float r2  = x*x + y*y;\n"
-"        float f = 1.0 + coeffs1[0]*r2 + coeffs1[1]*r2*r2 + coeffs1[4]*r2*r2*r2;\n"
-"        float ux = x*f + 2.0*coeffs1[2]*x*y + coeffs1[3]*(r2 + 2.0*x*x);\n"
-"        float uy = y*f + 2.0*coeffs1[3]*x*y + coeffs1[2]*(r2 + 2.0*y*y);\n"
-"        x = ux;\n"
-"        y = uy;\n"
+"       for (int i = 0; i < 10; i++)\n"
+"       {\n"
+"           float r2 = x * x + y * y;\n"
+"           float icdist = 1.0 / (1.0 + ((coeffs1[4] * r2 + coeffs1[1])*r2 + coeffs1[0])*r2);\n"
+"           float xq = x / icdist;\n"
+"           float yq = y / icdist;\n"
+"           float delta_x = 2 * coeffs1[2] * xq*yq + coeffs1[3] * (r2 + 2 * xq*xq);\n"
+"           float delta_y = 2 * coeffs1[3] * xq*yq + coeffs1[2] * (r2 + 2 * yq*yq);\n"
+"           x = (xo - delta_x)*icdist;\n"
+"           y = (yo - delta_y)*icdist;\n"
+"       }\n"
+"    }\n"
+"    if (is_bc1 == 4.0)\n"
+"    {\n"
+"        for (int i = 0; i < 10; i++)\n"
+"        {\n"
+"            float r2 = x * x + y * y;\n"
+"            float icdist = 1.0 / (1.0 + ((coeffs1[4] * r2 + coeffs1[1])*r2 + coeffs1[0])*r2);\n"
+"            float delta_x = 2 * coeffs1[2] * x*y + coeffs1[3] * (r2 + 2 * x*x);\n"
+"            float delta_y = 2 * coeffs1[3] * x*y + coeffs1[2] * (r2 + 2 * y*y);\n"
+"            x = (xo - delta_x)*icdist;\n"
+"            y = (yo - delta_y)*icdist;\n"
+"        }\n"
 "    }\n"
 "    vec2 tex = vec2(textCoords.x, 1.0 - textCoords.y);\n"
 "    vec4 dp = texture(textureSampler, tex);\n"
@@ -74,7 +93,7 @@ static const char* project_fragment_text =
 "    x = trans.x / trans.z;\n"
 "    y = trans.y / trans.z;\n"
 "\n"
-"    if(is_bc2 > 0.0)\n"
+"    if(is_bc2 == 2.0)\n"
 "    {\n"
 "        float r2  = x*x + y*y;\n"
 "        float f = 1.0 + coeffs2[0]*r2 + coeffs2[1]*r2*r2 + coeffs2[4]*r2*r2*r2;\n"
@@ -85,6 +104,17 @@ static const char* project_fragment_text =
 "        x = dx;\n"
 "        y = dy;\n"
 "    }\n"
+"    if (is_bc2 == 4.0)\n"
+"    {\n"
+"        float r2 = x * x + y * y;\n"
+"        float f = 1 + coeffs2[0] * r2 + coeffs2[1] * r2*r2 + coeffs2[4] * r2*r2*r2;\n"
+"        float xf = x * f;\n"
+"        float yf = y * f;\n"
+"        float dx = xf + 2 * coeffs2[2] * x*y + coeffs2[3] * (r2 + 2 * x*x);\n"
+"        float dy = yf + 2 * coeffs2[3] * x*y + coeffs2[2] * (r2 + 2 * y*y);\n"
+"        x = dx;\n"
+"        y = dy;\n"
+"    }\n"
 "    // TODO: Enable F-Thetha\n"
 "    //if (intrin->model == RS2_DISTORTION_FTHETA)\n"
 "    //{\n"
@@ -248,10 +278,9 @@ class project_shader : public texture_2d_shader
     {
         rs2::float2 focal{ intr.fx, intr.fy };
         rs2::float2 principal{ intr.ppx, intr.ppy };
-        float is_bc = (intr.model == RS2_DISTORTION_INVERSE_BROWN_CONRADY ? 1.f : 0.f);
         _shader->load_uniform(_focal_location[idx], focal);
         _shader->load_uniform(_principal_location[idx], principal);
-        _shader->load_uniform(_is_bc_location[idx], is_bc);
+        _shader->load_uniform(_is_bc_location[idx], (float)(int)intr.model);
         glUniform1fv(_coeffs_location[idx], 5, intr.coeffs);
     }
 

src/l500/l500-color.cpp

@@ -258,7 +258,8 @@ namespace librealsense
                     intrinsics.coeffs[3] = model.distort.tangential_p2;
                     intrinsics.coeffs[4] = model.distort.radial_k3;
 
-                    intrinsics.model = RS2_DISTORTION_INVERSE_BROWN_CONRADY;
+                    intrinsics.model = l500_distortion;
+
                 }
 
                 return intrinsics;
@@ -345,8 +346,9 @@ namespace librealsense
         // The distortion model is not part of the table. The FW assumes it is brown,
         // but in LRS we (mistakenly) use INVERSE brown. We therefore make sure the user
         // has not tried to change anything from the intrinsics reported:
-        if( intr.model != RS2_DISTORTION_INVERSE_BROWN_CONRADY )
-            throw invalid_value_exception( "invalid intrinsics distortion model" );
+
+        if( intr.model != l500_distortion)
+            throw invalid_value_exception("invalid intrinsics distortion model");
 
         rgb_calibration_table table;
         AC_LOG( DEBUG, "Reading RGB calibration table 0x" << std::hex << table.table_id );

src/l500/l500-color.h

@@ -16,6 +16,8 @@
 namespace librealsense
 {
 
+    const rs2_distortion l500_distortion = RS2_DISTORTION_BROWN_CONRADY;
+
     class l500_color
         : public virtual l500_device
     {

src/proc/sse/sse-pointcloud.cpp

@@ -139,15 +139,15 @@ namespace librealsense
         return (float3*)output.get_vertices();
     }
 
-    void pointcloud_sse::get_texture_map(rs2::points output,
-        const float3* points,
-        const unsigned int width,
-        const unsigned int height,
-        const rs2_intrinsics &other_intrinsics,
-        const rs2_extrinsics& extr,
-        float2* pixels_ptr)
+    void pointcloud_sse::get_texture_map_sse( float2 * texture_map,
+                                          const float3 * points,
+                                          const unsigned int width,
+                                          const unsigned int height,
+                                          const rs2_intrinsics & other_intrinsics,
+                                          const rs2_extrinsics & extr,
+                                          float2 * pixels_ptr )
     {
-        auto tex_ptr = (float2*)output.get_texture_coordinates();
+        auto tex_ptr = texture_map;
 
 #ifdef __SSSE3__
         auto point = reinterpret_cast<const float*>(points);
@@ -177,7 +177,8 @@ namespace librealsense
         auto ppy = _mm_set_ps1(other_intrinsics.ppy);
         auto w = _mm_set_ps1(float(other_intrinsics.width));
         auto h = _mm_set_ps1(float(other_intrinsics.height));
-        auto mask_inv_brown_conrady = _mm_set_ps1(RS2_DISTORTION_INVERSE_BROWN_CONRADY);
+        auto mask_brown_conrady = _mm_set_ps1(RS2_DISTORTION_BROWN_CONRADY);
+        auto mask_distortion_none = _mm_set_ps1(RS2_DISTORTION_NONE);
         auto zero = _mm_set_ps1(0);
         auto one = _mm_set_ps1(1);
         auto two = _mm_set_ps1(2);
@@ -212,24 +213,29 @@ namespace librealsense
             auto r3 = _mm_add_ps(_mm_mul_ps(c[1], _mm_mul_ps(r2, r2)), _mm_mul_ps(c[4], _mm_mul_ps(r2, _mm_mul_ps(r2, r2))));
             auto f = _mm_add_ps(one, _mm_add_ps(_mm_mul_ps(c[0], r2), r3));
 
+            auto brown = _mm_cmpeq_ps(mask_brown_conrady, dist);
+
             auto x_f = _mm_mul_ps(p_x, f);
             auto y_f = _mm_mul_ps(p_y, f);
 
-            auto r4 = _mm_mul_ps(c[3], _mm_add_ps(r2, _mm_mul_ps(two, _mm_mul_ps(x_f, x_f))));
-            auto d_x = _mm_add_ps(x_f, _mm_add_ps(_mm_mul_ps(two, _mm_mul_ps(c[2], _mm_mul_ps(x_f, y_f))), r4));
+            auto x_f_dist = _mm_or_ps(_mm_and_ps(brown, p_x), _mm_andnot_ps(brown, x_f));
+            auto y_f_dist = _mm_or_ps(_mm_and_ps(brown, p_y), _mm_andnot_ps(brown, y_f));
 
-            auto r5 = _mm_mul_ps(c[2], _mm_add_ps(r2, _mm_mul_ps(two, _mm_mul_ps(y_f, y_f))));
-            auto d_y = _mm_add_ps(y_f, _mm_add_ps(_mm_mul_ps(two, _mm_mul_ps(c[3], _mm_mul_ps(x_f, y_f))), r4));
+            auto r4 = _mm_mul_ps(c[3], _mm_add_ps(r2, _mm_mul_ps(two, _mm_mul_ps(x_f_dist, x_f_dist))));
+            auto d_x = _mm_add_ps(x_f, _mm_add_ps(_mm_mul_ps(two, _mm_mul_ps(c[2], _mm_mul_ps(x_f_dist, y_f_dist))), r4));
 
-            auto cmp = _mm_cmpeq_ps(mask_inv_brown_conrady, dist);
+            auto r5 = _mm_mul_ps(c[2], _mm_add_ps(r2, _mm_mul_ps(two, _mm_mul_ps(y_f_dist, y_f_dist))));
+            auto d_y = _mm_add_ps(y_f, _mm_add_ps(_mm_mul_ps(two, _mm_mul_ps(c[3], _mm_mul_ps(x_f_dist, y_f_dist))), r5));
 
-            p_x = _mm_or_ps(_mm_and_ps(cmp, d_x), _mm_andnot_ps(cmp, p_x));
-            p_y = _mm_or_ps(_mm_and_ps(cmp, d_y), _mm_andnot_ps(cmp, p_y));
+            auto distortion_none = _mm_cmpeq_ps(mask_distortion_none, dist);
+
+            p_x = _mm_or_ps(_mm_and_ps(distortion_none, p_x ), _mm_andnot_ps(distortion_none, d_x));
+            p_y = _mm_or_ps(_mm_and_ps(distortion_none, p_y ), _mm_andnot_ps(distortion_none, d_y));
 
             //TODO: add handle to RS2_DISTORTION_FTHETA
 
             //zero the x and y if z is zero
-            cmp = _mm_cmpneq_ps(z, zero);
+            auto cmp = _mm_cmpneq_ps(z, zero);
             p_x = _mm_and_ps(_mm_add_ps(_mm_mul_ps(p_x, fx), ppx), cmp);
             p_y = _mm_and_ps(_mm_add_ps(_mm_mul_ps(p_y, fy), ppy), cmp);
 
@@ -262,4 +268,22 @@ namespace librealsense
 #endif
 
     }
-}
+
+    void pointcloud_sse::get_texture_map( rs2::points output,
+                                          const float3 * points,
+                                          const unsigned int width,
+                                          const unsigned int height,
+                                          const rs2_intrinsics & other_intrinsics,
+                                          const rs2_extrinsics & extr,
+                                          float2 * pixels_ptr )
+    {
+
+        get_texture_map_sse( (float2 *)output.get_texture_coordinates(),
+                         points,
+                         width,
+                         height,
+                         other_intrinsics,
+                         extr,
+                         pixels_ptr );
+    }
+    }

src/proc/sse/sse-pointcloud.h

@@ -10,6 +10,15 @@ namespace librealsense
     {
     public:
         pointcloud_sse();
+
+        void get_texture_map_sse(float2 * texture_map,
+            const float3 * points,
+            const unsigned int width,
+            const unsigned int height,
+            const rs2_intrinsics & other_intrinsics,
+            const rs2_extrinsics & extr,
+            float2 * pixels_ptr);
+
     private:
         void preprocess() override;
         const float3 * depth_to_points(