Merge pull request #4741 from honpong/tm2-calibration-write-api

T26x calibration write api bug fix
IntelRealSense · Aug 28, 2019 · cbeaed4 · cbeaed4
2 parents 8b48b3c + e9bdf86
commit cbeaed4
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 5 deletions.
diff --git a/include/librealsense2/hpp/rs_device.hpp b/include/librealsense2/hpp/rs_device.hpp
@@ -508,7 +508,7 @@ namespace rs2
         * \param[in] from_stream     only support RS2_STREAM_FISHEYE
         * \param[in] from_id         only support left fisheye = 1
         * \param[in] to_stream       only support RS2_STREAM_FISHEYE
-        * \param[in] to_id           only support left fisheye = 2
+        * \param[in] to_id           only support right fisheye = 2
         * \param[in] extrinsics      extrinsics value to be written to the device
         */
         void set_extrinsics(rs2_stream from_stream, int from_id, rs2_stream to_stream, int to_id, rs2_extrinsics& extrinsics)

diff --git a/src/tm2/tm-device.cpp b/src/tm2/tm-device.cpp
@@ -941,7 +941,7 @@ namespace librealsense
             auto inv = [](const rs2_extrinsics& src) {
                 rs2_extrinsics dst; 
                 auto dst_rotation = dst.rotation;
-                for (auto i : { 0,3,6,1,4,7,2,5,8 }) { *dst_rotation++ = dst.rotation[i]; }
+                for (auto i : { 0,3,6,1,4,7,2,5,8 }) { *dst_rotation++ = src.rotation[i]; }
                 dst.translation[0] = - src.rotation[0] * src.translation[0] - src.rotation[3] * src.translation[1] - src.rotation[6] * src.translation[2];
                 dst.translation[1] = - src.rotation[1] * src.translation[0] - src.rotation[4] * src.translation[1] - src.rotation[7] * src.translation[2];
                 dst.translation[2] = - src.rotation[2] * src.translation[0] - src.rotation[5] * src.translation[1] - src.rotation[8] * src.translation[2];
@@ -967,9 +967,9 @@ namespace librealsense
 
             perc::SensorId reference_sensor_id;
             auto& H_fe1_fe2 = extr;
+            auto H_fe2_fe1  = inv(H_fe1_fe2);
             auto H_fe1_pose = get_extrinsics(from_profile, reference_sensor_id);
-            //H_fe2_pose = H_fe2_fe1 * H_fe1_pose
-            auto H_fe2_pose = mult(inv(H_fe1_fe2), H_fe1_pose);
+            auto H_fe2_pose = mult(H_fe2_fe1, H_fe1_pose);  //H_fe2_pose = H_fe2_fe1 * H_fe1_pose
             set_extrinsics_to_ref(RS2_STREAM_FISHEYE, 2, H_fe2_pose);
             break;
         }

diff --git a/third-party/libtm/libtm/src/Device.cpp b/third-party/libtm/libtm/src/Device.cpp
@@ -1750,7 +1750,7 @@ namespace perc {
         Bulk_Message msg((uint8_t*)&request, request.header.dwLength, (uint8_t*)&response, sizeof(response), mEndpointBulkMessages | TO_DEVICE, mEndpointBulkMessages | TO_HOST);
 
         mDispatcher->sendMessage(&mFsm, msg);
-        if (msg.Result != toUnderlying(Status::SUCCESS))
+        if (msg.Result != toUnderlying(Status::SUCCESS) && msg.Result != toUnderlying(Status::TABLE_NOT_EXIST))
         {
             DEVICELOGE("USB Error (0x%X)", msg.Result);
             return Status::ERROR_USB_TRANSFER;
@@ -3967,6 +3967,11 @@ namespace perc {
         {
             msg.Result = toUnderlying(Status::ERROR_FW_INTERNAL);
         }
+        else if (res->wMessageID == DEV_RESET_CONFIGURATION && res->wStatus == toUnderlying(MESSAGE_STATUS::TABLE_NOT_EXIST))
+        { 
+            DEVICELOGW("MessageID 0x%X (%s) warning with status 0x%X TABLE_NOT_EXIST", res->wMessageID, messageCodeToString(LIBUSB_TRANSFER_TYPE_BULK, header->wMessageID).c_str(), res->wStatus);
+            msg.Result = toUnderlying(Status::TABLE_NOT_EXIST);
+        }
         else
         {
             DEVICELOGE("MessageID 0x%X (%s) failed with status 0x%X", res->wMessageID, messageCodeToString(LIBUSB_TRANSFER_TYPE_BULK, header->wMessageID).c_str(), res->wStatus);