{Feature} calibration - expose imu rectification matrix and bias vector

Summary: Add getter functions for accelerometer and gyroscope rectification matrix and bias vector

Reviewed By: chengshy, SeaOtocinclus

Differential Revision: D53718442

fbshipit-source-id: 3a0459551c881c389494020f00adbcd9c5b64f49

core/calibration/ImuMagnetometerCalibration.cpp

@@ -32,6 +32,14 @@ Eigen::Vector3d LinearRectificationModel3d::rectifiedToRaw(const Eigen::Vector3d
   return rectificationMatrix_ * rectified + bias_;
 }
 
+Eigen::Matrix3d LinearRectificationModel3d::getRectification() const {
+  return rectificationMatrix_;
+}
+
+Eigen::Vector3d LinearRectificationModel3d::getBias() const {
+  return bias_;
+}
+
 /* ImuCalibration */
 ImuCalibration::ImuCalibration(
     const std::string& label,
@@ -68,6 +76,14 @@ Eigen::Vector3d ImuCalibration::rectifiedToRawGyro(const Eigen::Vector3d& rectif
   return gyro_.rectifiedToRaw(rectified);
 }
 
+LinearRectificationModel3d ImuCalibration::getAccelModel() const {
+  return accel_;
+}
+
+LinearRectificationModel3d ImuCalibration::getGyroModel() const {
+  return gyro_;
+}
+
 /* MagnetometerCalibration */
 MagnetometerCalibration::MagnetometerCalibration(
     const std::string& label,

core/calibration/ImuMagnetometerCalibration.h

@@ -50,6 +50,16 @@ class LinearRectificationModel3d {
    */
   Eigen::Vector3d rectifiedToRaw(const Eigen::Vector3d& rectified) const;
 
+  /**
+   * @brief getter function for rectification matrix
+   */
+  Eigen::Matrix3d getRectification() const;
+
+  /**
+   * @brief getter function for bias vector
+   */
+  Eigen::Vector3d getBias() const;
+
  private:
   Eigen::Matrix3d rectificationMatrix_;
   Eigen::Vector3d bias_;
@@ -100,6 +110,16 @@ class ImuCalibration {
    */
   Eigen::Vector3d rectifiedToRawGyro(const Eigen::Vector3d& rectified) const;
 
+  /**
+   * @brief get accelerometer linear rectification model that includes rectification matrix and bias
+   */
+  LinearRectificationModel3d getAccelModel() const;
+
+  /**
+   * @brief get gyroscope linear rectification model that includes rectification matrix and bias
+   */
+  LinearRectificationModel3d getGyroModel() const;
+
  private:
   std::string label_;
   LinearRectificationModel3d accel_;

core/python/DeviceCalibrationPyBind.h

@@ -195,6 +195,19 @@ inline void declareCameraCalibration(py::module& m) {
       py::arg("camera_calibration"));
 }
 
+inline void declareLinearRectificationModel(py::module& m) {
+  py::class_<LinearRectificationModel3d>(
+      m,
+      "LinearRectificationModel3d",
+      "A class that contains imu and mag intrinsics rectification model.")
+      .def(py::init<const Eigen::Matrix3d&, const Eigen::Vector3d&>())
+      .def(
+          "get_rectification",
+          &LinearRectificationModel3d::getRectification,
+          "Get the rectification matrix. ")
+      .def("get_bias", &LinearRectificationModel3d::getBias, "Get the bias vector.");
+}
+
 inline void declareImuCalibration(py::module& m) {
   py::class_<ImuCalibration>(
       m,
@@ -233,6 +246,14 @@ inline void declareImuCalibration(py::module& m) {
           py::arg("rectified"),
           "simulate imu gyro sensor readout from actual angular velocity: "
           " raw = rectificationMatrix * rectified + bias.")
+      .def(
+          "get_accel_model",
+          &ImuCalibration::getAccelModel,
+          "Get accelerometer intrinsics model that contains rectification matrix and bias vector.")
+      .def(
+          "get_gyro_model",
+          &ImuCalibration::getGyroModel,
+          "Get gyroscope intrinsics model that contains rectification matrix and bias vector.")
       .def("get_transform_device_imu", &ImuCalibration::getT_Device_Imu)
       .def("__repr__", [](const ImuCalibration& self) { return fmt::to_string(self); });
 }
@@ -278,6 +299,7 @@ inline void declareMicrophoneCalibration(py::module& m) {
 
 inline void declareSensorCalibration(py::module& m) {
   declareCameraCalibration(m);
+  declareLinearRectificationModel(m);
   declareImuCalibration(m);
   declareMagnetometerCalibration(m);
   declareBarometerCalibration(m);

core/python/test/corePyBindTest.py

@@ -34,6 +34,44 @@
 
 
 class CalibrationTests(unittest.TestCase):
+    def test_imu_calibration_getter(self) -> None:
+        provider = data_provider.create_vrs_data_provider(vrs_filepath)
+
+        device_calib = provider.get_device_calibration()
+        assert device_calib is not None
+
+        imu_labels = ["imu-left", "imu-right"]
+
+        raw_accel_data = np.array([1.0, 0.2, 3.1])
+        raw_gyro_data = np.array([11.7, 1.2, 10.1])
+
+        for label in imu_labels:
+            imu_calib = device_calib.get_imu_calib(label)
+            assert imu_calib is not None
+
+            accel_model = imu_calib.get_accel_model()
+            gyro_model = imu_calib.get_gyro_model()
+
+            accel_rect = accel_model.get_rectification()
+            accel_bias = accel_model.get_bias()
+
+            gyro_rect = gyro_model.get_rectification()
+            gyro_bias = gyro_model.get_bias()
+
+            rectified_accel = imu_calib.raw_to_rectified_accel(raw_accel_data)
+            rectified_accel_compare = np.linalg.inv(accel_rect) @ (
+                raw_accel_data - accel_bias
+            )
+            np.testing.assert_array_almost_equal(
+                rectified_accel, rectified_accel_compare
+            )
+
+            rectified_gyro = imu_calib.raw_to_rectified_gyro(raw_gyro_data)
+            rectified_gyro_compare = np.linalg.inv(gyro_rect) @ (
+                raw_gyro_data - gyro_bias
+            )
+            np.testing.assert_array_almost_equal(rectified_gyro, rectified_gyro_compare)
+
     def test_calibration_label(self) -> None:
         provider = data_provider.create_vrs_data_provider(vrs_filepath)