feat(src): added imu params: assembly error, temperature error

include/mynteye/types.h

@@ -500,12 +500,24 @@ struct MYNTEYE_API ImuIntrinsics {
    * \endcode
    */
   double scale[3][3];
+  /** Assembly error [3][3] */
+  double assembly[3][3];
   /* Zero-drift: X, Y, Z */
   double drift[3];
   /** Noise density variances */
   double noise[3];
   /** Random walk variances */
   double bias[3];
+
+  /** Temperature drift
+   *  \code
+   *    0 - Constant value
+   *    1 - Slope
+   *  \endcode
+   */
+  double x[2];
+  double y[2];
+  double z[2];
 };
 
 MYNTEYE_API

src/mynteye/device/channel/bytes.cc

@@ -103,7 +103,8 @@ std::size_t from_data(IntrinsicsEquidistant *in, const std::uint8_t *data,
   return i;
 }
 
-std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data) {
+std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data,
+    bool get_size) {
   std::size_t i = 0;
 
   // scale
@@ -113,6 +114,15 @@ std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data) {
     }
   }
   i += 72;
+  if (get_size) {
+    // assembly
+    for (std::size_t j = 0; j < 3; j++) {
+      for (std::size_t k = 0; k < 3; k++) {
+        in->assembly[j][k] = _from_data<double>(data + i + (j * 3 + k) * 8);
+      }
+    }
+    i += 72;
+  }
   // drift
   for (std::size_t j = 0; j < 3; j++) {
     in->drift[j] = _from_data<double>(data + i + j * 8);
@@ -128,6 +138,24 @@ std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data) {
     in->bias[j] = _from_data<double>(data + i + j * 8);
   }
   i += 24;
+  if (get_size) {
+    // temperature drift
+    // x
+    for (std::size_t j = 0; j < 2; j++) {
+      in->x[j] = _from_data<double>(data + i + j * 8);
+    }
+    i += 16;
+    // y
+    for (std::size_t j = 0; j < 2; j++) {
+      in->y[j] = _from_data<double>(data + i + j * 8);
+    }
+    i += 16;
+    // z
+    for (std::size_t j = 0; j < 2; j++) {
+      in->z[j] = _from_data<double>(data + i + j * 8);
+    }
+    i += 16;
+  }
 
   return i;
 }
@@ -236,7 +264,8 @@ std::size_t to_data(const IntrinsicsEquidistant *in, std::uint8_t *data,
   return i;
 }
 
-std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data) {
+std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data,
+    bool set_size) {
   std::size_t i = 0;
 
   // scale
@@ -246,6 +275,15 @@ std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data) {
     }
   }
   i += 72;
+  if (set_size) {
+    // assembly
+    for (std::size_t j = 0; j < 3; j++) {
+      for (std::size_t k = 0; k < 3; k++) {
+        _to_data(in->assembly[j][k], data + i + (j * 3 + k) * 8);
+      }
+    }
+    i += 72;
+  }
   // drift
   for (std::size_t j = 0; j < 3; j++) {
     _to_data(in->drift[j], data + i + j * 8);
@@ -261,6 +299,24 @@ std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data) {
     _to_data(in->bias[j], data + i + j * 8);
   }
   i += 24;
+  if (set_size) {
+    // temperature drift
+    // x
+    for (std::size_t j = 0; j < 2; j++) {
+      _to_data<double>(in->x[j], data + i + j * 8);
+    }
+    i += 16;
+    // y
+    for (std::size_t j = 0; j < 2; j++) {
+      _to_data<double>(in->y[j], data + i + j * 8);
+    }
+    i += 16;
+    // z
+    for (std::size_t j = 0; j < 2; j++) {
+      _to_data<double>(in->z[j], data + i + j * 8);
+    }
+    i += 16;
+  }
 
   return i;
 }

src/mynteye/device/channel/bytes.h

@@ -55,7 +55,8 @@ std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data,
 std::size_t from_data(IntrinsicsEquidistant *in, const std::uint8_t *data,
     bool get_size);
 
-std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data);
+std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data,
+    bool get_size);
 
 std::size_t from_data(Extrinsics *ex, const std::uint8_t *data);
 
@@ -88,7 +89,8 @@ std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data,
 std::size_t to_data(const IntrinsicsEquidistant *in, std::uint8_t *data,
     bool set_size);
 
-std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data);
+std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data, bool
+    set_size);
 
 std::size_t to_data(const Extrinsics *ex, std::uint8_t *data);
 

src/mynteye/device/channel/file_channel.cc

@@ -442,16 +442,19 @@ std::size_t ImuParamsParser::GetFromData(
 
 std::size_t ImuParamsParser::SetToData(
     const imu_params_t *imu_params, std::uint8_t *data) const {
-  // always set imu params with new version format
+  if (spec_version_ >= Version(1, 2)) {
     return SetToData_new(imu_params, data);
+  } else {
+    return SetToData_old(imu_params, data);
+  }
 }
 
 std::size_t ImuParamsParser::GetFromData_old(
     const std::uint8_t *data, const std::uint16_t &data_size,
     imu_params_t *imu_params) const {
   std::size_t i = 0;
-  i += bytes::from_data(&imu_params->in_accel, data + i);
+  i += bytes::from_data(&imu_params->in_accel, data + i, false);
-  i += bytes::from_data(&imu_params->in_gyro, data + i);
+  i += bytes::from_data(&imu_params->in_gyro, data + i, false);
   i += bytes::from_data(&imu_params->ex_left_to_imu, data + i);
   imu_params->version = spec_version_.to_string();
   MYNTEYE_UNUSED(data_size)
@@ -461,8 +464,8 @@ std::size_t ImuParamsParser::GetFromData_old(
 std::size_t ImuParamsParser::SetToData_old(
     const imu_params_t *imu_params, std::uint8_t *data) const {
   std::size_t i = 3;  // skip id, size
-  i += bytes::to_data(&imu_params->in_accel, data + i);
+  i += bytes::to_data(&imu_params->in_accel, data + i, false);
-  i += bytes::to_data(&imu_params->in_gyro, data + i);
+  i += bytes::to_data(&imu_params->in_gyro, data + i, false);
   i += bytes::to_data(&imu_params->ex_left_to_imu, data + i);
   // others
   std::size_t size = i - 3;
@@ -482,8 +485,8 @@ std::size_t ImuParamsParser::GetFromData_new(
   i += 2;
   // get imu params according to version
   if (version == Version(1, 2)) {  // v1.2
-    i += bytes::from_data(&imu_params->in_accel, data + i);
+    i += bytes::from_data(&imu_params->in_accel, data + i, true);
-    i += bytes::from_data(&imu_params->in_gyro, data + i);
+    i += bytes::from_data(&imu_params->in_gyro, data + i, true);
     i += bytes::from_data(&imu_params->ex_left_to_imu, data + i);
   } else {
     LOG(FATAL) << "Could not get imu params of version "
@@ -506,8 +509,8 @@ std::size_t ImuParamsParser::SetToData_new(
   i += 2;
   // set imu params with new version format
   if (version_raw <= version_new) {
-    i += bytes::to_data(&imu_params->in_accel, data + i);
+    i += bytes::to_data(&imu_params->in_accel, data + i, true);
-    i += bytes::to_data(&imu_params->in_gyro, data + i);
+    i += bytes::to_data(&imu_params->in_gyro, data + i, true);
     i += bytes::to_data(&imu_params->ex_left_to_imu, data + i);
   } else {
     LOG(FATAL) << "Could not set imu params of version "

src/mynteye/types.cc

@@ -246,6 +246,15 @@ std::ostream &operator<<(std::ostream &os, const ImuIntrinsics &in) {
     os << in.scale[2][i] << ", ";
   os << in.scale[2][2] << "]";
 
+  os << ", assembly: [";
+  for (int i = 0; i <= 2; i++)
+    os << in.assembly[0][i] << ", ";
+  for (int i = 0; i <= 2; i++)
+    os << in.assembly[1][i] << ", ";
+  for (int i = 0; i <= 2; i++)
+    os << in.assembly[2][i] << ", ";
+  os << in.assembly[2][2] << "]";
+
   os << ", drift: [";
   for (int i = 0; i <= 1; i++)
     os << in.drift[i] << ", ";
@@ -261,6 +270,21 @@ std::ostream &operator<<(std::ostream &os, const ImuIntrinsics &in) {
     os << in.bias[i] << ", ";
   os << in.bias[2] << "]";
 
+  os << ", x: [";
+  for (int i = 0; i <= 0; i++)
+    os << in.x[i] << ", ";
+  os << in.x[1] << "]";
+
+  os << ", y: [";
+  for (int i = 0; i <= 0; i++)
+    os << in.y[i] << ", ";
+  os << in.y[1] << "]";
+
+  os << ", z: [";
+  for (int i = 0; i <= 0; i++)
+    os << in.z[i] << ", ";
+  os << in.z[1] << "]";
+
   return os;
 }
 

tools/writer/config/S1030/imu.params

@@ -3,18 +3,26 @@
 version: "1.2"
 in_accel:
    scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+   assembly: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
    drift: [ 0., 0., 0. ]
    noise: [ 1.6925432397973516e-02, 1.6735310195561025e-02,
        1.7452487504590969e-02 ]
    bias: [ 1.9031356589714596e-04, 1.6996777864587261e-04,
        5.4490537096493644e-04 ]
+   x: [ 0.0, 0.0 ]
+   y: [ 0.0, 0.0 ]
+   z: [ 0.0, 0.0 ]
 in_gyro:
    scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+   assembly: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
    drift: [ 0., 0., 0. ]
    noise: [ 1.0848026158819934e-03, 1.2466367883501759e-03,
        1.1003229919806443e-03 ]
    bias: [ 2.3404834136742844e-05, 2.3596771567764949e-05,
        1.4970418056326829e-05 ]
+   x: [ 0.0, 0.0 ]
+   y: [ 0.0, 0.0 ]
+   z: [ 0.0, 0.0 ]
 ex_left_to_imu:
    rotation: [ -6.4662000000000001e-03, -9.9994994000000004e-01,
        -7.6356499999999999e-03, 9.9997908999999996e-01,

tools/writer/config/S210A/imu.params

@@ -3,18 +3,26 @@
 version: "1.2"
 in_accel:
    scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+   assembly: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
    drift: [ 0., 0., 0. ]
    noise: [ 1.6925432397973516e-02, 1.6735310195561025e-02,
        1.7452487504590969e-02 ]
    bias: [ 1.9031356589714596e-04, 1.6996777864587261e-04,
        5.4490537096493644e-04 ]
+   x: [ 0.0, 0.0 ]
+   y: [ 0.0, 0.0 ]
+   z: [ 0.0, 0.0 ]
 in_gyro:
    scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+   assembly: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
    drift: [ 0., 0., 0. ]
    noise: [ 1.0848026158819934e-03, 1.2466367883501759e-03,
        1.1003229919806443e-03 ]
    bias: [ 2.3404834136742844e-05, 2.3596771567764949e-05,
        1.4970418056326829e-05 ]
+   x: [ 0.0, 0.0 ]
+   y: [ 0.0, 0.0 ]
+   z: [ 0.0, 0.0 ]
 ex_left_to_imu:
    rotation: [ -6.4662000000000001e-03, -9.9994994000000004e-01,
        -7.6356499999999999e-03, 9.9997908999999996e-01,

tools/writer/device_writer.cc

@@ -157,11 +157,20 @@ cv::FileStorage &operator<<(cv::FileStorage &fs, const ImuIntrinsics &in) {
       scales.push_back(in.scale[i][j]);
     }
   }
+  std::vector<double> assembly;
+  for (std::size_t i = 0; i < 3; i++) {
+    for (std::size_t j = 0; j < 3; j++) {
+      assembly.push_back(in.assembly[i][j]);
+    }
+  }
   fs << "{"
-     << "scale" << scales << "drift"
+     << "scale" << scales << "assembly" << assembly << "drift"
      << std::vector<double>(in.drift, in.drift + 3) << "noise"
      << std::vector<double>(in.noise, in.noise + 3) << "bias"
-     << std::vector<double>(in.bias, in.bias + 3) << "}";
+     << std::vector<double>(in.bias, in.bias + 3) << "x"
+     << std::vector<double>(in.x, in.x + 2) << "y"
+     << std::vector<double>(in.y, in.y + 2) << "z"
+     << std::vector<double>(in.z, in.z + 2) << "}";
   return fs;
 }
 
@@ -343,6 +352,11 @@ void operator>>(const cv::FileNode &n, ImuIntrinsics &in) {
       in.scale[i][j] = n["scale"][3 * i + j];
     }
   }
+  for (std::size_t i = 0; i < 3; i++) {
+    for (std::size_t j = 0; j < 3; j++) {
+      in.assembly[i][j] = n["assembly"][3 * i + j];
+    }
+  }
   for (std::size_t i = 0; i < 3; i++) {
     in.drift[i] = n["drift"][i];
   }
@@ -352,6 +366,15 @@ void operator>>(const cv::FileNode &n, ImuIntrinsics &in) {
   for (std::size_t i = 0; i < 3; i++) {
     in.bias[i] = n["bias"][i];
   }
+  for (std::size_t i = 0; i < 2; i++) {
+    in.x[i] = n["x"][i];
+  }
+  for (std::size_t i = 0; i < 2; i++) {
+    in.y[i] = n["y"][i];
+  }
+  for (std::size_t i = 0; i < 2; i++) {
+    in.z[i] = n["z"][i];
+  }
 }
 
 void operator>>(const cv::FileNode &n, Extrinsics &ex) {