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
parent c79cfd4c00 242dc2b6fb
commit 6ddc5758c5
10 changed files with 219 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

216
src/mynteye/device/types.h
View File

@@ -1,216 +0,0 @@
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MYNTEYE_DEVICE_TYPES_H_
#define MYNTEYE_DEVICE_TYPES_H_
#pragma once
#include <cstdint>
#include <array>
#include <bitset>
#include <string>
#include <vector>
#include "mynteye/mynteye.h"
MYNTEYE_BEGIN_NAMESPACE
#define MYNTEYE_PROPERTY(TYPE, NAME) \
public: \
void set_##NAME(TYPE NAME) { \
NAME##_ = NAME; \
} \
TYPE NAME() const { \
return NAME##_; \
} \
\
private: \
TYPE NAME##_;
/**
* Version.
*/
class MYNTEYE_API Version {
public:
using size_t = std::size_t;
using value_t = std::uint8_t;
Version() = default;
Version(value_t major, value_t minor) : major_(major), minor_(minor) {}
explicit Version(const std::string &name)
: major_(parse_part(name, 0)), minor_(parse_part(name, 1)) {}
virtual ~Version() {}
bool empty() const {
return major_ == 0 && minor_ == 0;
}
bool operator==(const Version &other) const {
return major_ == other.major_ && minor_ == other.minor_;
}
bool operator<=(const Version &other) const {
if (major_ < other.major_)
return true;
if (major_ > other.major_)
return false;
return minor_ <= other.minor_;
}
bool operator!=(const Version &other) const {
return !(*this == other);
}
bool operator<(const Version &other) const {
return !(*this == other) && (*this <= other);
}
bool operator>(const Version &other) const {
return !(*this <= other);
}
bool operator>=(const Version &other) const {
return (*this == other) || (*this > other);
}
bool is_between(const Version &from, const Version &until) {
return (from <= *this) && (*this <= until);
}
std::string to_string() const;
static std::vector<std::string> split(const std::string &s);
static value_t parse_part(const std::string &name, size_t part);
MYNTEYE_PROPERTY(value_t, major)
MYNTEYE_PROPERTY(value_t, minor)
};
/**
* Hardware version.
*/
class MYNTEYE_API HardwareVersion : public Version {
public:
using flag_t = std::bitset<8>;
HardwareVersion() = default;
HardwareVersion(value_t major, value_t minor, value_t flag = 0)
: Version(major, minor), flag_(flag) {}
explicit HardwareVersion(const std::string &name, value_t flag = 0)
: Version(parse_part(name, 0), parse_part(name, 1)), flag_(flag) {}
MYNTEYE_PROPERTY(flag_t, flag)
};
/**
* Type.
*/
class MYNTEYE_API Type {
public:
using size_t = std::size_t;
using value_t = std::uint16_t;
Type() = default;
Type(value_t vendor, value_t product) : vendor_(vendor), product_(product) {}
explicit Type(const std::string &name)
: vendor_(parse_part(name, 0, 2)), product_(parse_part(name, 2, 2)) {}
virtual ~Type() {}
std::string to_string() const;
static value_t parse_part(const std::string &name, size_t pos, size_t count);
MYNTEYE_PROPERTY(value_t, vendor)
MYNTEYE_PROPERTY(value_t, product)
};
/**
* @ingroup datatypes
* Device infomation.
*/
struct MYNTEYE_API DeviceInfo {
std::string name;
std::string serial_number;
Version firmware_version;
HardwareVersion hardware_version;
Version spec_version;
Type lens_type;
Type imu_type;
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
#endif // MYNTEYE_DEVICE_TYPES_H_