Add interpolated hand API to MPS.

Summary: This diff adds interpolated hand API in MPS data provider.

Reviewed By: kongchen1992

Differential Revision: D82865601

fbshipit-source-id: 576816c4a1b4fca4a5733d4415499c300eceb61b

Author: YLouWashU

core/mps/MpsDataProvider.cpp

@@ -132,6 +132,31 @@ std::optional<HandTrackingResult> MpsDataProvider::getHandTrackingResult(
   return iter == handTrackingResults_.end() ? std::optional<HandTrackingResult>() : iter->second;
 }
 
+std::optional<HandTrackingResult> MpsDataProvider::getInterpolatedHandTrackingResult(
+    int64_t deviceTimeStampNs) {
+  // Query for the hand pose before and after the query timestamp
+  auto handBefore = getHandTrackingResult(deviceTimeStampNs, TimeQueryOptions::Before);
+  auto handAfter = getHandTrackingResult(deviceTimeStampNs, TimeQueryOptions::After);
+
+  // Check for before and after hand availability
+  if (!handBefore.has_value() && !handAfter.has_value()) {
+    std::string error = fmt::format(
+        "Query before and after timestamp {} returns empty results, hand tracking data is likely corrupted",
+        deviceTimeStampNs);
+    XR_LOGE("{}", error);
+    throw std::runtime_error{error};
+  }
+
+  if (!handBefore.has_value() || !handAfter.has_value()) {
+    // query before the beginning of traj or after the end
+    return {};
+  }
+
+  std::chrono::microseconds queryTimestampUs = std::chrono::microseconds(deviceTimeStampNs / 1000);
+
+  return interpolateHandTrackingResult(handBefore.value(), handAfter.value(), queryTimestampUs);
+}
+
 std::optional<EyeGaze> MpsDataProvider::getPersonalizedEyeGaze(
     int64_t deviceTimeStampNs,
     const TimeQueryOptions& timeQueryOptions) {

core/mps/MpsDataProvider.h

@@ -207,6 +207,15 @@ class MpsDataProvider {
       int64_t captureTimestampNs,
       const TimeQueryOptions& timeQueryOptions = TimeQueryOptions::Closest);
 
+  /**
+   * @brief Query MPS for Hand tracking result, at a specific timestamp, and return interpolated
+   * pose.
+   * @param deviceTimeStampNs The query timestamp in `TimeDomain::DeviceTime`.
+   * @return optional HandTrackingResult, will return invalid if the query is
+   * out of bound
+   */
+  std::optional<HandTrackingResult> getInterpolatedHandTrackingResult(int64_t deviceTimeStampNs);
+
   /**
    * @brief Check if HandTrackingResults are available in the MPS data paths
    * @return true if data is available, false otherwise

core/python/MpsPyBind.h

@@ -801,6 +801,13 @@ void exportMps(py::module& m) {
           "Check for data availability first using 'has_hand_tracking_results()'",
           py::arg("capture_timestamp_ns"),
           py::arg("time_query_options") = TimeQueryOptions::Closest)
+      .def(
+          "get_interpolated_hand_tracking_result",
+          &MpsDataProvider::getInterpolatedHandTrackingResult,
+          py::return_value_policy::reference_internal,
+          "Get the interploated MPS hand tracking result (landmarks, wrist transform, wrist and palm normals, etc.). "
+          "This will return None if the interpolation fails. Check the return value for validity.",
+          py::arg("capture_timestamp_ns"))
       .def("get_slam_version", &MpsDataProvider::getSlamVersion, "Get the MPS SLAM version.")
       .def(
           "get_eyegaze_version",