Merge branch 'develop' of http://gitlab.mynt.com/mynteye/mynt-eye-s-sdk into develop

src/mynteye/api/processor/points_processor.cc

@@ -105,6 +105,9 @@ bool PointsProcessor::OnProcess(
 
       // Missing points denoted by NaNs
       if (!DepthTraits<uint16_t>::valid(depth)) {
+        dptr[u][0] = 0;
+        dptr[u][1] = 0;
+        dptr[u][2] = 0;
         continue;
       }
       dptr[u][0] = (u - center_x) * depth * constant_x ;

src/mynteye/device/channel/channels.cc

@@ -475,7 +475,9 @@ bool Channels::GetFiles(
           if (file_size > 0) {
             auto &&n = file_channel_.GetImgParamsFromData(
                 data + i, file_size, img_params);
-            CHECK_EQ(n, file_size);
+            CHECK_EQ(n, file_size)
+              << "The firmware not support getting device info, you could "
+                 "upgrade to latest";
           }
         } break;
         case FID_IMU_PARAMS: {
@@ -483,7 +485,9 @@ bool Channels::GetFiles(
           if (imu_params->ok) {
             auto &&n = file_channel_.GetImuParamsFromData(
                 data + i, file_size, imu_params);
-            CHECK_EQ(n, file_size);
+            CHECK_EQ(n, file_size)
+              << "The firmware not support getting device info, you could "
+                 "upgrade to latest";
           }
         } break;
         default:

src/mynteye/device/motions.cc

@@ -169,7 +169,9 @@ Motions::motion_datas_t Motions::GetMotionDatas() {
 }
 
 void Motions::ProcImuAssembly(std::shared_ptr<ImuData> data) const {
-  if (nullptr == motion_intrinsics_) return;
+  if (nullptr == motion_intrinsics_ ||
+      IsNullAssemblyOrTempDrift(ProcessMode::PROC_IMU_ASSEMBLY))
+    return;
 
   double dst[3][3] = {0};
   if (data->flag == 1) {
@@ -200,7 +202,9 @@ void Motions::ProcImuAssembly(std::shared_ptr<ImuData> data) const {
 }
 
 void Motions::ProcImuTempDrift(std::shared_ptr<ImuData> data) const {
-  if (nullptr == motion_intrinsics_) return;
+  if (nullptr == motion_intrinsics_ ||
+      IsNullAssemblyOrTempDrift(ProcessMode::PROC_IMU_TEMP_DRIFT))
+    return;
 
   double temp = data->temperature;
   if (data->flag == 1) {
@@ -228,4 +232,36 @@ void Motions::EnableProcessMode(const std::int32_t& mode) {
   proc_mode_ = mode;
 }
 
+bool Motions::IsNullAssemblyOrTempDrift(const ProcessMode& mode) const {
+  if (mode == ProcessMode::PROC_IMU_ASSEMBLY) {
+    for (int i = 0; i < 3; i++) {
+      for (int j = 0; j < 3; j++) {
+        if (motion_intrinsics_->accel.assembly[i][j] != 0.0)
+          return false;
+      }
+    }
+    for (int i = 0; i < 3; i++) {
+      for (int j = 0; j < 3; j++) {
+        if (motion_intrinsics_->gyro.assembly[i][j] != 0.0)
+          return false;
+      }
+    }
+  } else if (mode == ProcessMode::PROC_IMU_TEMP_DRIFT) {
+    for (int i = 0; i < 2; i++) {
+      if (motion_intrinsics_->accel.x[i] != 0.0 ||
+          motion_intrinsics_->accel.y[i] != 0.0 ||
+          motion_intrinsics_->accel.z[i] != 0.0)
+        return false;
+    }
+    for (int i = 0; i < 2; i++) {
+      if (motion_intrinsics_->gyro.x[i] != 0 ||
+          motion_intrinsics_->gyro.y[i] != 0 ||
+          motion_intrinsics_->gyro.z[i] != 0)
+        return false;
+    }
+  }
+
+  return true;
+}
+
 MYNTEYE_END_NAMESPACE

src/mynteye/device/motions.h

@@ -52,6 +52,7 @@ class Motions {
  private:
   void ProcImuAssembly(std::shared_ptr<ImuData> data) const;
   void ProcImuTempDrift(std::shared_ptr<ImuData> data) const;
+  bool IsNullAssemblyOrTempDrift(const ProcessMode& mode) const;
 
   std::shared_ptr<Channels> channels_;