Implements retrieving camera intrinsic parameters from T265

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);

include/librealsense2/rsutil.h

@@ -13,6 +13,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <math.h>
+#include <float.h>
 
 /* Given a point in 3D space, compute the corresponding pixel coordinates in an image with no distortion or forward distortion coefficients produced by the same camera */
 static void rs2_project_point_to_pixel(float pixel[2], const struct rs2_intrinsics * intrin, const float point[3])
@@ -36,10 +37,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 +68,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 +81,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;

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");
         }

src/tm2/tm-device.cpp

@@ -198,6 +198,10 @@ namespace librealsense
         std::vector<TrackingData::VideoProfile> video_profiles(_tm_supported_profiles.video, _tm_supported_profiles.video + VideoProfileMax);
         for (auto tm_profile : video_profiles)
         {
+            if (tm_profile.sensorIndex == VideoProfileMax)
+            {
+                continue;
+            }
             rs2_stream stream = RS2_STREAM_FISHEYE; //TM2_API provides only fisheye video streams
             platform::stream_profile p = { tm_profile.profile.width, tm_profile.profile.height, tm_profile.fps, static_cast<uint32_t>(tm_profile.profile.pixelFormat) };
             auto profile = std::make_shared<video_stream_profile>(p);
@@ -607,15 +611,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;

src/types.cpp

@@ -328,6 +328,7 @@ namespace librealsense
             CASE(INVERSE_BROWN_CONRADY)
             CASE(FTHETA)
             CASE(BROWN_CONRADY)
+            CASE(KANNALA_BRANDT4)
         default: assert(!is_valid(value)); return UNKNOWN_VALUE;
         }
 #undef CASE

third-party/libtm/libtm/include/TrackingData.h

@@ -677,7 +677,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                                                                                                                */
         };
 

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;