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

CMakeLists.txt

@@ -94,6 +94,18 @@ if(OS_WIN)
   )
 endif()
 
+# rpath
+
+set(CMAKE_MACOSX_RPATH 1)
+set(MYNTEYE_CMAKE_RPATH "")
+if(WITH_OPENCV)
+  list(APPEND MYNTEYE_CMAKE_RPATH ${OpenCV_LIB_PATH})
+endif()
+if(MYNTEYE_CMAKE_RPATH)
+  message(STATUS "RPATH: ${MYNTEYE_CMAKE_RPATH}")
+  set(CMAKE_INSTALL_RPATH "${MYNTEYE_CMAKE_RPATH}")
+endif()
+
 # targets
 
 add_definitions(-DMYNTEYE_EXPORTS)
@@ -297,6 +309,7 @@ install(FILES
   ${CMAKE_CURRENT_SOURCE_DIR}/include/mynteye/device/callbacks.h
   ${CMAKE_CURRENT_SOURCE_DIR}/include/mynteye/device/context.h
   ${CMAKE_CURRENT_SOURCE_DIR}/include/mynteye/device/device.h
+  ${CMAKE_CURRENT_SOURCE_DIR}/include/mynteye/device/types.h
   ${CMAKE_CURRENT_SOURCE_DIR}/include/mynteye/device/utils.h
   DESTINATION ${MYNTEYE_CMAKE_INCLUDE_DIR}/device
 )

cmake/DetectOpenCV.cmake

@@ -41,6 +41,25 @@ if(${__index} GREATER -1)
   set(WITH_OPENCV_WORLD TRUE)
 endif()
 
+if(NOT OpenCV_LIB_PATH)
+  list(LENGTH OpenCV_INCLUDE_DIRS __length)
+  if(${__length} GREATER 0)
+    list(GET OpenCV_INCLUDE_DIRS 0 __include_dir)
+    string(REGEX REPLACE "include.*$" "lib" __lib_dir "${__include_dir}")
+    find_library(__opencv_lib
+      NAMES opencv_core3 opencv_core opencv_world
+      PATHS "${__lib_dir}" "${__lib_dir}/x86_64-linux-gnu"
+      NO_DEFAULT_PATH)
+    #message(STATUS "__opencv_lib: ${__opencv_lib}")
+    if(__opencv_lib)
+      get_filename_component(OpenCV_LIB_PATH "${__opencv_lib}" DIRECTORY)
+    else()
+      set(OpenCV_LIB_PATH "${__lib_dir}")
+    endif()
+    #message(STATUS "OpenCV_LIB_PATH: ${OpenCV_LIB_PATH}")
+  endif()
+endif()
+
 if(MSVC OR MSYS OR MINGW)
   get_filename_component(OpenCV_LIB_SEARCH_PATH "${OpenCV_LIB_PATH}/../bin" ABSOLUTE)
 else()

include/mynteye/device/device.h

@@ -22,8 +22,8 @@
 #include <vector>
 
 #include "mynteye/mynteye.h"
-#include "mynteye/types.h"
 #include "mynteye/device/callbacks.h"
+#include "mynteye/device/types.h"
 
 MYNTEYE_BEGIN_NAMESPACE
 
@@ -51,26 +51,6 @@ class StreamsAdapter;
 template <class Data>
 class AsyncCallback;
 
-namespace device {
-
-typedef struct ImgParams {
-  bool ok;
-  std::string version;
-  std::shared_ptr<IntrinsicsBase> in_left;
-  std::shared_ptr<IntrinsicsBase> in_right;
-  Extrinsics ex_right_to_left;
-} img_params_t;
-
-typedef struct ImuParams {
-  bool ok;
-  std::string version;
-  ImuIntrinsics in_accel;
-  ImuIntrinsics in_gyro;
-  Extrinsics ex_left_to_imu;
-} imu_params_t;
-
-}  // namespace device
-
 /**
  * The Device class to communicate with MYNT® EYE device.
  */
@@ -89,6 +69,7 @@ class MYNTEYE_API Device {
   using motion_async_callback_ptr_t = std::shared_ptr<motion_async_callback_t>;
 
   using img_params_t = device::img_params_t;
+  using img_params_map_t = std::map<Resolution, img_params_t>;
   using imu_params_t = device::imu_params_t;
 
  protected:
@@ -337,10 +318,10 @@ class MYNTEYE_API Device {
 
   virtual Capabilities GetKeyStreamCapability() const = 0;
 
-  std::map<Resolution, device::img_params_t> GetImgParams() const {
+  img_params_map_t GetImgParams() const {
     return all_img_params_;
   }
-  device::imu_params_t GetImuParams() const {
+  imu_params_t GetImuParams() const {
     return imu_params_;
   }
 
@@ -352,8 +333,8 @@ class MYNTEYE_API Device {
   std::shared_ptr<uvc::device> device_;
   std::shared_ptr<DeviceInfo> device_info_;
 
-  std::map<Resolution, device::img_params_t> all_img_params_;
+  img_params_map_t all_img_params_;
-  device::imu_params_t imu_params_;
+  imu_params_t imu_params_;
 
   std::map<Stream, std::shared_ptr<IntrinsicsBase>> stream_intrinsics_;
   std::map<Stream, std::map<Stream, Extrinsics>> stream_from_extrinsics_;
@@ -384,6 +365,11 @@ class MYNTEYE_API Device {
   void CallbackPushedStreamData(const Stream &stream);
   void CallbackMotionData(const device::MotionData &data);
 
+  bool GetFiles(
+      DeviceInfo *info, img_params_map_t *img_params, imu_params_t *imu_params);
+  bool SetFiles(
+      DeviceInfo *info, img_params_map_t *img_params, imu_params_t *imu_params);
+
   friend API;
   friend tools::DeviceWriter;
 };

include/mynteye/device/types.h

@@ -18,13 +18,35 @@
 #include <cstdint>
 #include <array>
 #include <bitset>
+#include <memory>
 #include <string>
 #include <vector>
 
 #include "mynteye/mynteye.h"
+#include "mynteye/types.h"
 
 MYNTEYE_BEGIN_NAMESPACE
 
+namespace device {
+
+typedef struct ImgParams {
+  bool ok;
+  std::string version;
+  std::shared_ptr<IntrinsicsBase> in_left;
+  std::shared_ptr<IntrinsicsBase> in_right;
+  Extrinsics ex_right_to_left;
+} img_params_t;
+
+typedef struct ImuParams {
+  bool ok;
+  std::string version;
+  ImuIntrinsics in_accel;
+  ImuIntrinsics in_gyro;
+  Extrinsics ex_left_to_imu;
+} imu_params_t;
+
+}  // namespace device
+
 #define MYNTEYE_PROPERTY(TYPE, NAME) \
  public:                             \
   void set_##NAME(TYPE NAME) {       \
@@ -142,73 +164,6 @@ struct MYNTEYE_API DeviceInfo {
   std::uint16_t nominal_baseline;
 };
 
-/**
- * @ingroup datatypes
- * Imu request packet.
- */
-#pragma pack(push, 1)
-struct ImuReqPacket {
-  std::uint8_t header;
-  std::uint32_t serial_number;
-
-  ImuReqPacket() = default;
-  explicit ImuReqPacket(std::uint32_t serial_number)
-      : ImuReqPacket(0x5A, serial_number) {}
-  ImuReqPacket(std::uint8_t header, std::uint32_t serial_number)
-      : header(header), serial_number(serial_number) {}
-
-  std::array<std::uint8_t, 5> to_data() const {
-    return {{header, static_cast<std::uint8_t>((serial_number >> 24) & 0xFF),
-             static_cast<std::uint8_t>((serial_number >> 16) & 0xFF),
-             static_cast<std::uint8_t>((serial_number >> 8) & 0xFF),
-             static_cast<std::uint8_t>(serial_number & 0xFF)}};
-  }
-};
-#pragma pack(pop)
-
-/**
- * @ingroup datatypes
- * Imu segment.
- */
-#pragma pack(push, 1)
-struct ImuSegment {
-  std::uint32_t frame_id;
-  std::uint64_t timestamp;
-  std::uint8_t flag;
-  std::int16_t temperature;
-  std::int16_t accel[3];
-  std::int16_t gyro[3];
-};
-#pragma pack(pop)
-
-/**
- * @ingroup datatypes
- * Imu packet.
- */
-#pragma pack(push, 1)
-struct ImuPacket {
-  std::uint8_t version;
-  std::uint8_t count;
-  std::uint32_t serial_number;
-  std::vector<ImuSegment> segments;
-};
-#pragma pack(pop)
-
-/**
- * @ingroup datatypes
- * Imu response packet.
- */
-#pragma pack(push, 1)
-struct ImuResPacket {
-  std::uint8_t version;
-  std::uint8_t header;
-  std::uint8_t state;
-  std::uint16_t size;
-  std::vector<ImuPacket> packets;
-  std::uint8_t checksum;
-};
-#pragma pack(pop)
-
 #undef MYNTEYE_PROPERTY
 
 MYNTEYE_END_NAMESPACE

src/mynteye/api/processor/points_processor.cc

@@ -14,6 +14,8 @@
 #include "mynteye/api/processor/points_processor.h"
 
 #include <utility>
+#include <vector>
+#include <limits>
 
 #include <opencv2/calib3d/calib3d.hpp>
 
@@ -21,6 +23,33 @@
 
 MYNTEYE_BEGIN_NAMESPACE
 
+namespace {
+// Encapsulate differences between processing float and uint16_t depths
+template<typename T> struct DepthTraits {};
+
+template<>
+struct DepthTraits<uint16_t> {
+  static inline bool valid(uint16_t depth) { return depth != 0; }
+  static inline float toMeters(uint16_t depth) { return depth * 0.001f; } // originally mm
+  static inline uint16_t fromMeters(float depth) { return (depth * 1000.0f) + 0.5f; }
+  static inline void initializeBuffer(std::vector<uint8_t>& buffer) {} // Do nothing - already zero-filled
+};
+
+template<>
+struct DepthTraits<float> {
+  static inline bool valid(float depth) { return std::isfinite(depth); }
+  static inline float toMeters(float depth) { return depth; }
+  static inline float fromMeters(float depth) { return depth; }
+
+  static inline void initializeBuffer(std::vector<uint8_t>& buffer) {
+    float* start = reinterpret_cast<float*>(&buffer[0]);
+    float* end = reinterpret_cast<float*>(&buffer[0] + buffer.size());
+    std::fill(start, end, std::numeric_limits<float>::quiet_NaN());
+  }
+};
+
+}; // namespace
+
 const char PointsProcessor::NAME[] = "PointsProcessor";
 
 PointsProcessor::PointsProcessor(std::int32_t proc_period)
@@ -42,6 +71,46 @@ Object *PointsProcessor::OnCreateOutput() {
 
 bool PointsProcessor::OnProcess(
   Object *const in, Object *const out, Processor *const parent) {
+  MYNTEYE_UNUSED(parent)
+
+  float fx = 3.6797709792391299e+02;
+  float fy = 3.6808712539453859e+02;
+  float cx = 3.7414963027144353e+02;
+  float cy = 2.3125000326472903e+02;
+
+  // Use correct principal point from calibration
+  float center_x = cx;
+  float center_y = cy;
+
+  // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
+  double unit_scaling = DepthTraits<float>::toMeters(static_cast<float>(1));
+  float constant_x = unit_scaling / fx;
+  float constant_y = unit_scaling / fy;
+  // float bad_point = std::numeric_limits<float>::quiet_NaN();
+
+  const ObjMat *input = Object::Cast<ObjMat>(in);
+  ObjMat *output = Object::Cast<ObjMat>(out);
+  output->value.create(input->value.size(), CV_MAKETYPE(CV_32F, 3));
+
+  int height = static_cast<int>(output->value.rows);
+  int width = static_cast<int>(output->value.cols);
+  for (int v = 0; v < height; ++v) {
+
+    cv::Vec3f *dptr = output->value.ptr<cv::Vec3f>(v);
+    for (int u = 0; u < width; ++u) {
+      float depth = input->value.at<float>(v, u);
+
+      // Missing points denoted by NaNs
+      if (!DepthTraits<float>::valid(depth)) {
+        continue;
+      }
+
+      dptr[u][0] = (u - center_x) * depth * constant_x;
+      dptr[u][1] = (v - center_y) * depth * constant_y;
+      dptr[u][2] = DepthTraits<float>::toMeters(depth);
+    }
+  }
+
   return true;
 }
 

src/mynteye/device/channel/def.h

@@ -15,6 +15,10 @@
 #define MYNTEYE_DEVICE_CHANNEL_DEF_H_
 #pragma once
 
+#include <array>
+#include <cstdint>
+#include <vector>
+
 #include "mynteye/mynteye.h"
 
 MYNTEYE_BEGIN_NAMESPACE
@@ -35,6 +39,73 @@ typedef enum FileId {
   FID_LAST,
 } file_id_t;
 
+/**
+ * @ingroup datatypes
+ * Imu request packet.
+ */
+#pragma pack(push, 1)
+struct ImuReqPacket {
+  std::uint8_t header;
+  std::uint32_t serial_number;
+
+  ImuReqPacket() = default;
+  explicit ImuReqPacket(std::uint32_t serial_number)
+      : ImuReqPacket(0x5A, serial_number) {}
+  ImuReqPacket(std::uint8_t header, std::uint32_t serial_number)
+      : header(header), serial_number(serial_number) {}
+
+  std::array<std::uint8_t, 5> to_data() const {
+    return {{header, static_cast<std::uint8_t>((serial_number >> 24) & 0xFF),
+             static_cast<std::uint8_t>((serial_number >> 16) & 0xFF),
+             static_cast<std::uint8_t>((serial_number >> 8) & 0xFF),
+             static_cast<std::uint8_t>(serial_number & 0xFF)}};
+  }
+};
+#pragma pack(pop)
+
+/**
+ * @ingroup datatypes
+ * Imu segment.
+ */
+#pragma pack(push, 1)
+struct ImuSegment {
+  std::uint32_t frame_id;
+  std::uint64_t timestamp;
+  std::uint8_t flag;
+  std::int16_t temperature;
+  std::int16_t accel[3];
+  std::int16_t gyro[3];
+};
+#pragma pack(pop)
+
+/**
+ * @ingroup datatypes
+ * Imu packet.
+ */
+#pragma pack(push, 1)
+struct ImuPacket {
+  std::uint8_t version;
+  std::uint8_t count;
+  std::uint32_t serial_number;
+  std::vector<ImuSegment> segments;
+};
+#pragma pack(pop)
+
+/**
+ * @ingroup datatypes
+ * Imu response packet.
+ */
+#pragma pack(push, 1)
+struct ImuResPacket {
+  std::uint8_t version;
+  std::uint8_t header;
+  std::uint8_t state;
+  std::uint16_t size;
+  std::vector<ImuPacket> packets;
+  std::uint8_t checksum;
+};
+#pragma pack(pop)
+
 MYNTEYE_END_NAMESPACE
 
 #endif  // MYNTEYE_DEVICE_CHANNEL_DEF_H_

src/mynteye/device/device.cc

@@ -676,4 +676,14 @@ void Device::CallbackMotionData(const device::MotionData &data) {
   }
 }
 
+bool Device::GetFiles(
+    DeviceInfo *info, img_params_map_t *img_params, imu_params_t *imu_params) {
+  return channels_->GetFiles(info, img_params, imu_params);
+}
+
+bool Device::SetFiles(
+    DeviceInfo *info, img_params_map_t *img_params, imu_params_t *imu_params) {
+  return channels_->SetFiles(info, img_params, imu_params);
+}
+
 MYNTEYE_END_NAMESPACE

tools/writer/CMakeLists.txt

@@ -20,10 +20,6 @@ set_outdir(
   "${OUT_DIR}/bin/${DIR_NAME}"
 )
 
-include_directories(
-  ${PRO_DIR}/src
-)
-
 ## device_writer
 
 add_library(device_writer STATIC device_writer.cc)

tools/writer/device_writer.cc

@@ -18,7 +18,6 @@
 #include <opencv2/core/core.hpp>
 
 #include "mynteye/logger.h"
-#include "mynteye/device/device.h"
 #include "mynteye/util/files.h"
 
 #define SAVE_LATEST_VERSION Version(1, 2)
@@ -36,15 +35,13 @@ DeviceWriter::~DeviceWriter() {
 }
 
 bool DeviceWriter::WriteDeviceInfo(const dev_info_t &info) {
-  auto &&channels = device_->channels();
-
   // Update device info
   auto &&dev_info = device_->GetInfo();
   dev_info->lens_type = Type(info.lens_type);
   dev_info->imu_type = Type(info.imu_type);
   dev_info->nominal_baseline = info.nominal_baseline;
 
-  if (channels->SetFiles(dev_info.get(), nullptr, nullptr)) {
+  if (device_->SetFiles(dev_info.get(), nullptr, nullptr)) {
     LOG(INFO) << "Write device info success";
     LOG(INFO) << "Device info: {name: " << dev_info->name
               << ", serial_number: " << dev_info->serial_number
@@ -68,8 +65,6 @@ bool DeviceWriter::WriteDeviceInfo(const std::string &filepath) {
 }
 
 bool DeviceWriter::WriteImgParams(const img_params_map_t &img_params_map) {
-  auto &&channels = device_->channels();
-
   img_params_map_t *img_params_new =
       const_cast<img_params_map_t *>(&img_params_map);
   // Update image params with raw
@@ -81,7 +76,7 @@ bool DeviceWriter::WriteImgParams(const img_params_map_t &img_params_map) {
     }
   }
 
-  if (channels->SetFiles(nullptr, img_params_new, nullptr)) {
+  if (device_->SetFiles(nullptr, img_params_new, nullptr)) {
     LOG(INFO) << "Write img params success";
     for (auto it = img_params_new->begin(); it != img_params_new->end(); it++) {
       LOG(INFO) << "Resolution: {width: " << (*it).first.width
@@ -103,8 +98,7 @@ bool DeviceWriter::WriteImgParams(const std::string &filepath) {
 }
 
 bool DeviceWriter::WriteImuParams(const imu_params_t &params) {
-  auto &&channels = device_->channels();
+  if (device_->SetFiles(
-  if (channels->SetFiles(
           nullptr, nullptr, const_cast<imu_params_t *>(&params))) {
     LOG(INFO) << "Write imu params success";
     LOG(INFO) << "Imu intrinsics accel: {" << params.in_accel << "}";

tools/writer/device_writer.h

@@ -19,9 +19,7 @@
 #include <memory>
 #include <string>
 
-#include "mynteye/mynteye.h"
+#include "mynteye/device/device.h"
-#include "mynteye/device/channel/channels.h"
-#include "mynteye/device/types.h"
 
 MYNTEYE_BEGIN_NAMESPACE