[Fix]: a variable type problem causes the gyro output to be zero

BOX/firmwares/Checksum.txt

@@ -0,0 +1,37 @@
+# Programs
+
+File: upgrader-setup.exe
+Size: 58.45 MB
+MD5: 3d015c2443fd726aaa8e3eee213bdba5
+
+# Firmwares
+
+## 2.0
+
+File: MYNTEYE_S_2.0.0_rc.img
+Size: 169.82 KB
+MD5: dad721f76837578610990b6501d65aa5
+
+File: MYNTEYE_S_2.0.0_rc2.img
+Size: 169.82 KB
+MD5: dad721f76837578610990b6501d65aa5
+
+File: MYNTEYE_S_2.0.0_rc1.img
+Size: 169.0 KB
+MD5: 9881ac1323bb2559fcbf774a4b32d72f
+
+File: MYNTEYE_S_2.0.0_rc0.img
+Size: 164.84 KB
+MD5: 1fe03c254d9377375d0b1e09416692ec
+
+File: MYNTEYE_S_2.0.0_alpha1.1.img
+Size: 164.84 KB
+MD5: 2285a3bf1767743a159cff423b0c2179
+
+File: MYNTEYE_S_2.0.0_alpha1.img
+Size: 164.95 KB
+MD5: 3e044f01d4d37fae9455095961e108ef
+
+File: MYNTEYE_S_2.0.0_alpha0.img
+Size: 163.96 KB
+MD5: d309872a44ebd14735d04f91323be01f

include/mynteye/types.h

@@ -625,7 +625,9 @@ struct MYNTEYE_API Extrinsics {
     return {{{rotation[0][0], rotation[1][0], rotation[2][0]},
              {rotation[0][1], rotation[1][1], rotation[2][1]},
              {rotation[0][2], rotation[1][2], rotation[2][2]}},
-            {-translation[0], -translation[1], -translation[2]}};
+            {-(rotation[0][0]*translation[0] + rotation[1][0]*translation[1] + rotation[2][0]*translation[2]),
+             -(rotation[0][1]*translation[0] + rotation[1][1]*translation[1] + rotation[2][1]*translation[2]),
+             -(rotation[0][2]*translation[0] + rotation[1][2]*translation[1] + rotation[2][2]*translation[2])}};
   }
 };
 

samples/camera_with_junior_device_api.cc

@@ -31,6 +31,8 @@ int main(int argc, char *argv[]) {
   auto &&request = device::select_request(device, &ok);
   if (!ok) return 1;
   device->ConfigStreamRequest(request);
+  device->EnableImuCorrespondence(true);
+  device->EnableProcessMode(ProcessMode::PROC_IMU_ASSEMBLY);
 
   std::size_t left_count = 0;
   device->SetStreamCallback(

samples/get_depth_with_region.cc

@@ -170,6 +170,7 @@ int main(int argc, char *argv[]) {
 
   api->SetOptionValue(Option::IR_CONTROL, 80);
 
+  api->SetDisparityComputingMethodType(DisparityComputingMethod::SGBM);
   api->EnableStreamData(Stream::DISPARITY_NORMALIZED);
   api->EnableStreamData(Stream::DEPTH);
 

samples/get_imu.cc

@@ -33,6 +33,7 @@ int main(int argc, char *argv[]) {
   api->EnableMotionDatas();
   // Enable imu timestamp correspondence int device;
   api->EnableImuTimestampCorrespondence(true);
+  api->EnableProcessMode(ProcessMode::PROC_IMU_ASSEMBLY);
 
   api->Start(Source::ALL);
 

src/mynteye/device/channel/channels.cc

@@ -45,9 +45,9 @@ mynteye::ImuPacket2 to_pak2(const mynteye::ImuPacket& pak1,
     tpr.accel[0] = pak1.segments[i].accel[0] * 1.f * accel_range / 0x10000;
     tpr.accel[1] = pak1.segments[i].accel[1] * 1.f * accel_range / 0x10000;
     tpr.accel[2] = pak1.segments[i].accel[2] * 1.f * accel_range / 0x10000;
-    tpr.gyro[0] = pak1.segments[i].gyro[0] * gyro_range / 0x10000;
-    tpr.gyro[1] = pak1.segments[i].gyro[1] * gyro_range / 0x10000;
-    tpr.gyro[2] = pak1.segments[i].gyro[2] * gyro_range / 0x10000;
+    tpr.gyro[0] = pak1.segments[i].gyro[0] * 1.f * gyro_range / 0x10000;
+    tpr.gyro[1] = pak1.segments[i].gyro[1] * 1.f * gyro_range / 0x10000;
+    tpr.gyro[2] = pak1.segments[i].gyro[2] * 1.f * gyro_range / 0x10000;
     tpr.flag = pak1.segments[i].flag;
     tpr.frame_id = pak1.segments[i].frame_id;
     tpr.is_ets = pak1.segments[i].is_ets;

src/mynteye/device/motions.cc

@@ -20,10 +20,14 @@ MYNTEYE_BEGIN_NAMESPACE
 
 namespace {
 
+#define RAD2DEG(rad) (rad/M_PI*180.)
+#define MPSS2G(mpss) (mpss/9.81)
+
 void matrix_3x1(const double (*src1)[3], const double (*src2)[1],
     double (*dst)[1]) {
   for (int i = 0; i < 3; i++) {
     for (int j = 0; j < 1; j++) {
+      dst[i][j] = 0.;
       for (int k = 0; k < 3; k++) {
         dst[i][j] += src1[i][k] * src2[k][j];
       }
@@ -35,6 +39,7 @@ void matrix_3x3(const double (*src1)[3], const double (*src2)[3],
     double (*dst)[3]) {
   for (int i = 0; i < 3; i++) {
     for (int j = 0; j < 3; j++) {
+      dst[i][j] = 0.;
       for (int k = 0; k < 3; k++) {
         dst[i][j] += src1[i][k] * src2[k][j];
       }
@@ -199,29 +204,74 @@ void Motions::ProcImuAssembly(std::shared_ptr<ImuData> data) const {
   if (nullptr == motion_intrinsics_ ||
       IsNullAssemblyOrTempDrift())
     return;
+  // use imu_tk to calibration the imu
+  // the param assembly is the Misalignment matrix: T
+  // the param scale is the scale matrix: K
+  // the param drift is the drift vector: B
+  // X' = T*K*(X - B)
+  static bool init = false;
+  static double TxK_accel[3][3] = {0};
+  static double TxK_gyro[3][3] = {0};
+  static double accel_drift[3] = {0};
+  static double gyro_drift[3] = {0};
+  if (!init) {
+    matrix_3x3(motion_intrinsics_->accel.assembly,
+               motion_intrinsics_->accel.scale, TxK_accel);
+    matrix_3x3(motion_intrinsics_->gyro.assembly,
+               motion_intrinsics_->gyro.scale, TxK_gyro);
+    for (int i = 0; i < 3; ++i) {
+      accel_drift[i] = MPSS2G(motion_intrinsics_->accel.drift[i]);
+      gyro_drift[i] = RAD2DEG(motion_intrinsics_->gyro.drift[i]);
+    }
 
-  double dst[3][3] = {0};
-  if (data->flag == 1) {
-    matrix_3x3(motion_intrinsics_->accel.scale,
-        motion_intrinsics_->accel.assembly, dst);
+//    LOG(INFO) << "-------------- accel T*K --------------" << std::endl;
+//    for (int i = 0; i < 3; ++i)
+//      LOG(INFO) << TxK_accel[i][0] << ", " << TxK_accel[i][1] << ", " << TxK_accel[i][2];
+//    LOG(INFO) << "-------------- gyro T*K --------------" << std::endl;
+//    for (int i = 0; i < 3; ++i)
+//      LOG(INFO) << TxK_gyro[i][0] << ", " << TxK_gyro[i][1] << ", " << TxK_gyro[i][2];
+//    LOG(INFO) << "-------------- accel bias --------------" << std::endl;
+//    LOG(INFO) << accel_drift[0] << ", " << accel_drift[1] << ", " << accel_drift[2];
+//    LOG(INFO) << "-------------- gyro bias --------------" << std::endl;
+//    LOG(INFO) << gyro_drift[0] << ", " << gyro_drift[1] << ", " << gyro_drift[2];
+
+    init = true;
+  }
+
+  if (data->flag == 0) {
+    double s[3][1] = {0};
+    double d[3][1] = {0};
+    for (int i = 0; i < 3; i++) {
+      s[i][0] = data->accel[i] - accel_drift[i];
+    }
+    matrix_3x1(TxK_accel, s, d);
+    for (int i = 0; i < 3; i++) {
+      data->accel[i] = d[i][0];
+    }
+    for (int i = 0; i < 3; i++) {
+      s[i][0] = data->gyro[i] - gyro_drift[i];
+    }
+    matrix_3x1(TxK_gyro, s, d);
+    for (int i = 0; i < 3; i++) {
+      data->gyro[i] = d[i][0];
+    }
+  } else if (data->flag == 1) {
     double s[3][1] = {0};
     double d[3][1] = {0};
     for (int i = 0; i < 3; i++) {
-      s[i][0] = data->accel[i];
+      s[i][0] = data->accel[i] - accel_drift[i];
     }
-    matrix_3x1(dst, s, d);
+    matrix_3x1(TxK_accel, s, d);
     for (int i = 0; i < 3; i++) {
       data->accel[i] = d[i][0];
     }
   } else if (data->flag == 2) {
-    matrix_3x3(motion_intrinsics_->gyro.scale,
-        motion_intrinsics_->gyro.assembly, dst);
     double s[3][1] = {0};
     double d[3][1] = {0};
     for (int i = 0; i < 3; i++) {
-      s[i][0] = data->gyro[i];
+      s[i][0] = data->gyro[i] - gyro_drift[i];
     }
-    matrix_3x1(dst, s, d);
+    matrix_3x1(TxK_gyro, s, d);
     for (int i = 0; i < 3; i++) {
       data->gyro[i] = d[i][0];
     }

wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard.yaml

@@ -56,3 +56,7 @@ standard/gyro_range: -1
 # index_s_1: 1
 
 standard/request_index: 0
+
+
+# 0 - none process, 1 - scale and assembly process, 2 - temperature process, 3 - both
+standard/imu_process_mode: 1

wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch

@@ -89,7 +89,7 @@
       <rosparam file="$(find mynt_eye_ros_wrapper)/config/mesh/mesh.yaml" command="load" />
 
       <param name="gravity" value="$(arg gravity)" />
-      <!-- <param name="ros_output_framerate_cut"     value="2" /> -->
+<!--      <param name="ros_output_framerate_cut" value="4" />-->
     </node>
 
     <!-- disable compressed depth plugin for image topics -->

wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc

@@ -1177,12 +1177,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
           break;
       }
     }
+
+    std::int32_t process_mode = 0;
     if (model_ == Model::STANDARD) {
       private_nh_.getParamCached("standard/request_index", request_index);
       frame_rate_ = api_->GetOptionValue(Option::FRAME_RATE);
+      private_nh_.getParamCached("standard/imu_process_mode", process_mode);
+      api_->EnableProcessMode(process_mode);
     }
-
-    std::int32_t process_mode = 0;
     if (model_ == Model::STANDARD2 ||
         model_ == Model::STANDARD210A || model_ == Model::STANDARD200B) {
       private_nh_.getParamCached("standard2/imu_process_mode", process_mode);