From 667796139be2d3a0ec466f502ea8a0ddedc45a8d Mon Sep 17 00:00:00 2001 From: Daniel Mirota Date: Fri, 22 Mar 2019 16:53:43 -0700 Subject: [PATCH 1/2] Implements retrieving camera intrinsic parameters from T265 --- include/librealsense2/h/rs_types.h | 1 + include/librealsense2/rsutil.h | 57 ++++++++++++++++++- src/tm2/tm-conversions.h | 8 +-- src/tm2/tm-device.cpp | 19 ++++--- src/types.cpp | 1 + .../libtm/libtm/include/TrackingData.h | 2 +- third-party/libtm/libtm/src/Message.h | 2 +- 7 files changed, 74 insertions(+), 16 deletions(-) diff --git a/include/librealsense2/h/rs_types.h b/include/librealsense2/h/rs_types.h index b8d3891c82..ce1b905fdc 100644 --- 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 index 5120d4cfe3..88807555b4 100644 --- a/include/librealsense2/rsutil.h +++ b/include/librealsense2/rsutil.h @@ -13,6 +13,7 @@ #include #include #include +#include /* 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; diff --git a/src/tm2/tm-conversions.h b/src/tm2/tm-conversions.h index 0d1b6a5d52..2efc3c42a5 100644 --- 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 index fe668341b9..45d9d7ebd6 100644 --- a/src/tm2/tm-device.cpp +++ b/src/tm2/tm-device.cpp @@ -198,6 +198,10 @@ namespace librealsense std::vector 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(tm_profile.profile.pixelFormat) }; auto profile = std::make_shared(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; diff --git a/src/types.cpp b/src/types.cpp index 36e84fcd49..42b6ad302d 100644 --- a/src/types.cpp +++ b/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 diff --git a/third-party/libtm/libtm/include/TrackingData.h b/third-party/libtm/libtm/include/TrackingData.h index f3da075269..5275af61a1 100644 --- a/third-party/libtm/libtm/include/TrackingData.h +++ b/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 */ }; diff --git a/third-party/libtm/libtm/src/Message.h b/third-party/libtm/libtm/src/Message.h index e28505d395..f9179302af 100644 --- 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; From 0fb2ac21c641352b32ac18a53c3c0aa7049e86e5 Mon Sep 17 00:00:00 2001 From: Daniel Mirota Date: Thu, 21 Mar 2019 11:38:03 -0700 Subject: [PATCH 2/2] Implements API for profile extrinsic parameters on T265 --- src/tm2/tm-device.cpp | 58 +++++++++++++++++++ src/tm2/tm-device.h | 1 + .../libtm/libtm/include/TrackingCommon.h | 5 +- 3 files changed, 63 insertions(+), 1 deletion(-) diff --git a/src/tm2/tm-device.cpp b/src/tm2/tm-device.cpp index 45d9d7ebd6..c175b67ce0 100644 --- a/src/tm2/tm-device.cpp +++ b/src/tm2/tm-device.cpp @@ -194,6 +194,8 @@ namespace librealsense { throw io_exception("Failed to get supported raw streams"); } + + std::map > profile_map; //extract video profiles std::vector video_profiles(_tm_supported_profiles.video, _tm_supported_profiles.video + VideoProfileMax); for (auto tm_profile : video_profiles) @@ -219,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 @@ -246,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; } } @@ -266,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; } } @@ -291,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; } @@ -664,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) diff --git a/src/tm2/tm-device.h b/src/tm2/tm-device.h index 6e5829b067..9a594b0b81 100644 --- a/src/tm2/tm-device.h +++ b/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 //////////////////// diff --git a/third-party/libtm/libtm/include/TrackingCommon.h b/third-party/libtm/libtm/include/TrackingCommon.h index 67a2c40f49..5b0b30d9a7 100644 --- a/third-party/libtm/libtm/include/TrackingCommon.h +++ b/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 };