From d2b4cc3022b491db3aaa8bb835f9865367fe08fb Mon Sep 17 00:00:00 2001
From: Kalman <kalmanxiong@slightech.com>
Date: Wed, 1 Aug 2018 20:20:39 +0800
Subject: [PATCH] fix two warning

---
 src/internal/channels.cc           | 40 ++++++++++++++++--------------
 src/internal/types.h               | 33 ++++++++++++++----------
 tools/analytics/stamp_analytics.py |  4 +--
 tools/dataset/dataset.cc           | 18 ++++++++------
 tools/dataset/dataset.h            |  2 ++
 tools/internal/data/__init__.py    | 12 ++++-----
 6 files changed, 63 insertions(+), 46 deletions(-)

diff --git a/src/internal/channels.cc b/src/internal/channels.cc
index f4a065a..58c6668 100644
--- a/src/internal/channels.cc
+++ b/src/internal/channels.cc
@@ -128,25 +128,29 @@ void Channels::LogControlInfos() const {
 }
 
 void Channels::UpdateControlInfos() {
-  for (auto &&option : std::vector<Option>{Option::GAIN, Option::BRIGHTNESS,
-                                           Option::CONTRAST}) {
-    // control_infos_[option] = PuControlInfo(option);
-  }
-
-  for (auto &&option : std::vector<Option>{
-           Option::FRAME_RATE, Option::IMU_FREQUENCY, Option::EXPOSURE_MODE,
-           Option::MAX_GAIN, Option::MAX_EXPOSURE_TIME,
-           Option::DESIRED_BRIGHTNESS, Option::IR_CONTROL, Option::HDR_MODE}) {
-    // control_infos_[option] = XuControlInfo(option);
-  }
-
-  if (VLOG_IS_ON(2)) {
-    for (auto &&it = control_infos_.begin(); it != control_infos_.end(); it++) {
-      VLOG(2) << it->first << ": min=" << it->second.min
-              << ", max=" << it->second.max << ", def=" << it->second.def
-              << ", cur=" << GetControlValue(it->first);
+  /*
+    for (auto &&option : std::vector<Option>{Option::GAIN, Option::BRIGHTNESS,
+                                             Option::CONTRAST}) {
+       control_infos_[option] = PuControlInfo(option);
     }
-  }
+
+    for (auto &&option : std::vector<Option>{
+             Option::FRAME_RATE, Option::IMU_FREQUENCY, Option::EXPOSURE_MODE,
+             Option::MAX_GAIN, Option::MAX_EXPOSURE_TIME,
+             Option::DESIRED_BRIGHTNESS, Option::IR_CONTROL, Option::HDR_MODE})
+    {
+       control_infos_[option] = XuControlInfo(option);
+    }
+
+    if (VLOG_IS_ON(2)) {
+      for (auto &&it = control_infos_.begin(); it != control_infos_.end(); it++)
+    {
+        VLOG(2) << it->first << ": min=" << it->second.min
+                << ", max=" << it->second.max << ", def=" << it->second.def
+                << ", cur=" << GetControlValue(it->first);
+      }
+    }
+  */
 }
 
 Channels::control_info_t Channels::GetControlInfo(const Option &option) const {
diff --git a/src/internal/types.h b/src/internal/types.h
index 69d7d59..a34f1ad 100644
--- a/src/internal/types.h
+++ b/src/internal/types.h
@@ -16,7 +16,6 @@
 #pragma once
 
 #include <cstdint>
-
 #include <array>
 #include <bitset>
 #include <string>
@@ -158,12 +157,17 @@ struct ImagePacket {
   }
 
   void from_data(std::uint8_t *data) {
+    std::uint32_t timestamp_l;
+    std::uint32_t timestamp_h;
+
     header = *data;
     size = *(data + 1);
     frame_id = (*(data + 2) << 8) | *(data + 3);
-    timestamp = (*(data + 4) << 56) | (*(data + 5) << 48) | (*(data + 6) << 40) |
-                (*(data + 7) << 32) | (*(data + 8) << 24) | (*(data + 9) << 16) |
-                (*(data + 10) << 8) | *(data + 11);
+    timestamp_h = (*(data + 4) << 24) | (*(data + 5) << 16) |
+                  (*(data + 6) << 8) | *(data + 7);
+    timestamp_l = (*(data + 8) << 24) | (*(data + 9) << 16) |
+                  (*(data + 10) << 8) | *(data + 11);
+    timestamp = (static_cast<std::uint64_t>(timestamp_h) << 32) | timestamp_l;
     exposure_time = (*(data + 12) << 8) | *(data + 13);
     checksum = *(data + 14);
   }
@@ -212,11 +216,16 @@ struct ImuSegment {
   }
 
   void from_data(std::uint8_t *data) {
+    std::uint32_t timestamp_l;
+    std::uint32_t timestamp_h;
+
     serial_number = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) |
                     *(data + 3);
-    timestamp = (*(data + 4) << 56) | (*(data + 5) << 48) | (*(data + 6) << 40) |
-                (*(data + 7) << 32) | (*(data + 8) << 24) | (*(data + 9) << 16) |
-                (*(data + 10) << 8) | *(data + 11);
+    timestamp_h = (*(data + 4) << 24) | (*(data + 5) << 16) |
+                  (*(data + 6) << 8) | *(data + 7);
+    timestamp_l = (*(data + 8) << 24) | (*(data + 9) << 16) |
+                  (*(data + 10) << 8) | *(data + 11);
+    timestamp = (static_cast<std::uint64_t>(timestamp_h) << 32) | timestamp_l;
     flag = *(data + 12);
     temperature = (*(data + 13) << 8) | *(data + 14);
     aceel_or_gyro[0] = (*(data + 15) << 8) | *(data + 16);
@@ -236,15 +245,13 @@ struct ImuPacket {
   std::vector<ImuSegment> segments;
 
   ImuPacket() = default;
-  
-  explicit ImuPacket(std::uint8_t seg_count,std::uint8_t *data) {
+  explicit ImuPacket(std::uint8_t seg_count, std::uint8_t *data) {
     count = seg_count;
     from_data(data);
   }
-  
   void from_data(std::uint8_t *data) {
         std::size_t seg_n = sizeof(ImuSegment);  // 21
-        for(std::size_t i = 0; i < count; i++) {
+        for (std::size_t i = 0; i < count; i++) {
           segments.push_back(ImuSegment(data + seg_n * i));
         }
   }
@@ -275,9 +282,9 @@ struct ImuResPacket {
 
     std::size_t seg_n = sizeof(ImuSegment);  // 21
     std::uint8_t seg_count = size / seg_n;
-    ImuPacket packet(seg_count,data + 4);
+    ImuPacket packet(seg_count, data + 4);
     packets.push_back(packet);
-    //packet(2);
+    // packet(2);
     checksum = *(data + 4 + size);
   }
 };
diff --git a/tools/analytics/stamp_analytics.py b/tools/analytics/stamp_analytics.py
index 3814126..908e518 100644
--- a/tools/analytics/stamp_analytics.py
+++ b/tools/analytics/stamp_analytics.py
@@ -288,14 +288,14 @@ def _parse_args():
       '--rate-img',
       dest='rate_img',
       metavar='RATE',
-      default=25,
+      default=60,
       type=int,
       help='the img rate (default: %(default)s)')
   parser.add_argument(
       '--rate-imu',
       dest='rate_imu',
       metavar='RATE',
-      default=500,
+      default=200,
       type=int,
       help='the imu rate (default: %(default)s)')
   return parser.parse_args()
diff --git a/tools/dataset/dataset.cc b/tools/dataset/dataset.cc
index c87d964..ef82e14 100644
--- a/tools/dataset/dataset.cc
+++ b/tools/dataset/dataset.cc
@@ -75,12 +75,14 @@ void Dataset::SaveStreamData(
 void Dataset::SaveMotionData(const device::MotionData &data) {
   auto &&writer = GetMotionWriter();
   auto seq = motion_count_;
-  writer->ofs << seq << ", " << data.imu->timestamp << ", "
-              << data.imu->accel[0] << ", " << data.imu->accel[1] << ", "
-              << data.imu->accel[2] << ", " << data.imu->gyro[0] << ", "
-              << data.imu->gyro[1] << ", " <<  data.imu->gyro[2] << ", "
-              << data.imu->temperature << std::endl;           
-  ++motion_count_;
+  if (data.imu->flag == 1 || data.imu->flag == 2) {
+    writer->ofs << seq << ", " << data.imu->timestamp << ", "
+                << data.imu->accel[0] << ", " << data.imu->accel[1] << ", "
+                << data.imu->accel[2] << ", " << data.imu->gyro[0] << ", "
+                << data.imu->gyro[1] << ", " << data.imu->gyro[2] << ", "
+                << data.imu->temperature << std::endl;
+    ++motion_count_;
+  }
 }
 
 Dataset::writer_t Dataset::GetStreamWriter(const Stream &stream) {
@@ -102,7 +104,7 @@ Dataset::writer_t Dataset::GetStreamWriter(const Stream &stream) {
 
     files::mkdir(writer->outdir);
     writer->ofs.open(writer->outfile, std::ofstream::out);
-    writer->ofs << "seq, timestamp, exposure_time" << std::endl;
+    writer->ofs << "seq, frame_id, timestamp, exposure_time" << std::endl;
     writer->ofs << FULL_PRECISION;
 
     stream_writers_[stream] = writer;
@@ -126,6 +128,8 @@ Dataset::writer_t Dataset::GetMotionWriter() {
 
     motion_writer_ = writer;
     motion_count_ = 0;
+    accel_count_ = 0;
+    gyro_count_ = 0;
   }
   return motion_writer_;
 }
diff --git a/tools/dataset/dataset.h b/tools/dataset/dataset.h
index 06f52c4..485adb0 100644
--- a/tools/dataset/dataset.h
+++ b/tools/dataset/dataset.h
@@ -54,6 +54,8 @@ class Dataset {
 
   std::map<Stream, std::size_t> stream_counts_;
   std::size_t motion_count_;
+  std::size_t accel_count_;
+  std::size_t gyro_count_;
 };
 
 }  // namespace tools
diff --git a/tools/internal/data/__init__.py b/tools/internal/data/__init__.py
index 5c3a0dc..e5fd610 100644
--- a/tools/internal/data/__init__.py
+++ b/tools/internal/data/__init__.py
@@ -339,9 +339,9 @@ class MYNTEYE(Dataset):
       if index == -1:
         sys.exit('Error: Dataset is unexpected format, timestamp not found')
 
-      # unit from 0.01ms to 1s
-      info.timebeg = float(first.split(',')[index].strip()) * 0.00001
-      info.timeend = float(last.split(',')[index].strip()) * 0.00001
+      # unit from 1us to 1s
+      info.timebeg = float(first.split(',')[index].strip()) * 0.000001
+      info.timeend = float(last.split(',')[index].strip()) * 0.000001
       # print('time: [{}, {}]'.format(info.timebeg, info.timeend))
 
     return info
@@ -364,7 +364,7 @@ class MYNTEYE(Dataset):
         for line in f:
           values = [_.strip() for _ in line.split(',')]
           img = Image()
-          img.timestamp = float(values[fields['timestamp']]) * 0.00001
+          img.timestamp = float(values[fields['timestamp']]) * 0.000001
           yield {What.img_left: img}
     if hit_img_right and self._info.has_img_right:
       with open(self._info.img_right_txt) as f:
@@ -372,7 +372,7 @@ class MYNTEYE(Dataset):
         for line in f:
           values = [_.strip() for _ in line.split(',')]
           img = Image()
-          img.timestamp = float(values[fields['timestamp']]) * 0.00001
+          img.timestamp = float(values[fields['timestamp']]) * 0.000001
           yield {What.img_right: img}
     if (hit_imu or hit_temp) and self._info.has_imu:
       with open(self._info.imu_txt) as f:
@@ -380,7 +380,7 @@ class MYNTEYE(Dataset):
         for line in f:
           values = [_.strip() for _ in line.split(',')]
           imu = IMU()
-          imu.timestamp = float(values[fields['timestamp']]) * 0.00001
+          imu.timestamp = float(values[fields['timestamp']]) * 0.000001
           imu.accel_x = float(values[fields['accel_x']])
           imu.accel_y = float(values[fields['accel_y']])
           imu.accel_z = float(values[fields['accel_z']])