T265 profile extrinsics

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

@@ -194,10 +194,16 @@ namespace librealsense
         {
             throw io_exception("Failed to get supported raw streams");
         }
+
+        std::map<SensorId, std::shared_ptr<stream_profile_interface> > profile_map;
         //extract video profiles
         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);
@@ -215,6 +221,7 @@ namespace librealsense
             stream_profile sp = { stream, profile->get_stream_index(), p.width, p.height, p.fps, profile->get_format() };
             auto intrinsics = get_intrinsics(sp);
             profile->set_intrinsics([intrinsics]() { return intrinsics; });
+            profile_map[SET_SENSOR_ID(SensorType::Fisheye, profile->get_stream_index() - 1)] = profile;
             if (tm_profile.sensorIndex <= 1) results.push_back(profile);
 
             //TODO - need to register to have resolve_requests work
@@ -242,6 +249,7 @@ namespace librealsense
             {
                 profile->tag_profile(profile_tag::PROFILE_TAG_DEFAULT | profile_tag::PROFILE_TAG_SUPERSET);
                 results.push_back(profile);
+                profile_map[SET_SENSOR_ID(SensorType::Gyro, profile->get_stream_index())] = profile;
             }
         }
 
@@ -262,6 +270,7 @@ namespace librealsense
             {
                 profile->tag_profile(profile_tag::PROFILE_TAG_DEFAULT | profile_tag::PROFILE_TAG_SUPERSET);
                 results.push_back(profile);
+                profile_map[SET_SENSOR_ID(SensorType::Accelerometer, profile->get_stream_index())] = profile;
             }
         }
 
@@ -287,10 +296,19 @@ namespace librealsense
             {
                 profile->tag_profile(profile_tag::PROFILE_TAG_DEFAULT | profile_tag::PROFILE_TAG_SUPERSET);
                 results.push_back(profile);
+                profile_map[SET_SENSOR_ID(SensorType::Pose, profile->get_stream_index())] = profile;
             }
             //TODO - do I need to define native_pixel_format for RS2_STREAM_POSE? and how to draw it?
         }
 
+        //add extrinic parameters
+        for (auto profile : results)
+        {
+            SensorId current_reference;
+            auto current_extrinsics = get_extrinsics(*profile, current_reference);
+            environment::get_instance().get_extrinsics_graph().register_extrinsics(*profile, *(profile_map[current_reference]), current_extrinsics);
+        }
+
         return results;
     }
 
@@ -607,15 +625,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;
@@ -661,6 +678,50 @@ namespace librealsense
         return result;
     }
 
+    rs2_extrinsics tm2_sensor::get_extrinsics(const stream_profile_interface & profile, perc::SensorId & reference_sensor_id) const
+    {
+
+        rs2_extrinsics result{0};
+        TrackingData::SensorExtrinsics tm_extrinsics{};
+        int stream_index = profile.get_stream_index();
+        SensorType type = SensorType::Max;
+        switch (profile.get_stream_type())
+        {
+        case RS2_STREAM_FISHEYE:
+            type = SensorType::Fisheye;
+            break;
+        case RS2_STREAM_ACCEL:
+            type = SensorType::Accelerometer;
+            break;
+        case RS2_STREAM_GYRO:
+            type = SensorType::Gyro;
+            break;
+        case RS2_STREAM_POSE:
+            type = SensorType::Pose;
+            break;
+        default:
+            throw invalid_value_exception("Invalid stream type");
+        }
+
+        if (type == SensorType::Fisheye)
+        {
+            stream_index--;
+        }
+
+        auto status = _tm_dev->GetExtrinsics(SET_SENSOR_ID(type, stream_index), tm_extrinsics);
+        if (status != Status::SUCCESS)
+        {
+            throw io_exception("Failed to read TM2 intrinsics");
+        }
+
+        librealsense::copy_array(result.rotation, tm_extrinsics.rotation);
+        librealsense::copy_array(result.translation, tm_extrinsics.translation);
+        reference_sensor_id = tm_extrinsics.referenceSensorId;
+
+        return result;
+    }
+
+
     // Tracking listener
     ////////////////////
     void tm2_sensor::onVideoFrame(perc::TrackingData::VideoFrame& tm_frame)

src/tm2/tm-device.h

@@ -62,6 +62,7 @@ namespace librealsense
         void stop() override;
         rs2_intrinsics get_intrinsics(const stream_profile& profile) const override;
         rs2_motion_device_intrinsic get_motion_intrinsics(const motion_stream_profile_interface& profile) const;
+        rs2_extrinsics get_extrinsics(const stream_profile_interface & profile, perc::SensorId & reference_sensor_id) const;
 
         // Tracking listener
         ////////////////////

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/TrackingCommon.h

@@ -55,11 +55,14 @@ namespace perc {
         Depth = 1,
         IR = 2,
         Fisheye = 3,
-        Gyro = 4, 
+        Gyro = 4,
         Accelerometer = 5,
         Controller = 6,
         Rssi = 7,
         Velocimeter = 8,
+        Stereo = 9,
+        Pose = 10,
+        ControllerProperty = 11,
         Max
     };
 

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;