diff --git a/samples/api/camera.cc b/samples/api/camera.cc index 92546ab..41ca81f 100644 --- a/samples/api/camera.cc +++ b/samples/api/camera.cc @@ -1,6 +1,10 @@ +#include + #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(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(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; } diff --git a/src/api/synthetic.cc b/src/api/synthetic.cc index 3dc730c..9845e5b 100644 --- a/src/api/synthetic.cc +++ b/src/api/synthetic.cc @@ -17,6 +17,20 @@ MYNTEYE_BEGIN_NAMESPACE +namespace { + +cv::Mat frame2mat(const std::shared_ptr &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 Synthetic::GetStreamDatas(const Stream &stream) { - UNUSED(stream) + auto &&mode = GetStreamEnabledMode(stream); + if (mode == MODE_NATIVE) { + auto &&device = api_->device(); + std::vector 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_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(static_cast(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()); } 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()); } 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()); } return; case Stream::DISPARITY_NORMALIZED: { - stream_supports_mode_[stream] = MODE_SYNTHETIC; + stream_enabled_mode_[stream] = MODE_SYNTHETIC; EnableStreamData(Stream::DISPARITY, depth + 1); CHECK(ActivateProcessor()); } return; case Stream::POINTS: { - stream_supports_mode_[stream] = MODE_SYNTHETIC; + stream_enabled_mode_[stream] = MODE_SYNTHETIC; EnableStreamData(Stream::DISPARITY, depth + 1); CHECK(ActivateProcessor()); } return; case Stream::DEPTH: { - stream_supports_mode_[stream] = MODE_SYNTHETIC; + stream_enabled_mode_[stream] = MODE_SYNTHETIC; EnableStreamData(Stream::POINTS, depth + 1); CHECK(ActivateProcessor()); } @@ -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(); } 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(); } 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(); @@ -185,7 +284,7 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) { DeactivateProcessor(); } break; case Stream::POINTS: { - if (GetMode(Stream::DEPTH) == MODE_SYNTHETIC) { + if (IsStreamEnabledSynthetic(Stream::DEPTH)) { DisableStreamData(Stream::DEPTH, depth + 1); } DeactivateProcessor(); @@ -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) diff --git a/src/api/synthetic.h b/src/api/synthetic.h index 81bcb60..f181ecf 100644 --- a/src/api/synthetic.h +++ b/src/api/synthetic.h @@ -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 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_supports_mode_; + std::map stream_enabled_mode_; std::map stream_callbacks_;