From ee2d437c89a47df873ede8cb1d224f87818bb52d Mon Sep 17 00:00:00 2001 From: John Zhao Date: Wed, 9 Jan 2019 14:30:21 +0800 Subject: [PATCH 1/3] build(cmake): add install rpath --- CMakeLists.txt | 12 ++++++++++++ cmake/DetectOpenCV.cmake | 19 +++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3c01f53..65fc184 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/cmake/DetectOpenCV.cmake b/cmake/DetectOpenCV.cmake index 34a5779..d166f50 100644 --- a/cmake/DetectOpenCV.cmake +++ b/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() From fb1ffb81ca93e32741dad5325372ebe7abe7b4ca Mon Sep 17 00:00:00 2001 From: Osenberg Date: Wed, 9 Jan 2019 16:00:34 +0800 Subject: [PATCH 2/3] feat(points_processor.cc): implement equidistant model for pointscloud --- src/mynteye/api/processor/points_processor.cc | 69 +++++++++++++++++++ 1 file changed, 69 insertions(+) diff --git a/src/mynteye/api/processor/points_processor.cc b/src/mynteye/api/processor/points_processor.cc index 81102d5..3bb9524 100644 --- a/src/mynteye/api/processor/points_processor.cc +++ b/src/mynteye/api/processor/points_processor.cc @@ -14,6 +14,8 @@ #include "mynteye/api/processor/points_processor.h" #include +#include +#include #include @@ -21,6 +23,33 @@ MYNTEYE_BEGIN_NAMESPACE +namespace { +// Encapsulate differences between processing float and uint16_t depths +template struct DepthTraits {}; + +template<> +struct DepthTraits { + 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& buffer) {} // Do nothing - already zero-filled +}; + +template<> +struct DepthTraits { + 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& buffer) { + float* start = reinterpret_cast(&buffer[0]); + float* end = reinterpret_cast(&buffer[0] + buffer.size()); + std::fill(start, end, std::numeric_limits::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::toMeters(static_cast(1)); + float constant_x = unit_scaling / fx; + float constant_y = unit_scaling / fy; + // float bad_point = std::numeric_limits::quiet_NaN(); + + const ObjMat *input = Object::Cast(in); + ObjMat *output = Object::Cast(out); + output->value.create(input->value.size(), CV_MAKETYPE(CV_32F, 3)); + + int height = static_cast(output->value.rows); + int width = static_cast(output->value.cols); + for (int v = 0; v < height; ++v) { + + cv::Vec3f *dptr = output->value.ptr(v); + for (int u = 0; u < width; ++u) { + float depth = input->value.at(v, u); + + // Missing points denoted by NaNs + if (!DepthTraits::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::toMeters(depth); + } + } + return true; } From 242dc2b6fb5a6a8aa5fad18f985dcda440ae4405 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Wed, 9 Jan 2019 16:51:28 +0800 Subject: [PATCH 3/3] refactor(device): add files getter/setter to device --- CMakeLists.txt | 1 + include/mynteye/device/device.h | 36 +++------- {src => include}/mynteye/device/types.h | 89 ++++++------------------- src/mynteye/device/channel/def.h | 71 ++++++++++++++++++++ src/mynteye/device/device.cc | 10 +++ tools/writer/CMakeLists.txt | 4 -- tools/writer/device_writer.cc | 12 +--- tools/writer/device_writer.h | 4 +- 8 files changed, 119 insertions(+), 108 deletions(-) rename {src => include}/mynteye/device/types.h (73%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 65fc184..745ba70 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -309,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 ) diff --git a/include/mynteye/device/device.h b/include/mynteye/device/device.h index 6dded7a..30634b9 100644 --- a/include/mynteye/device/device.h +++ b/include/mynteye/device/device.h @@ -22,8 +22,8 @@ #include #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 AsyncCallback; -namespace device { - -typedef struct ImgParams { - bool ok; - std::string version; - std::shared_ptr in_left; - std::shared_ptr 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; using img_params_t = device::img_params_t; + using img_params_map_t = std::map; using imu_params_t = device::imu_params_t; protected: @@ -337,10 +318,10 @@ class MYNTEYE_API Device { virtual Capabilities GetKeyStreamCapability() const = 0; - std::map 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 device_; std::shared_ptr device_info_; - std::map all_img_params_; - device::imu_params_t imu_params_; + img_params_map_t all_img_params_; + imu_params_t imu_params_; std::map> stream_intrinsics_; std::map> 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; }; diff --git a/src/mynteye/device/types.h b/include/mynteye/device/types.h similarity index 73% rename from src/mynteye/device/types.h rename to include/mynteye/device/types.h index 41b372a..7ad1b75 100644 --- a/src/mynteye/device/types.h +++ b/include/mynteye/device/types.h @@ -18,13 +18,35 @@ #include #include #include +#include #include #include #include "mynteye/mynteye.h" +#include "mynteye/types.h" MYNTEYE_BEGIN_NAMESPACE +namespace device { + +typedef struct ImgParams { + bool ok; + std::string version; + std::shared_ptr in_left; + std::shared_ptr 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 to_data() const { - return {{header, static_cast((serial_number >> 24) & 0xFF), - static_cast((serial_number >> 16) & 0xFF), - static_cast((serial_number >> 8) & 0xFF), - static_cast(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 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 packets; - std::uint8_t checksum; -}; -#pragma pack(pop) - #undef MYNTEYE_PROPERTY MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/channel/def.h b/src/mynteye/device/channel/def.h index 7e5d39e..a903854 100644 --- a/src/mynteye/device/channel/def.h +++ b/src/mynteye/device/channel/def.h @@ -15,6 +15,10 @@ #define MYNTEYE_DEVICE_CHANNEL_DEF_H_ #pragma once +#include +#include +#include + #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 to_data() const { + return {{header, static_cast((serial_number >> 24) & 0xFF), + static_cast((serial_number >> 16) & 0xFF), + static_cast((serial_number >> 8) & 0xFF), + static_cast(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 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 packets; + std::uint8_t checksum; +}; +#pragma pack(pop) + MYNTEYE_END_NAMESPACE #endif // MYNTEYE_DEVICE_CHANNEL_DEF_H_ diff --git a/src/mynteye/device/device.cc b/src/mynteye/device/device.cc index a539a2f..372d62e 100644 --- a/src/mynteye/device/device.cc +++ b/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 diff --git a/tools/writer/CMakeLists.txt b/tools/writer/CMakeLists.txt index f62cf3e..93b2a10 100644 --- a/tools/writer/CMakeLists.txt +++ b/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) diff --git a/tools/writer/device_writer.cc b/tools/writer/device_writer.cc index a164dc0..cb57712 100644 --- a/tools/writer/device_writer.cc +++ b/tools/writer/device_writer.cc @@ -18,7 +18,6 @@ #include #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); // 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 ¶ms) { - auto &&channels = device_->channels(); - if (channels->SetFiles( + if (device_->SetFiles( nullptr, nullptr, const_cast(¶ms))) { LOG(INFO) << "Write imu params success"; LOG(INFO) << "Imu intrinsics accel: {" << params.in_accel << "}"; diff --git a/tools/writer/device_writer.h b/tools/writer/device_writer.h index f3a7af2..1e36ed5 100644 --- a/tools/writer/device_writer.h +++ b/tools/writer/device_writer.h @@ -19,9 +19,7 @@ #include #include -#include "mynteye/mynteye.h" -#include "mynteye/device/channel/channels.h" -#include "mynteye/device/types.h" +#include "mynteye/device/device.h" MYNTEYE_BEGIN_NAMESPACE