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

refactor(synthetic): remove usless logic

Browse Source
This commit is contained in:
TinyOh
2019-03-01 16:08:15 +08:00
parent d1cd7fadc5
commit 71e3286014
10 changed files with 375 additions and 338 deletions
Download Patch File Download Diff File Expand all files Collapse all files

372
src/mynteye/api/synthetic.cc
View File

@@ -36,6 +36,7 @@
#include "mynteye/api/processor/rectify_processor.h"
#endif
#include "mynteye/device/device.h"
#include "mynteye/api/data_tools.h"
#define RECTIFY_PROC_PERIOD 0
#define DISPARITY_PROC_PERIOD 0
@@ -46,74 +47,6 @@
MYNTEYE_BEGIN_NAMESPACE
namespace {
cv::Mat frame2mat(const std::shared_ptr<device::Frame> &frame) {
if (frame->format() == Format::YUYV) {
cv::Mat img(frame->height(), frame->width(), CV_8UC2, frame->data());
cv::cvtColor(img, img, cv::COLOR_YUV2BGR_YUY2);
return img;
} else if (frame->format() == Format::BGR888) {
cv::Mat img(frame->height(), frame->width(), CV_8UC3, frame->data());
return img;
} else { // Format::GRAY
return cv::Mat(frame->height(), frame->width(), CV_8UC1, frame->data());
}
}
api::StreamData data2api(const device::StreamData &data) {
return {data.img, frame2mat(data.frame), data.frame, data.frame_id};
}
void process_childs(
const std::shared_ptr<Processor> &proc, const std::string &name,
const Object &obj) {
auto &&processor = find_processor<Processor>(proc, name);
for (auto child : processor->GetChilds()) {
child->Process(obj);
}
}
// ObjMat/ObjMat2 > api::StreamData
api::StreamData obj_data_first(const ObjMat2 *obj) {
return {obj->first_data, obj->first, nullptr, obj->first_id};
}
api::StreamData obj_data_second(const ObjMat2 *obj) {
return {obj->second_data, obj->second, nullptr, obj->second_id};
}
api::StreamData obj_data(const ObjMat *obj) {
return {obj->data, obj->value, nullptr, obj->id};
}
api::StreamData obj_data_first(const std::shared_ptr<ObjMat2> &obj) {
return {obj->first_data, obj->first, nullptr, obj->first_id};
}
api::StreamData obj_data_second(const std::shared_ptr<ObjMat2> &obj) {
return {obj->second_data, obj->second, nullptr, obj->second_id};
}
api::StreamData obj_data(const std::shared_ptr<ObjMat> &obj) {
return {obj->data, obj->value, nullptr, obj->id};
}
// api::StreamData > ObjMat/ObjMat2
ObjMat data_obj(const api::StreamData &data) {
return ObjMat{data.frame, data.frame_id, data.img};
}
ObjMat2 data_obj(const api::StreamData &first, const api::StreamData &second) {
return ObjMat2{
first.frame, first.frame_id, first.img,
second.frame, second.frame_id, second.img};
}
} // namespace
void Synthetic::InitCalibInfo() {
if (calib_model_ == CalibrationModel::PINHOLE) {
LOG(INFO) << "camera calib model: pinhole";
@@ -149,7 +82,6 @@ Synthetic::Synthetic(API *api, CalibrationModel calib_model)
CHECK_NOTNULL(api_);
InitCalibInfo();
InitProcessors();
InitStreamSupports();
}
Synthetic::~Synthetic() {
@@ -171,19 +103,18 @@ void Synthetic::NotifyImageParamsChanged() {
extr_ = std::make_shared<Extrinsics>(
api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT));
}
if (calib_model_ == CalibrationModel::PINHOLE) {
auto &&processor = find_processor<RectifyProcessorOCV>(processor_);
if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_);
auto processor = getProcessorWithStream(Stream::LEFT_RECTIFIED);
if (processor && calib_model_ == CalibrationModel::PINHOLE) {
auto proc = static_cast<RectifyProcessorOCV*>(&(*processor));
proc->ReloadImageParams(intr_left_, intr_right_, extr_);
#ifdef WITH_CAM_MODELS
} else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
auto &&processor = find_processor<RectifyProcessor>(processor_);
if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_);
} else if (processor && calib_model_ == CalibrationModel::KANNALA_BRANDT) {
auto proc = static_cast<RectifyProcessor*>(&(*processor));
proc->ReloadImageParams(intr_left_, intr_right_, extr_);
#endif
} else {
LOG(ERROR) << "Unknow calib model type in device: "
<< calib_model_ << ", use default pinhole model";
auto &&processor = find_processor<RectifyProcessorOCV>(processor_);
if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_);
LOG(ERROR) << "Unknow calib model type in device" << std::endl;
}
}
@@ -242,14 +173,6 @@ bool Synthetic::Supports(const Stream &stream) const {
return checkControlDateWithStream(stream);
}
Synthetic::mode_t Synthetic::SupportsMode(const Stream &stream) const {
if (checkControlDateWithStream(stream)) {
auto data = getControlDateWithStream(stream);
return data.support_mode_;
}
return MODE_LAST;
}
void Synthetic::EnableStreamData(
const Stream &stream, stream_switch_callback_t callback,
bool try_tag) {
@@ -344,42 +267,13 @@ bool Synthetic::HasStreamCallback(const Stream &stream) const {
}
void Synthetic::StartVideoStreaming() {
auto &&device = api_->device();
for (unsigned int i =0; i< processors_.size(); i++) {
auto streams = processors_[i]->getTargetStreams();
for (unsigned int j =0; j< streams.size(); j++) {
if (processors_[i]->target_streams_[j].support_mode_ == MODE_NATIVE) {
auto stream = processors_[i]->target_streams_[j].stream;
device->SetStreamCallback(
stream,
[this, stream](const device::StreamData &data) {
auto &&stream_data = data2api(data);
ProcessNativeStream(stream, stream_data);
// Need mutex if set callback after start
if (HasStreamCallback(stream)) {
auto data = getControlDateWithStream(stream);
data.stream_callback(stream_data);
}
},
true);
}
}
}
device->Start(Source::VIDEO_STREAMING);
auto processor_root = static_cast<RootProcessor*>(&(*processor_));
processor_root->StartVideoStreaming();
}
void Synthetic::StopVideoStreaming() {
auto &&device = api_->device();
for (unsigned int i =0; i< processors_.size(); i++) {
auto streams = processors_[i]->getTargetStreams();
for (unsigned int j =0; j< streams.size(); j++) {
if (processors_[i]->target_streams_[j].support_mode_ == MODE_NATIVE) {
auto stream = processors_[i]->target_streams_[j].stream;
device->SetStreamCallback(stream, nullptr);
}
}
}
device->Stop(Source::VIDEO_STREAMING);
auto processor_root = static_cast<RootProcessor*>(&(*processor_));
processor_root->StopVideoStreaming();
}
void Synthetic::WaitForStreams() {
@@ -387,69 +281,11 @@ void Synthetic::WaitForStreams() {
}
api::StreamData Synthetic::GetStreamData(const Stream &stream) {
auto &&mode = GetStreamEnabledMode(stream);
if (mode == MODE_NATIVE) {
auto &&device = api_->device();
return data2api(device->GetStreamData(stream));
} else if (mode == MODE_SYNTHETIC) {
auto processor = getProcessorWithStream(stream);
auto sum = processor->getStreamsSum();
auto &&out = processor->GetOutput();
static std::shared_ptr<ObjMat2> output = nullptr;
if (sum == 1) {
if (out != nullptr) {
auto &&output = Object::Cast<ObjMat>(out);
if (output != nullptr) {
return obj_data(output);
}
VLOG(2) << "Rectify not ready now";
}
} else if (sum == 2) {
if (out != nullptr) {
output = Object::Cast<ObjMat2>(out);
}
auto streams = processor->getTargetStreams();
if (output != nullptr) {
int num = 0;
for (auto it : streams) {
if (it.stream == stream) {
if (num == 1) {
return obj_data_first(output);
} else {
return obj_data_second(output);
}
}
num++;
}
}
VLOG(2) << "Rectify not ready now";
} else {
LOG(ERROR) << "error: invalid sum!";
}
return {}; // frame.empty() == true
} else {
LOG(ERROR) << "Failed to get stream data of " << stream
<< ", unsupported or disabled";
return {}; // frame.empty() == true
}
return getProcessorWithStream(stream)->GetStreamData(stream);
}
std::vector<api::StreamData> Synthetic::GetStreamDatas(const Stream &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) {
return {GetStreamData(stream)};
} else {
LOG(ERROR) << "Failed to get stream data of " << stream
<< ", unsupported or disabled";
}
return {};
return getProcessorWithStream(stream)->GetStreamDatas(stream);
}
void Synthetic::SetPlugin(std::shared_ptr<Plugin> plugin) {
@@ -460,39 +296,6 @@ bool Synthetic::HasPlugin() const {
return plugin_ != nullptr;
}
void Synthetic::InitStreamSupports() {
auto &&device = api_->device();
if (device->Supports(Stream::LEFT) && device->Supports(Stream::RIGHT)) {
auto processor = getProcessorWithStream(Stream::LEFT);
for (unsigned int i = 0; i< processor->target_streams_.size(); i++) {
if (processor->target_streams_[i].stream == Stream::LEFT) {
processor->target_streams_[i].support_mode_ = MODE_NATIVE;
}
if (processor->target_streams_[i].stream == Stream::RIGHT) {
processor->target_streams_[i].support_mode_ = 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) {
auto processor = getProcessorWithStream(stream);
for (unsigned int i = 0; i< processor->target_streams_.size(); i++) {
if (processor->target_streams_[i].stream == stream) {
if (device->Supports(stream)) {
processor->target_streams_[i].support_mode_ = MODE_NATIVE;
processor->target_streams_[i].enabled_mode_ = MODE_NATIVE;
} else {
processor->target_streams_[i].support_mode_ = MODE_SYNTHETIC;
}
}
}
}
}
}
Synthetic::mode_t Synthetic::GetStreamEnabledMode(const Stream &stream) const {
if (checkControlDateWithStream(stream)) {
auto data = getControlDateWithStream(stream);
@@ -501,14 +304,6 @@ Synthetic::mode_t Synthetic::GetStreamEnabledMode(const Stream &stream) const {
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::InitProcessors() {
std::shared_ptr<Processor> rectify_processor = nullptr;
std::shared_ptr<Processor> points_processor = nullptr;
@@ -522,7 +317,7 @@ void Synthetic::InitProcessors() {
DISPARITY_NORM_PROC_PERIOD);
auto root_processor =
std::make_shared<RootProcessor>(ROOT_PROC_PERIOD);
std::make_shared<s1s2Processor>(api_->device(), ROOT_PROC_PERIOD);
if (calib_model_ == CalibrationModel::PINHOLE) {
// PINHOLE
@@ -567,21 +362,21 @@ void Synthetic::InitProcessors() {
}
rectify_processor->addTargetStreams(
{Stream::LEFT_RECTIFIED, Mode::MODE_LAST, Mode::MODE_LAST, nullptr});
{Stream::LEFT_RECTIFIED, Mode::MODE_LAST, nullptr});
rectify_processor->addTargetStreams(
{Stream::RIGHT_RECTIFIED, Mode::MODE_LAST, Mode::MODE_LAST, nullptr});
{Stream::RIGHT_RECTIFIED, Mode::MODE_LAST, nullptr});
disparity_processor->addTargetStreams(
{Stream::DISPARITY, Mode::MODE_LAST, Mode::MODE_LAST, nullptr});
{Stream::DISPARITY, Mode::MODE_LAST, nullptr});
disparitynormalized_processor->addTargetStreams(
{Stream::DISPARITY_NORMALIZED, Mode::MODE_LAST, Mode::MODE_LAST, nullptr});
{Stream::DISPARITY_NORMALIZED, Mode::MODE_LAST, nullptr});
points_processor->addTargetStreams(
{Stream::POINTS, Mode::MODE_LAST, Mode::MODE_LAST, nullptr});
{Stream::POINTS, Mode::MODE_LAST, nullptr});
depth_processor->addTargetStreams(
{Stream::DEPTH, Mode::MODE_LAST, Mode::MODE_LAST, nullptr});
{Stream::DEPTH, Mode::MODE_LAST, nullptr});
root_processor->addTargetStreams(
{Stream::LEFT, Mode::MODE_NATIVE, Mode::MODE_NATIVE, nullptr});
{Stream::LEFT, Mode::MODE_LAST, nullptr});
root_processor->addTargetStreams(
{Stream::RIGHT, Mode::MODE_NATIVE, Mode::MODE_NATIVE, nullptr});
{Stream::RIGHT, Mode::MODE_LAST, nullptr});
processors_.push_back(root_processor);
processors_.push_back(rectify_processor);
@@ -590,6 +385,8 @@ void Synthetic::InitProcessors() {
processors_.push_back(points_processor);
processors_.push_back(depth_processor);
using namespace std::placeholders; // NOLINT
root_processor->SetProcessCallback(
std::bind(&Synthetic::OnDeviceProcess, this, _1, _2, _3));
rectify_processor->SetProcessCallback(
std::bind(&Synthetic::OnRectifyProcess, this, _1, _2, _3));
disparity_processor->SetProcessCallback(
@@ -601,6 +398,8 @@ void Synthetic::InitProcessors() {
depth_processor->SetProcessCallback(
std::bind(&Synthetic::OnDepthProcess, this, _1, _2, _3));
root_processor->SetPostProcessCallback(
std::bind(&Synthetic::OnDevicePostProcess, this, _1));
rectify_processor->SetPostProcessCallback(
std::bind(&Synthetic::OnRectifyPostProcess, this, _1));
disparity_processor->SetPostProcessCallback(
@@ -615,100 +414,11 @@ void Synthetic::InitProcessors() {
processor_ = root_processor;
}
void Synthetic::ProcessNativeStream(
const Stream &stream, const api::StreamData &data) {
NotifyStreamData(stream, data);
if (stream == Stream::LEFT || stream == Stream::RIGHT) {
std::unique_lock<std::mutex> lk(mtx_left_right_ready_);
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) {
std::shared_ptr<Processor> processor = nullptr;
if (calib_model_ == CalibrationModel::PINHOLE) {
processor = find_processor<RectifyProcessorOCV>(processor_);
#ifdef WITH_CAM_MODELS
} else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
processor = find_processor<RectifyProcessor>(processor_);
#endif
} else {
LOG(ERROR) << "Unknow calib model type in device: "
<< calib_model_ << ", use default pinhole model";
processor = find_processor<RectifyProcessorOCV>(processor_);
}
processor->Process(data_obj(left_data, right_data));
}
return;
}
if (stream == Stream::LEFT_RECTIFIED || stream == Stream::RIGHT_RECTIFIED) {
static api::StreamData left_rect_data, right_rect_data;
if (stream == Stream::LEFT_RECTIFIED) {
left_rect_data = data;
} else if (stream == Stream::RIGHT_RECTIFIED) {
right_rect_data = data;
}
if (left_rect_data.img && right_rect_data.img &&
left_rect_data.img->frame_id == right_rect_data.img->frame_id) {
std::string name = RectifyProcessorOCV::NAME;
if (calib_model_ == CalibrationModel::PINHOLE) {
name = RectifyProcessorOCV::NAME;
#ifdef WITH_CAM_MODELS
} else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
name = RectifyProcessor::NAME;
#endif
}
process_childs(processor_, name,
data_obj(left_rect_data, right_rect_data));
}
return;
}
switch (stream) {
case Stream::DISPARITY: {
process_childs(processor_, DisparityProcessor::NAME, data_obj(data));
} break;
case Stream::DISPARITY_NORMALIZED: {
process_childs(processor_, DisparityNormalizedProcessor::NAME,
data_obj(data));
} break;
case Stream::POINTS: {
if (calib_model_ == CalibrationModel::PINHOLE) {
// PINHOLE
process_childs(processor_, PointsProcessorOCV::NAME, data_obj(data));
#ifdef WITH_CAM_MODELS
} else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
// KANNALA_BRANDT
process_childs(processor_, PointsProcessor::NAME, data_obj(data));
#endif
} else {
// UNKNOW
LOG(ERROR) << "Unknow calib model type in device: "
<< calib_model_;
}
} break;
case Stream::DEPTH: {
if (calib_model_ == CalibrationModel::PINHOLE) {
// PINHOLE
process_childs(processor_, DepthProcessorOCV::NAME, data_obj(data));
#ifdef WITH_CAM_MODELS
} else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
// KANNALA_BRANDT
process_childs(processor_, DepthProcessor::NAME, data_obj(data));
#endif
} else {
// UNKNOW
LOG(ERROR) << "Unknow calib model type in device: "
<< calib_model_;
}
} break;
default:
break;
}
bool Synthetic::OnDeviceProcess(
Object *const in, Object *const out,
std::shared_ptr<Processor> const parent) {
MYNTEYE_UNUSED(parent)
return GetStreamEnabledMode(Stream::LEFT) != MODE_SYNTHETIC;
}
bool Synthetic::OnRectifyProcess(
@@ -762,6 +472,20 @@ bool Synthetic::OnDepthProcess(
return GetStreamEnabledMode(Stream::DEPTH) != MODE_SYNTHETIC;
}
void Synthetic::OnDevicePostProcess(Object *const out) {
const ObjMat2 *output = Object::Cast<ObjMat2>(out);
NotifyStreamData(Stream::LEFT, obj_data_first(output));
NotifyStreamData(Stream::RIGHT, obj_data_second(output));
if (HasStreamCallback(Stream::LEFT)) {
auto data = getControlDateWithStream(Stream::LEFT);
data.stream_callback(obj_data_first(output));
}
if (HasStreamCallback(Stream::RIGHT)) {
auto data = getControlDateWithStream(Stream::RIGHT);
data.stream_callback(obj_data_second(output));
}
}
void Synthetic::OnRectifyPostProcess(Object *const out) {
const ObjMat2 *output = Object::Cast<ObjMat2>(out);
NotifyStreamData(Stream::LEFT_RECTIFIED, obj_data_first(output));