Unpack image data

include/mynteye/types.h

@@ -345,6 +345,21 @@ struct MYNTEYE_API ImgData {
     timestamp = 0;
     exposure_time = 0;
   }
+
+  ImgData() {
+    Reset();
+  }
+  ImgData(const ImgData &other) {
+    frame_id = other.frame_id;
+    timestamp = other.timestamp;
+    exposure_time = other.exposure_time;
+  }
+  ImgData &operator=(const ImgData &other) {
+    frame_id = other.frame_id;
+    timestamp = other.timestamp;
+    exposure_time = other.exposure_time;
+    return *this;
+  }
 };
 
 /**
@@ -370,6 +385,10 @@ struct MYNTEYE_API ImuData {
     std::fill(gyro, gyro + 3, 0);
     temperature = 0;
   }
+
+  ImuData() {
+    Reset();
+  }
 };
 
 MYNTEYE_END_NAMESPACE

samples/device/camera.cc

@@ -47,14 +47,22 @@ int main(int argc, char *argv[]) {
       Stream::LEFT, [&left_count](const device::StreamData &data) {
         ++left_count;
         VLOG(2) << Stream::LEFT << ", count: " << left_count;
-        UNUSED(data)
+        if (data.img) {
+          VLOG(2) << "  frame_id: " << data.img->frame_id
+                  << ", timestamp: " << data.img->timestamp
+                  << ", exposure_time: " << data.img->exposure_time;
+        }
       });
   std::size_t right_count = 0;
   device->SetStreamCallback(
       Stream::RIGHT, [&right_count](const device::StreamData &data) {
         ++right_count;
         VLOG(2) << Stream::RIGHT << ", count: " << right_count;
-        UNUSED(data)
+        if (data.img) {
+          VLOG(2) << "  frame_id: " << data.img->frame_id
+                  << ", timestamp: " << data.img->timestamp
+                  << ", exposure_time: " << data.img->exposure_time;
+        }
       });
 
   device->Start(Source::ALL);

src/device/device.cc

@@ -193,12 +193,14 @@ void Device::WaitForStreams() {
 std::vector<device::StreamData> Device::GetStreamDatas(const Stream &stream) {
   CHECK(video_streaming_);
   CHECK_NOTNULL(streams_);
+  std::lock_guard<std::mutex> _(mtx_streams_);
   return streams_->GetStreamDatas(stream);
 }
 
 device::StreamData Device::GetLatestStreamData(const Stream &stream) {
   CHECK(video_streaming_);
   CHECK_NOTNULL(streams_);
+  std::lock_guard<std::mutex> _(mtx_streams_);
   return streams_->GetLatestStreamData(stream);
 }
 
@@ -247,6 +249,7 @@ void Device::StartVideoStreaming() {
         *device_, stream_request.width, stream_request.height,
         static_cast<int>(stream_request.format), stream_request.fps,
         [this](const void *data) {
+          std::lock_guard<std::mutex> _(mtx_streams_);
           streams_->PushStream(Capabilities::STEREO, data);
           if (HasStreamCallback(Stream::LEFT)) {
             auto &&stream_data = streams_->stream_datas(Stream::LEFT).back();

src/device/device.h

@@ -4,6 +4,7 @@
 
 #include <map>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <vector>
 
@@ -112,6 +113,8 @@ class Device {
 
   std::map<Capabilities, StreamRequest> stream_config_requests_;
 
+  std::mutex mtx_streams_;
+
   void ReadDeviceInfo();
 
   void WriteImgIntrinsics(const ImgIntrinsics &intrinsics);

src/internal/streams.cc

@@ -4,6 +4,9 @@
 
 #include <algorithm>
 #include <chrono>
+#include <iomanip>
+
+#include "internal/types.h"
 
 MYNTEYE_BEGIN_NAMESPACE
 
@@ -11,9 +14,60 @@ namespace {
 
 void unpack_stereo_img_data(
     const void *data, const StreamRequest &request, ImgData &img) {  // NOLINT
-  UNUSED(data)
-  UNUSED(request)
-  UNUSED(img)
+  CHECK_EQ(request.format, Format::YUYV);
+
+  auto data_new = reinterpret_cast<const std::uint8_t *>(data);
+  std::size_t data_n =
+      request.width * request.height * bytes_per_pixel(request.format);
+  auto data_end = data_new + data_n;
+
+  // LOG(INFO) << "ImagePacket (raw): header=0x" << std::hex <<
+  // static_cast<int>(*(data_end - 1))
+  //   << ", size=0x" << std::hex << static_cast<int>(*(data_end - 2))
+  //   << ", frame_id="<< std::dec << ((*(data_end - 3) << 8) + *(data_end - 4))
+  //   << ", timestamp="<< std::dec << ((*(data_end - 5) << 24) + (*(data_end -
+  //   6) << 16) + (*(data_end - 7) << 8) + *(data_end - 8))
+  //   << ", exposure_time="<< std::dec << ((*(data_end - 9) << 8) + *(data_end
+  //   - 10))
+  //   << ", checksum=0x" << std::hex << static_cast<int>(*(data_end - 11));
+
+  std::size_t packet_n = sizeof(ImagePacket);
+  // LOG(INFO) << "ImagePacket Size: " << packet_n;
+  std::vector<std::uint8_t> packet(packet_n);
+  std::reverse_copy(data_end - packet_n, data_end, packet.begin());
+
+  ImagePacket img_packet(packet.data());
+  // LOG(INFO) << "ImagePacket (new): header=0x" << std::hex <<
+  // static_cast<int>(img_packet.header)
+  //   << ", size=0x" << std::hex << static_cast<int>(img_packet.size)
+  //   << ", frame_id="<< std::dec << img_packet.frame_id
+  //   << ", timestamp="<< std::dec << img_packet.timestamp
+  //   << ", exposure_time="<< std::dec << img_packet.exposure_time
+  //   << ", checksum=0x" << std::hex << static_cast<int>(img_packet.checksum);
+
+  if (img_packet.header != 0x3B) {
+    LOG(WARNING) << "Image packet header must be 0x3B, but 0x" << std::hex
+                 << std::uppercase << std::setw(2) << std::setfill('0')
+                 << static_cast<int>(img_packet.header) << " now";
+    return;
+  }
+
+  std::uint8_t checksum = 0;
+  for (std::size_t i = 1, n = packet_n - 2; i <= n; i++) {
+    checksum = (checksum ^ packet[i]);
+  }
+  if (checksum != img_packet.checksum) {
+    LOG(WARNING) << "Image packet checksum should be 0x" << std::hex
+                 << std::uppercase << std::setw(2) << std::setfill('0')
+                 << static_cast<int>(img_packet.checksum) << ", but 0x"
+                 << std::setw(2) << std::setfill('0')
+                 << static_cast<int>(img_packet.checksum) << " now";
+    return;
+  }
+
+  img.frame_id = img_packet.frame_id;
+  img.timestamp = img_packet.timestamp;
+  img.exposure_time = img_packet.exposure_time;
 }
 
 void unpack_left_img_pixels(
@@ -85,7 +139,7 @@ void Streams::PushStream(const Capabilities &capability, const void *data) {
       auto &&right_data = stream_datas_map_[Stream::RIGHT].back();
       // unpack img data
       unpack_img_data_map_[Stream::LEFT](data, request, *left_data.img);
-      right_data.img = left_data.img;
+      *right_data.img = *left_data.img;
       // unpack frame
       unpack_img_pixels_map_[Stream::LEFT](data, request, *left_data.frame);
       unpack_img_pixels_map_[Stream::RIGHT](data, request, *right_data.frame);
@@ -106,12 +160,12 @@ void Streams::WaitForStreams() {
 }
 
 Streams::stream_datas_t Streams::GetStreamDatas(const Stream &stream) {
+  std::unique_lock<std::mutex> lock(mtx_);
   if (!HasStreamDatas(stream) || stream_datas_map_.at(stream).empty()) {
     LOG(WARNING) << "There are stream datas of " << stream
                  << ", do you first call WaitForStreams?";
     return {};
   }
-  std::unique_lock<std::mutex> lock(mtx_);
   stream_datas_t datas = stream_datas_map_.at(stream);
   stream_datas_map_[stream].clear();
   return datas;
@@ -155,7 +209,11 @@ void Streams::AllocStreamData(
     const Stream &stream, const StreamRequest &request, const Format &format) {
   static std::size_t stream_data_limits_max = 4;
   stream_data_t data;
-  data.img = std::shared_ptr<ImgData>(new ImgData{0, 0, 0});
+  if (stream == Stream::LEFT || stream == Stream::RIGHT) {
+    data.img = std::make_shared<ImgData>();
+  } else {
+    data.img = nullptr;
+  }
   data.frame =
       std::make_shared<frame_t>(request.width, request.height, format, nullptr);
   stream_datas_map_[stream].push_back(data);

src/internal/types.h

@@ -125,6 +125,32 @@ struct DeviceInfo {
   std::uint16_t nominal_baseline;
 };
 
+/**
+ * @ingroup datatypes
+ * Image packet.
+ */
+#pragma pack(push, 1)
+struct ImagePacket {
+  std::uint8_t header;
+  std::uint8_t size;
+  std::uint16_t frame_id;
+  std::uint32_t timestamp;
+  std::uint16_t exposure_time;
+  std::uint8_t checksum;
+
+  ImagePacket() = default;
+  explicit ImagePacket(std::uint8_t *data) {
+    header = *data;
+    size = *(data + 1);
+    frame_id = (*(data + 2) << 8) + *(data + 3);
+    timestamp = (*(data + 4) << 24) + (*(data + 5) << 16) + (*(data + 6) << 8) +
+                *(data + 7);
+    exposure_time = (*(data + 8) << 8) + *(data + 9);
+    checksum = *(data + 10);
+  }
+};
+#pragma pack(pop)
+
 #undef MYNTEYE_PROPERTY
 
 MYNTEYE_END_NAMESPACE