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

Complete native streams

Browse Source
This commit is contained in:
John Zhao 2018-04-27 22:35:43 +08:00
parent 3467c822c9
commit cf80332e13
3 changed files with 249 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
samples/api/camera.cc
View File

@ -1,6 +1,10 @@
#include <opencv2/highgui/highgui.hpp>
#include "mynteye/glog_init.h"
#include "mynteye/api.h"
#include "mynteye/times.h"
#include "mynteye/utils.h"
MYNTEYE_USE_NAMESPACE
@ -8,6 +12,98 @@ int main(int argc, char *argv[]) {
glog_init _(argc, argv);
auto &&api = API::Create();
api->LogOptionInfos();
std::size_t left_count = 0;
api->SetStreamCallback(
Stream::LEFT, [&left_count](const api::StreamData &data) {
CHECK_NOTNULL(data.img);
++left_count;
VLOG(2) << Stream::LEFT << ", count: " << left_count;
VLOG(2) << " frame_id: " << data.img->frame_id
<< ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time;
});
std::size_t right_count = 0;
api->SetStreamCallback(
Stream::RIGHT, [&right_count](const api::StreamData &data) {
CHECK_NOTNULL(data.img);
++right_count;
VLOG(2) << Stream::RIGHT << ", count: " << right_count;
VLOG(2) << " frame_id: " << data.img->frame_id
<< ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time;
});
std::size_t imu_count = 0;
api->SetMotionCallback([&imu_count](const api::MotionData &data) {
CHECK_NOTNULL(data.imu);
++imu_count;
VLOG(2) << "Imu count: " << imu_count;
VLOG(2) << " frame_id: " << data.imu->frame_id
<< ", timestamp: " << data.imu->timestamp
<< ", accel_x: " << data.imu->accel[0]
<< ", accel_y: " << data.imu->accel[1]
<< ", accel_z: " << data.imu->accel[2]
<< ", gyro_x: " << data.imu->gyro[0]
<< ", gyro_y: " << data.imu->gyro[1]
<< ", gyro_z: " << data.imu->gyro[2]
<< ", temperature: " << data.imu->temperature;
});
// Enable this will cache the motion datas until you get them.
api->EnableMotionDatas();
api->Start(Source::ALL);
cv::namedWindow("frame");
std::size_t motion_count = 0;
auto &&time_beg = times::now();
while (true) {
api->WaitForStreams();
auto &&left_data = api->GetStreamData(Stream::LEFT);
auto &&right_data = api->GetStreamData(Stream::RIGHT);
auto &&motion_datas = api->GetMotionDatas();
motion_count += motion_datas.size();
for (auto &&data : motion_datas) {
LOG(INFO) << "Imu frame_id: " << data.imu->frame_id
<< ", timestamp: " << data.imu->timestamp
<< ", accel_x: " << data.imu->accel[0]
<< ", accel_y: " << data.imu->accel[1]
<< ", accel_z: " << data.imu->accel[2]
<< ", gyro_x: " << data.imu->gyro[0]
<< ", gyro_y: " << data.imu->gyro[1]
<< ", gyro_z: " << data.imu->gyro[2]
<< ", temperature: " << data.imu->temperature;
}
cv::Mat img;
cv::hconcat(left_data.frame, right_data.frame, img);
cv::imshow("frame", img);
char key = static_cast<char>(cv::waitKey(1));
if (key == 27 || key == 'q' || key == 'Q') { // ESC/Q
break;
}
}
auto &&time_end = times::now();
api->Stop(Source::ALL);
float elapsed_ms =
times::count<times::microseconds>(time_end - time_beg) * 0.001f;
LOG(INFO) << "Time beg: " << times::to_local_string(time_beg)
<< ", end: " << times::to_local_string(time_end)
<< ", cost: " << elapsed_ms << "ms";
LOG(INFO) << "Left count: " << left_count
<< ", fps: " << (1000.f * left_count / elapsed_ms);
LOG(INFO) << "Right count: " << right_count
<< ", fps: " << (1000.f * right_count / elapsed_ms);
LOG(INFO) << "Imu count: " << imu_count
<< ", hz: " << (1000.f * imu_count / elapsed_ms);
// LOG(INFO) << "Motion count: " << motion_count
// << ", hz: " << (1000.f * motion_count / elapsed_ms);
return 0;
}

178
src/api/synthetic.cc
View File

@ -17,6 +17,20 @@
MYNTEYE_BEGIN_NAMESPACE
namespace {
cv::Mat frame2mat(const std::shared_ptr<device::Frame> &frame) {
// TODO(JohnZhao) Support different format frame to cv::Mat
CHECK_EQ(frame->format(), Format::GREY);
return cv::Mat(frame->height(), frame->width(), CV_8UC1, frame->data());
}
api::StreamData data2api(const device::StreamData &data) {
return {data.img, frame2mat(data.frame)};
}
} // namespace
Synthetic::Synthetic(API *api) : api_(api) {
VLOG(2) << __func__;
CHECK_NOTNULL(api_);
@ -32,7 +46,11 @@ Synthetic::~Synthetic() {
}
}
Synthetic::mode_t Synthetic::GetMode(const Stream &stream) const {
bool Synthetic::Supports(const Stream &stream) const {
return stream_supports_mode_.find(stream) != stream_supports_mode_.end();
}
Synthetic::mode_t Synthetic::SupportsMode(const Stream &stream) const {
try {
return stream_supports_mode_.at(stream);
} catch (const std::out_of_range &e) {
@ -40,10 +58,6 @@ Synthetic::mode_t Synthetic::GetMode(const Stream &stream) const {
}
}
bool Synthetic::Supports(const Stream &stream) const {
return GetMode(stream) != MODE_LAST;
}
void Synthetic::EnableStreamData(const Stream &stream) {
EnableStreamData(stream, 0);
}
@ -53,7 +67,7 @@ void Synthetic::DisableStreamData(const Stream &stream) {
}
bool Synthetic::IsStreamDataEnabled(const Stream &stream) const {
return stream_supports_mode_.find(stream) != stream_supports_mode_.end();
return stream_enabled_mode_.find(stream) != stream_enabled_mode_.end();
}
void Synthetic::SetStreamCallback(
@ -69,72 +83,157 @@ bool Synthetic::HasStreamCallback(const Stream &stream) const {
return stream_callbacks_.find(stream) != stream_callbacks_.end();
}
void Synthetic::StartVideoStreaming() {}
void Synthetic::StartVideoStreaming() {
auto &&device = api_->device();
for (auto &&it = stream_supports_mode_.begin();
it != stream_supports_mode_.end(); it++) {
if (it->second == MODE_NATIVE) {
auto &&stream = it->first;
device->SetStreamCallback(
stream, [this, stream](const device::StreamData &data) {
auto &&stream_data = data2api(data);
Process(stream, stream_data);
// Need mutex if set callback after start
if (HasStreamCallback(stream)) {
stream_callbacks_.at(stream)(stream_data);
}
});
}
}
device->Start(Source::VIDEO_STREAMING);
}
void Synthetic::StopVideoStreaming() {}
void Synthetic::StopVideoStreaming() {
auto &&device = api_->device();
for (auto &&it = stream_supports_mode_.begin();
it != stream_supports_mode_.end(); it++) {
if (it->second == MODE_NATIVE) {
device->SetStreamCallback(it->first, nullptr);
}
}
device->Stop(Source::VIDEO_STREAMING);
}
void Synthetic::WaitForStreams() {}
void Synthetic::WaitForStreams() {
api_->device()->WaitForStreams();
}
api::StreamData Synthetic::GetStreamData(const Stream &stream) {
UNUSED(stream)
auto &&mode = GetStreamEnabledMode(stream);
if (mode == MODE_NATIVE) {
auto &&device = api_->device();
return data2api(device->GetLatestStreamData(stream));
} else if (mode == MODE_SYNTHETIC) {
// TODO(JohnZhao)
} else {
LOG(ERROR) << "Failed to get stream data of " << stream
<< ", unsupported or disabled";
}
return {};
}
std::vector<api::StreamData> Synthetic::GetStreamDatas(const Stream &stream) {
UNUSED(stream)
auto &&mode = GetStreamEnabledMode(stream);
if (mode == MODE_NATIVE) {
auto &&device = api_->device();
std::vector<api::StreamData> datas;
for (auto &&data : device->GetStreamDatas(stream)) {
datas.push_back(data2api(data));
}
return datas;
} else if (mode == MODE_SYNTHETIC) {
// TODO(JohnZhao)
} else {
LOG(ERROR) << "Failed to get stream data of " << stream
<< ", unsupported or disabled";
}
return {};
}
void Synthetic::InitStreamSupports() {
auto &&device = api_->device();
for (Stream s = Stream::LEFT; s < Stream::LAST;) {
if (device->Supports(s)) {
stream_supports_mode_[s] = MODE_NATIVE;
if (device->Supports(Stream::LEFT) && device->Supports(Stream::RIGHT)) {
stream_supports_mode_[Stream::LEFT] = MODE_NATIVE;
stream_supports_mode_[Stream::RIGHT] = MODE_NATIVE;
std::vector<Stream> stream_chain{
Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED,
Stream::DISPARITY, Stream::DISPARITY_NORMALIZED,
Stream::POINTS, Stream::DEPTH};
for (auto &&stream : stream_chain) {
if (device->Supports(stream)) {
stream_supports_mode_[stream] = MODE_NATIVE;
} else {
stream_supports_mode_[stream] = MODE_SYNTHETIC;
}
}
}
// Enabled native streams by default
for (auto &&it = stream_supports_mode_.begin();
it != stream_supports_mode_.end(); it++) {
if (it->second == MODE_NATIVE) {
stream_enabled_mode_[it->first] = MODE_NATIVE;
}
s = static_cast<Stream>(static_cast<std::uint8_t>(s) + 1);
}
}
Synthetic::mode_t Synthetic::GetStreamEnabledMode(const Stream &stream) const {
try {
return stream_enabled_mode_.at(stream);
} catch (const std::out_of_range &e) {
return MODE_LAST;
}
}
bool Synthetic::IsStreamEnabledNative(const Stream &stream) const {
return GetStreamEnabledMode(stream) == MODE_NATIVE;
}
bool Synthetic::IsStreamEnabledSynthetic(const Stream &stream) const {
return GetStreamEnabledMode(stream) == MODE_SYNTHETIC;
}
void Synthetic::EnableStreamData(const Stream &stream, std::uint32_t depth) {
if (Supports(stream))
if (IsStreamDataEnabled(stream))
return;
// Activate processors of synthetic stream
switch (stream) {
case Stream::LEFT_RECTIFIED: {
if (!Supports(Stream::LEFT))
if (!IsStreamDataEnabled(Stream::LEFT))
break;
stream_supports_mode_[stream] = MODE_SYNTHETIC;
stream_enabled_mode_[stream] = MODE_SYNTHETIC;
CHECK(ActivateProcessor<RectifyProcessor>());
}
return;
case Stream::RIGHT_RECTIFIED: {
if (!Supports(Stream::RIGHT))
if (!IsStreamDataEnabled(Stream::RIGHT))
break;
stream_supports_mode_[stream] = MODE_SYNTHETIC;
stream_enabled_mode_[stream] = MODE_SYNTHETIC;
CHECK(ActivateProcessor<RectifyProcessor>());
}
return;
case Stream::DISPARITY: {
stream_supports_mode_[stream] = MODE_SYNTHETIC;
stream_enabled_mode_[stream] = MODE_SYNTHETIC;
EnableStreamData(Stream::LEFT_RECTIFIED, depth + 1);
EnableStreamData(Stream::RIGHT_RECTIFIED, depth + 1);
CHECK(ActivateProcessor<DisparityProcessor>());
}
return;
case Stream::DISPARITY_NORMALIZED: {
stream_supports_mode_[stream] = MODE_SYNTHETIC;
stream_enabled_mode_[stream] = MODE_SYNTHETIC;
EnableStreamData(Stream::DISPARITY, depth + 1);
CHECK(ActivateProcessor<DisparityNormalizedProcessor>());
}
return;
case Stream::POINTS: {
stream_supports_mode_[stream] = MODE_SYNTHETIC;
stream_enabled_mode_[stream] = MODE_SYNTHETIC;
EnableStreamData(Stream::DISPARITY, depth + 1);
CHECK(ActivateProcessor<PointsProcessor>());
}
return;
case Stream::DEPTH: {
stream_supports_mode_[stream] = MODE_SYNTHETIC;
stream_enabled_mode_[stream] = MODE_SYNTHETIC;
EnableStreamData(Stream::POINTS, depth + 1);
CHECK(ActivateProcessor<DepthProcessor>());
}
@ -151,32 +250,32 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) {
if (!IsStreamDataEnabled(stream))
return;
// Deactivate processors of synthetic stream
if (stream_supports_mode_[stream] != MODE_NATIVE) {
stream_supports_mode_.erase(stream);
if (stream_enabled_mode_[stream] != MODE_NATIVE) {
stream_enabled_mode_.erase(stream);
switch (stream) {
case Stream::LEFT_RECTIFIED: {
if (GetMode(Stream::RIGHT_RECTIFIED) == MODE_SYNTHETIC) {
if (IsStreamEnabledSynthetic(Stream::RIGHT_RECTIFIED)) {
DisableStreamData(Stream::RIGHT_RECTIFIED, depth + 1);
}
if (GetMode(Stream::DISPARITY) == MODE_SYNTHETIC) {
if (IsStreamEnabledSynthetic(Stream::DISPARITY)) {
DisableStreamData(Stream::DISPARITY, depth + 1);
}
DeactivateProcessor<RectifyProcessor>();
} break;
case Stream::RIGHT_RECTIFIED: {
if (GetMode(Stream::LEFT_RECTIFIED) == MODE_SYNTHETIC) {
if (IsStreamEnabledSynthetic(Stream::LEFT_RECTIFIED)) {
DisableStreamData(Stream::LEFT_RECTIFIED, depth + 1);
}
if (GetMode(Stream::DISPARITY) == MODE_SYNTHETIC) {
if (IsStreamEnabledSynthetic(Stream::DISPARITY)) {
DisableStreamData(Stream::DISPARITY, depth + 1);
}
DeactivateProcessor<RectifyProcessor>();
} break;
case Stream::DISPARITY: {
if (GetMode(Stream::DISPARITY_NORMALIZED) == MODE_SYNTHETIC) {
if (IsStreamEnabledSynthetic(Stream::DISPARITY_NORMALIZED)) {
DisableStreamData(Stream::DISPARITY_NORMALIZED, depth + 1);
}
if (GetMode(Stream::POINTS) == MODE_SYNTHETIC) {
if (IsStreamEnabledSynthetic(Stream::POINTS)) {
DisableStreamData(Stream::POINTS, depth + 1);
}
DeactivateProcessor<DisparityProcessor>();
@ -185,7 +284,7 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) {
DeactivateProcessor<DisparityNormalizedProcessor>();
} break;
case Stream::POINTS: {
if (GetMode(Stream::DEPTH) == MODE_SYNTHETIC) {
if (IsStreamEnabledSynthetic(Stream::DEPTH)) {
DisableStreamData(Stream::DEPTH, depth + 1);
}
DeactivateProcessor<PointsProcessor>();
@ -232,6 +331,19 @@ void Synthetic::InitProcessors() {
processor_ = rectify_processor;
}
void Synthetic::Process(const Stream &stream, const api::StreamData &data) {
static api::StreamData left_data, right_data;
if (stream == Stream::LEFT) {
left_data = data;
} else if (stream == Stream::RIGHT) {
right_data = data;
}
if (left_data.img && right_data.img &&
left_data.img->frame_id == right_data.img->frame_id) {
// TODO(JohnZhao)
}
}
bool Synthetic::OnRectifyProcess(
Object *const in, Object *const out, Processor *const parent) {
UNUSED(in)

9
src/api/synthetic.h
View File

@ -28,8 +28,8 @@ class Synthetic {
explicit Synthetic(API *api);
~Synthetic();
mode_t GetMode(const Stream &stream) const;
bool Supports(const Stream &stream) const;
mode_t SupportsMode(const Stream &stream) const;
void EnableStreamData(const Stream &stream);
void DisableStreamData(const Stream &stream);
@ -49,6 +49,10 @@ class Synthetic {
private:
void InitStreamSupports();
mode_t GetStreamEnabledMode(const Stream &stream) const;
bool IsStreamEnabledNative(const Stream &stream) const;
bool IsStreamEnabledSynthetic(const Stream &stream) const;
void EnableStreamData(const Stream &stream, std::uint32_t depth);
void DisableStreamData(const Stream &stream, std::uint32_t depth);
@ -59,6 +63,8 @@ class Synthetic {
template <class T>
bool DeactivateProcessor(bool tree = false);
void Process(const Stream &stream, const api::StreamData &data);
bool OnRectifyProcess(
Object *const in, Object *const out, Processor *const parent);
bool OnDisparityProcess(
@ -73,6 +79,7 @@ class Synthetic {
API *api_;
std::map<Stream, mode_t> stream_supports_mode_;
std::map<Stream, mode_t> stream_enabled_mode_;
std::map<Stream, stream_callback_t> stream_callbacks_;