Implements retrieving camera intrinsic parameters from T265

IntelRealSense · Mar 20, 2019 · a5d8bb3 · a5d8bb3
1 parent 10058ec
commit a5d8bb3
Show file tree

Hide file tree

Showing 6 changed files with 69 additions and 19 deletions.
diff --git a/include/librealsense2/h/rs_types.h b/include/librealsense2/h/rs_types.h
@@ -48,6 +48,7 @@ typedef enum rs2_distortion
     RS2_DISTORTION_INVERSE_BROWN_CONRADY , /**< Equivalent to Brown-Conrady distortion, except undistorts image instead of distorting it */
     RS2_DISTORTION_FTHETA                , /**< F-Theta fish-eye distortion model */
     RS2_DISTORTION_BROWN_CONRADY         , /**< Unmodified Brown-Conrady distortion model */
+    RS2_DISTORTION_KANNALA_BRANDT4       , /**< Four parameter Kannala Brandt distortion model */
     RS2_DISTORTION_COUNT                   /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */
 } rs2_distortion;
 const char* rs2_distortion_to_string(rs2_distortion distortion);

diff --git a/include/librealsense2/rsutil.h b/include/librealsense2/rsutil.h
@@ -36,10 +36,28 @@ static void rs2_project_point_to_pixel(float pixel[2], const struct rs2_intrinsi
     if (intrin->model == RS2_DISTORTION_FTHETA)
     {
         float r = sqrtf(x*x + y*y);
+        if (r < FLT_EPSILON)
+        {
+            r = FLT_EPSILON;
+        }
         float rd = (float)(1.0f / intrin->coeffs[0] * atan(2 * r* tan(intrin->coeffs[0] / 2.0f)));
         x *= rd / r;
         y *= rd / r;
     }
+    if (intrin->model == RS2_DISTORTION_KANNALA_BRANDT4)
+    {
+        float r = sqrtf(x*x + y*y);
+        if (r < FLT_EPSILON)
+        {
+            r = FLT_EPSILON;
+        }
+        float theta = atan(r);
+        float theta2 = theta*theta;
+        float series = 1 + theta2*(intrin->coeffs[0] + theta2*(intrin->coeffs[1] + theta2*(intrin->coeffs[2] + theta2*intrin->coeffs[3])));
+        float rd = theta*series;
+        x *= rd / r;
+        y *= rd / r;
+    }
 
     pixel[0] = x * intrin->fx + intrin->ppx;
     pixel[1] = y * intrin->fy + intrin->ppy;
@@ -49,7 +67,6 @@ static void rs2_project_point_to_pixel(float pixel[2], const struct rs2_intrinsi
 static void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrinsics * intrin, const float pixel[2], float depth)
 {
     assert(intrin->model != RS2_DISTORTION_MODIFIED_BROWN_CONRADY); // Cannot deproject from a forward-distorted image
-    assert(intrin->model != RS2_DISTORTION_FTHETA); // Cannot deproject to an ftheta image
     //assert(intrin->model != RS2_DISTORTION_BROWN_CONRADY); // Cannot deproject to an brown conrady model
 
     float x = (pixel[0] - intrin->ppx) / intrin->fx;
@@ -63,6 +80,43 @@ static void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrin
         x = ux;
         y = uy;
     }
+    if (intrin->model == RS2_DISTORTION_KANNALA_BRANDT4)
+    {
+        float rd = sqrtf(x*x + y*y);
+        if (rd < FLT_EPSILON)
+        {
+            rd = FLT_EPSILON;
+        }
+
+        float theta = rd;
+        float theta2 = rd*rd;
+        for (int i = 0; i < 4; i++) 
+        {
+            float f = theta*(1 + theta2*(intrin->coeffs[0] + theta2*(intrin->coeffs[1] + theta2*(intrin->coeffs[2] + theta2*intrin->coeffs[3])))) - rd;
+            if (abs(f) < FLT_EPSILON)
+            {
+                break;
+            }
+            float df = 1 + theta2*(3 * intrin->coeffs[0] + theta2*(5 * intrin->coeffs[1] + theta2*(7 * intrin->coeffs[2] + 9 * theta2*intrin->coeffs[3])));
+            theta -= f / df;
+            theta2 = theta*theta;
+        }
+        float r = tan(theta);
+        x *= r / rd;
+        y *= r / rd;
+    }
+    if (intrin->model == RS2_DISTORTION_FTHETA)
+    {
+        float rd = sqrtf(x*x + y*y);
+        if (rd < FLT_EPSILON)
+        {
+            rd = FLT_EPSILON;
+        }
+        float r = (float)(tan(intrin->coeffs[0] * rd) / atan(2 * tan(intrin->coeffs[0] / 2.0f)));
+        x *= r / rd;
+        y *= r / rd;
+    }
+
     point[0] = depth * x;
     point[1] = depth * y;
     point[2] = depth;

diff --git a/src/tm2/tm-conversions.h b/src/tm2/tm-conversions.h
@@ -67,11 +67,9 @@ namespace librealsense
     {
         switch (model)
         {
-        case 0: return RS2_DISTORTION_NONE;
-        case 1: return RS2_DISTORTION_MODIFIED_BROWN_CONRADY;
-        case 2: return RS2_DISTORTION_INVERSE_BROWN_CONRADY;
-        case 3: return RS2_DISTORTION_FTHETA;
-        case 4: //TODO - add KANNALA_BRANDT4;
+        case 1: return RS2_DISTORTION_FTHETA;
+        case 3: return RS2_DISTORTION_NONE;
+        case 4: return RS2_DISTORTION_KANNALA_BRANDT4;
         default:
             throw invalid_value_exception("Invalid TM2 camera model");
         }

diff --git a/src/tm2/tm-device.cpp b/src/tm2/tm-device.cpp
@@ -607,15 +607,14 @@ namespace librealsense
     rs2_intrinsics tm2_sensor::get_intrinsics(const stream_profile& profile) const
     {
         rs2_intrinsics result;
-        const TrackingData::CameraIntrinsics tm_intrinsics{};
+        TrackingData::CameraIntrinsics tm_intrinsics{};
         int stream_index = profile.index - 1;
-        //TODO - wait for TM2 intrinsics impl
-        //TODO - assuming IR only
-//             auto status = _tm_dev->GetCameraIntrinsics(tm_intrinsics, SET_SENSOR_ID(SensorType::Fisheye,stream_index));
-//             if (status != Status::SUCCESS)
-//             {
-//                 throw io_exception("Failed to read TM2 intrinsics");
-//             }
+
+        auto status = _tm_dev->GetCameraIntrinsics(SET_SENSOR_ID(SensorType::Fisheye,stream_index), tm_intrinsics);
+        if (status != Status::SUCCESS)
+        {
+            throw io_exception("Failed to read TM2 intrinsics");
+        }
 
         result.width = tm_intrinsics.width;
         result.height = tm_intrinsics.height;

diff --git a/third-party/libtm/libtm/include/TrackingData.h b/third-party/libtm/libtm/include/TrackingData.h
@@ -38,9 +38,7 @@ namespace perc
     {
         VideoProfile0 = 0,  /* Sensor index 0 - HMD High exposure left camera                           */
         VideoProfile1 = 1,  /* Sensor index 1 - HMD High exposure right camera                          */
-        VideoProfile2 = 2,  /* Sensor index 2 - HMD Low exposure left camera for controller 1 tracking  */
-        VideoProfile3 = 3,  /* Sensor index 3 - HMD Low exposure right camera for controller 2 tracking */
-        VideoProfileMax = 4,
+        VideoProfileMax = 2,
     };
 
     enum AccelerometerProfileType
@@ -677,7 +675,7 @@ namespace perc
             float_t ppy;              /**< Vertical coordinate of the principal point of the image, as a pixel offset from the top edge                                           */
             float_t fx;               /**< Focal length of the image plane, as a multiple of pixel width                                                                          */
             float_t fy;               /**< Focal length of the image plane, as a multiple of pixel Height                                                                         */
-            uint32_t distortionModel; /**< Distortion model of the image: NONE = 0, MODIFIED_BROWN_CONRADY = 1, INVERSE_BROWN_CONRADY = 2, FTHETA = 3, KANNALA_BRANDT4 = 4        */
+            uint32_t distortionModel; /**< Distortion model of the image: F-THETA = 1, NONE (UNDISTORTED) = 3, KANNALA_BRANDT4 = 4                                                */
             float_t coeffs[5];        /**< Distortion coefficients                                                                                                                */
         };
 

diff --git a/third-party/libtm/libtm/src/Message.h b/third-party/libtm/libtm/src/Message.h
@@ -450,7 +450,7 @@ namespace perc
         float_t flPpy;              /**< Vertical coordinate of the principal point of the image, as a pixel offset from the top edge                                    */
         float_t flFx;               /**< Focal length of the image plane, as a multiple of pixel width                                                                   */
         float_t flFy;               /**< Focal length of the image plane, as a multiple of pixel Height                                                                  */
-        uint32_t dwDistortionModel; /**< Distortion model of the image: NONE = 0, MODIFIED_BROWN_CONRADY = 1, INVERSE_BROWN_CONRADY = 2, FTHETA = 3, KANNALA_BRANDT4 = 4 */
+        uint32_t dwDistortionModel; /**< Distortion model of the image: F-THETA = 1, NONE (UNDISTORTED) = 3, KANNALA_BRANDT4 = 4 */
         float_t flCoeffs[5];        /**< Distortion coefficients                                                                                                         */
     } camera_intrinsics;