tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
kalman 2019-01-09 16:52:00 +08:00
commit 6ddc5758c5
10 changed files with 219 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
CMakeLists.txt
View File

@ -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
)

19
cmake/DetectOpenCV.cmake
View File

@ -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()

36
include/mynteye/device/device.h
View File

@ -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_;
device::imu_params_t imu_params_;
img_params_map_t all_img_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;
};

89
src/mynteye/device/types.h → include/mynteye/device/types.h
View File

@ -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

69
src/mynteye/api/processor/points_processor.cc
View File

@ -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;
}

71
src/mynteye/device/channel/def.h
View File

@ -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_

10
src/mynteye/device/device.cc
View File

@ -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

4
tools/writer/CMakeLists.txt
View File

@ -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)

12
tools/writer/device_writer.cc
View File

@ -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 (channels->SetFiles(
if (device_->SetFiles(
nullptr, nullptr, const_cast<imu_params_t *>(&params))) {
LOG(INFO) << "Write imu params success";
LOG(INFO) << "Imu intrinsics accel: {" << params.in_accel << "}";

4
tools/writer/device_writer.h
View File

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