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

Merge branch 'develop' into feature/android

Browse Source 
* develop: (28 commits)
  feat(*): add function to querry some hardware info
  feat: forbid 2100/210A uless update sdk to 2.3.1 and above
  refactor(synthetic): remove usless logic
  chore(readme): update readme
  docs(doxyfile): update version
  fix: change cmake version to 2.3.2
  feat(api) sdk/firmware version check
  feat(api): version check
  fix(wrapper): fix camera info repeat bug
  build(makefile): ensure uninstall before install
  fix(correspondence): also wait stream matched ready
  fix(record): shield diable logic temporarily
  chore(readme): chore(readme): update readme
  chore(readme): update readme
  chore(doc): update version
  fix(samples): delete useless comment
  fix(ros): fix camera info bug
  fix(correspondence): improve warning if not start motion tracking
  fix(correspondence): fix include header
  fix(ros): record close bug
  ...
This commit is contained in:
John Zhao 2019-02-28 15:58:55 +08:00
commit 8f096612a3
36 changed files with 1169 additions and 304 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -14,7 +14,7 @@
cmake_minimum_required(VERSION 3.0)
project(mynteye VERSION 2.3.0 LANGUAGES C CXX)
project(mynteye VERSION 2.3.2 LANGUAGES C CXX)
include(cmake/Common.cmake)
@ -219,6 +219,8 @@ if(WITH_API)
src/mynteye/api/processor/depth_processor_ocv.cc
src/mynteye/api/processor/rectify_processor_ocv.cc
src/mynteye/api/config.cc
src/mynteye/api/correspondence.cc
src/mynteye/api/version_checker.cc
)
if(WITH_CAM_MODELS)
list(APPEND MYNTEYE_SRCS

2
Makefile
View File

@ -130,7 +130,7 @@ endif
# install
install: build
install: uninstall build
@$(call echo,Make $@)
ifeq ($(HOST_OS),Win)
ifneq ($(HOST_NAME),MinGW)

10
README.md
View File

@ -1,6 +1,6 @@
# MYNT® EYE S SDK
[![](https://img.shields.io/badge/MYNT%20EYE%20S%20SDK-2.3.0-brightgreen.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK)
[![](https://img.shields.io/badge/MYNT%20EYE%20S%20SDK-2.3.2-brightgreen.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK)
## Overview
@ -17,11 +17,11 @@ Please follow the guide doc to install the SDK on different platforms.
## Documentations
* [API Doc](https://github.com/slightech/MYNT-EYE-S-SDK/releases): API reference, some guides and data spec.
* en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2764152/mynt-eye-s-sdk-apidoc-2.3.0-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2764156/mynt-eye-s-sdk-apidoc-2.3.0-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK/)
* zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2764160/mynt-eye-s-sdk-apidoc-2.3.0-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2764173/mynt-eye-s-sdk-apidoc-2.3.0-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/api/mynt-eye-s-sdk-apidoc-2.3.0-zh-Hans/mynt-eye-s-sdk-apidoc-2.3.0-zh-Hans/index.html)
* en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2913110/mynt-eye-s-sdk-apidoc-2.3.2-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2913111/mynt-eye-s-sdk-apidoc-2.3.2-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK/)
* zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2913112/mynt-eye-s-sdk-apidoc-2.3.2-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2913113/mynt-eye-s-sdk-apidoc-2.3.2-zh-Hans.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/api/mynt-eye-s-sdk-apidoc-2.3.2-zh-Hans/mynt-eye-s-sdk-apidoc-2.3.2-zh-Hans/index.html)
* [Guide Doc](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/releases): How to install and start using the SDK.
* en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2764143/mynt-eye-s-sdk-guide-2.3.0-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2764145/mynt-eye-s-sdk-guide-2.3.0-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK-Guide/)
* zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2764150/mynt-eye-s-sdk-guide-2.3.0-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2764163/mynt-eye-s-sdk-guide-2.3.0-zh-Hans.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/sdk/mynt-eye-s-sdk-guide-2.3.0-zh-Hans/mynt-eye-s-sdk-guide-2.3.0-zh-Hans/index.html)
* en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2913052/mynt-eye-s-sdk-guide-2.3.2-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2913053/mynt-eye-s-sdk-guide-2.3.2-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK-Guide/)
* zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2913054/mynt-eye-s-sdk-guide-2.3.2-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2913056/mynt-eye-s-sdk-guide-2.3.2-zh-Hans.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/sdk/mynt-eye-s-sdk-guide-2.3.2-zh-Hans/mynt-eye-s-sdk-guide-2.3.2-zh-Hans/index.html)
> Supported languages: `en`, `zh-Hans`.

2
doc/en/api.doxyfile
View File

@ -38,7 +38,7 @@ PROJECT_NAME = "MYNT EYE S SDK"
# could be handy for archiving the generated documentation or if some version
# control system is used.
PROJECT_NUMBER = 2.3.0
PROJECT_NUMBER = 2.3.2
# Using the PROJECT_BRIEF tag one can provide an optional one line description
# for a project that appears at the top of each page and should give viewer a

2
doc/en/spec_hardware_info.md
View File

@ -12,3 +12,5 @@
| Lens type | lens_type | - | × | √ Get/Set | 4 | vendor(2),product(2); default: 0 |
| IMU type | imu_type | - | × | √ Get/Set | 4 | vendor(2),product(2); default: 0 |
| Nominal baseline | nominal_baseline | - | × | √ Get/Set | 2 | unit: mm; default: 0 |
| Auxiliary chip version | auxiliary_chip_version | - | × | √ Get | 2 | major,minor |
| isp version | isp_version | - | × | √ Get | 2 | major,minor |

2
doc/zh-Hans/api.doxyfile
View File

@ -38,7 +38,7 @@ PROJECT_NAME = "MYNT EYE S SDK"
# could be handy for archiving the generated documentation or if some version
# control system is used.
PROJECT_NUMBER = 2.3.0
PROJECT_NUMBER = 2.3.2
# Using the PROJECT_BRIEF tag one can provide an optional one line description
# for a project that appears at the top of each page and should give viewer a

2
doc/zh-Hans/spec_hardware_info.md
View File

@ -12,6 +12,8 @@
| 镜头类型 | lens_type | - | × | √ Get/Set | 4 | vendor(2),product(2) ，未 Set 默认 0 |
| IMU 类型 | imu_type | - | × | √ Get/Set | 4 | vendor(2),product(2) ，未 Set 默认 0 |
| 基线长度 | nominal_baseline | - | × | √ Get/Set | 2 | 单位 mm ，未 set 默认 0 |
| 辅助芯片版本 | auxiliary_chip_version | - | × | √ Get | 2 | major,minor |
| ISP版本 | isp_version | - | × | √ Get | 2 | major,minor |
* 描述符获取：指通用 USB 设备信息，可用工具查看。
* 拓展通道获取：指通过拓展通道（UVC Extension Unit）问硬件获取到的信息，需要读取。

32
include/mynteye/api/api.h
View File

@ -30,6 +30,7 @@ MYNTEYE_BEGIN_NAMESPACE
struct DeviceInfo;
class Correspondence;
class Device;
class Synthetic;
@ -91,6 +92,8 @@ class MYNTEYE_API API {
using stream_callback_t = std::function<void(const api::StreamData &data)>;
/** The api::MotionData callback. */
using motion_callback_t = std::function<void(const api::MotionData &data)>;
/** The enable/disable switch callback. */
using stream_switch_callback_t = std::function<void(const Stream &stream)>;
explicit API(std::shared_ptr<Device> device, CalibrationModel calib_model);
virtual ~API();
@ -280,11 +283,31 @@ class MYNTEYE_API API {
* still support this stream.
*/
void EnableStreamData(const Stream &stream);
/**
* Enable the data of stream.
* callback function will call before the father processor enable.
* when try_tag is true, the function will do nothing except callback.
*/
void EnableStreamData(
const Stream &stream,
stream_switch_callback_t callback,
bool try_tag = false);
/**
* Disable the data of stream.
*/
void DisableStreamData(const Stream &stream);
/**
* Disable the data of stream.
* callback function will call before the children processor disable.
* when try_tag is true, the function will do nothing except callback.
*/
void DisableStreamData(
const Stream &stream,
stream_switch_callback_t callback,
bool try_tag = false);
/**
* Get the latest data of stream.
*/
@ -305,6 +328,11 @@ class MYNTEYE_API API {
*/
std::vector<api::MotionData> GetMotionDatas();
/**
* Enable motion datas with timestamp correspondence of some stream.
*/
void EnableTimestampCorrespondence(const Stream &stream);
/**
* Enable the plugin.
*/
@ -317,6 +345,10 @@ class MYNTEYE_API API {
std::unique_ptr<Synthetic> synthetic_;
std::unique_ptr<Correspondence> correspondence_;
motion_callback_t callback_;
void CheckImageParams();
};

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

@ -278,6 +278,10 @@ class MYNTEYE_API Device {
*/
std::vector<device::StreamData> GetStreamDatas(const Stream &stream);
/**
* Disable cache motion datas.
*/
void DisableMotionDatas();
/**
* Enable cache motion datas.
*/

2
include/mynteye/device/types.h
View File

@ -162,6 +162,8 @@ struct MYNTEYE_API DeviceInfo {
Type lens_type;
Type imu_type;
std::uint16_t nominal_baseline;
Version auxiliary_chip_version;
Version isp_version;
};
#undef MYNTEYE_PROPERTY

6
include/mynteye/types.h
View File

@ -122,6 +122,12 @@ enum class Info : std::uint8_t {
IMU_TYPE,
/** Nominal baseline */
NOMINAL_BASELINE,
/** Auxiliary chip version */
AUXILIARY_CHIP_VERSION,
/** Isp version */
ISP_VERSION,
/** SDK version*/
SDK_VERSION,
/** Last guard */
LAST
};

4
samples/tutorials/CMakeLists.txt
View File

@ -105,6 +105,10 @@ if(PCL_FOUND)
WITH_OPENCV WITH_PCL
)
endif()
make_executable2(get_imu_correspondence
SRCS data/get_imu_correspondence.cc util/cv_painter.cc
WITH_OPENCV
)
make_executable2(get_imu SRCS data/get_imu.cc util/cv_painter.cc WITH_OPENCV)
make_executable2(get_from_callbacks
SRCS data/get_from_callbacks.cc util/cv_painter.cc

4
samples/tutorials/control/framerate.cc
View File

@ -34,10 +34,6 @@ int main(int argc, char *argv[]) {
// Set frame rate options for s1030
if (model == Model::STANDARD) {
// Attention: must set FRAME_RATE and IMU_FREQUENCY together,
// otherwise won't.
// succeed.
// FRAME_RATE values: 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60
api->SetOptionValue(Option::FRAME_RATE, 25);
// IMU_FREQUENCY values: 100, 200, 250, 333, 500

3
samples/tutorials/data/get_device_info.cc
View File

@ -28,6 +28,9 @@ int main(int argc, char *argv[]) {
LOG(INFO) << "Lens type: " << api->GetInfo(Info::LENS_TYPE);
LOG(INFO) << "IMU type: " << api->GetInfo(Info::IMU_TYPE);
LOG(INFO) << "Nominal baseline: " << api->GetInfo(Info::NOMINAL_BASELINE);
LOG(INFO) << "Auxiliary chip version: "
<< api->GetInfo(Info::AUXILIARY_CHIP_VERSION);
LOG(INFO) << "Nominal baseline: " << api->GetInfo(Info::ISP_VERSION);
return 0;
}

85
samples/tutorials/data/get_imu_correspondence.cc Normal file
View File

@ -0,0 +1,85 @@
// 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.
#include <opencv2/highgui/highgui.hpp>
#include "mynteye/logger.h"
#include "mynteye/api/api.h"
#include "util/cv_painter.h"
MYNTEYE_USE_NAMESPACE
int main(int argc, char *argv[]) {
auto &&api = API::Create(argc, argv);
if (!api) return 1;
bool ok;
auto &&request = api->SelectStreamRequest(&ok);
if (!ok) return 1;
api->ConfigStreamRequest(request);
// Enable motion datas with timestamp correspondence of some stream
api->EnableTimestampCorrespondence(Stream::LEFT);
api->Start(Source::ALL);
CVPainter painter;
cv::namedWindow("frame");
std::uint64_t prev_img_stamp = 0;
std::uint64_t prev_imu_stamp = 0;
while (true) {
api->WaitForStreams();
auto &&left_data = api->GetStreamData(Stream::LEFT);
auto &&right_data = api->GetStreamData(Stream::RIGHT);
auto img_stamp = left_data.img->timestamp;
LOG(INFO) << "Img timestamp: " << img_stamp
<< ", diff_prev=" << (img_stamp - prev_img_stamp);
prev_img_stamp = img_stamp;
cv::Mat img;
cv::hconcat(left_data.frame, right_data.frame, img);
auto &&motion_datas = api->GetMotionDatas();
LOG(INFO) << "Imu count: " << motion_datas.size();
for (auto &&data : motion_datas) {
auto imu_stamp = data.imu->timestamp;
LOG(INFO) << "Imu timestamp: " << imu_stamp
<< ", diff_prev=" << (imu_stamp - prev_imu_stamp)
<< ", diff_img=" << (1.f + imu_stamp - img_stamp);
prev_imu_stamp = imu_stamp;
}
LOG(INFO);
/*
painter.DrawImgData(img, *left_data.img);
if (!motion_datas.empty()) {
painter.DrawImuData(img, *motion_datas[0].imu);
}
*/
cv::imshow("frame", img);
char key = static_cast<char>(cv::waitKey(1));
if (key == 27 || key == 'q' || key == 'Q') { // ESC/Q
break;
}
}
api->Stop(Source::ALL);
return 0;
}

99
src/mynteye/api/api.cc
View File

@ -22,9 +22,11 @@
#include <thread>
#include "mynteye/logger.h"
#include "mynteye/api/correspondence.h"
#include "mynteye/api/dl.h"
#include "mynteye/api/plugin.h"
#include "mynteye/api/synthetic.h"
#include "mynteye/api/version_checker.h"
#include "mynteye/device/device.h"
#include "mynteye/device/utils.h"
@ -208,7 +210,7 @@ std::vector<std::string> get_plugin_paths() {
} // namespace
API::API(std::shared_ptr<Device> device, CalibrationModel calib_model)
: device_(device) {
: device_(device), correspondence_(nullptr) {
VLOG(2) << __func__;
// std::dynamic_pointer_cast<StandardDevice>(device_);
synthetic_.reset(new Synthetic(this, calib_model));
@ -221,7 +223,10 @@ API::~API() {
std::shared_ptr<API> API::Create(int argc, char *argv[]) {
auto &&device = device::select();
if (!device) return nullptr;
return Create(argc, argv, device);
auto api = Create(argc, argv, device);
if (api && checkFirmwareVersion(api))
return api;
return nullptr;
}
std::shared_ptr<API> API::Create(
@ -260,7 +265,7 @@ std::shared_ptr<API> API::Create(const std::shared_ptr<Device> &device) {
}
} else {
LOG(ERROR) <<"no device!";
api = std::make_shared<API>(device, CalibrationModel::UNKNOW);
return nullptr;
}
return api;
}
@ -323,6 +328,20 @@ std::shared_ptr<DeviceInfo> API::GetInfo() const {
}
std::string API::GetInfo(const Info &info) const {
if (info == Info::SDK_VERSION) {
std::string info_path =
utils::get_sdk_install_dir();
info_path.append(MYNTEYE_OS_SEP "share" \
MYNTEYE_OS_SEP "mynteye" MYNTEYE_OS_SEP "build.info");
cv::FileStorage fs(info_path, cv::FileStorage::READ);
if (!fs.isOpened()) {
LOG(WARNING) << "build.info not found: " << info_path;
return "null";
}
return fs["MYNTEYE_VERSION"];
}
return device_->GetInfo(info);
}
@ -377,10 +396,15 @@ void API::SetStreamCallback(const Stream &stream, stream_callback_t callback) {
}
void API::SetMotionCallback(motion_callback_t callback) {
static auto callback_ = callback;
if (correspondence_) {
correspondence_->SetMotionCallback(callback);
return;
}
callback_ = callback;
if (callback_) {
device_->SetMotionCallback(
[](const device::MotionData &data) { callback_({data.imu}); }, true);
device_->SetMotionCallback([this](const device::MotionData &data) {
callback_({data.imu});
}, true);
} else {
device_->SetMotionCallback(nullptr);
}
@ -435,7 +459,11 @@ void API::Stop(const Source &source) {
}
void API::WaitForStreams() {
synthetic_->WaitForStreams();
if (correspondence_) {
correspondence_->WaitForStreams();
} else {
synthetic_->WaitForStreams();
}
}
void API::EnableStreamData(const Stream &stream) {
@ -446,24 +474,69 @@ void API::DisableStreamData(const Stream &stream) {
synthetic_->DisableStreamData(stream);
}
void API::EnableStreamData(
const Stream &stream, stream_switch_callback_t callback,
bool try_tag) {
synthetic_->EnableStreamData(stream, callback, try_tag);
}
void API::DisableStreamData(
const Stream &stream, stream_switch_callback_t callback,
bool try_tag) {
synthetic_->DisableStreamData(stream, callback, try_tag);
}
api::StreamData API::GetStreamData(const Stream &stream) {
return synthetic_->GetStreamData(stream);
if (correspondence_ && correspondence_->Watch(stream)) {
return correspondence_->GetStreamData(stream);
} else {
return synthetic_->GetStreamData(stream);
}
}
std::vector<api::StreamData> API::GetStreamDatas(const Stream &stream) {
return synthetic_->GetStreamDatas(stream);
if (correspondence_ && correspondence_->Watch(stream)) {
return correspondence_->GetStreamDatas(stream);
} else {
return synthetic_->GetStreamDatas(stream);
}
}
void API::EnableMotionDatas(std::size_t max_size) {
if (correspondence_) return; // not cache them
device_->EnableMotionDatas(max_size);
}
std::vector<api::MotionData> API::GetMotionDatas() {
std::vector<api::MotionData> datas;
for (auto &&data : device_->GetMotionDatas()) {
datas.push_back({data.imu});
if (correspondence_) {
return correspondence_->GetMotionDatas();
} else {
std::vector<api::MotionData> datas;
for (auto &&data : device_->GetMotionDatas()) {
datas.push_back({data.imu});
}
return datas;
}
}
void API::EnableTimestampCorrespondence(const Stream &stream) {
if (correspondence_ == nullptr) {
correspondence_.reset(new Correspondence(device_, stream));
{
device_->DisableMotionDatas();
if (callback_) {
correspondence_->SetMotionCallback(callback_);
callback_ = nullptr;
}
}
using namespace std::placeholders; // NOLINT
device_->SetMotionCallback(
std::bind(&Correspondence::OnMotionDataCallback,
correspondence_.get(), _1),
true);
synthetic_->SetStreamDataListener(
std::bind(&Correspondence::OnStreamDataCallback,
correspondence_.get(), _1, _2));
}
return datas;
}
void API::EnablePlugin(const std::string &path) {

277
src/mynteye/api/correspondence.cc Normal file
View File

@ -0,0 +1,277 @@
// 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.
#include "mynteye/api/correspondence.h"
#include "mynteye/device/device.h"
#include "mynteye/logger.h"
MYNTEYE_BEGIN_NAMESPACE
Correspondence::Correspondence(const std::shared_ptr<Device> &device,
const Stream &stream)
: device_(device), stream_(stream), ready_image_timestamp_(0) {
VLOG(2) << __func__;
// set matched stream to be watched too,
// aim to make stream and matched stream correspondence
if (stream_ == Stream::LEFT) {
stream_match_ = Stream::RIGHT;
} else if (stream_ == Stream::RIGHT) {
stream_match_ = Stream::LEFT;
} else if (stream_ == Stream::LEFT_RECTIFIED) {
stream_match_ = Stream::RIGHT_RECTIFIED;
} else if (stream_ == Stream::RIGHT_RECTIFIED) {
stream_match_ = Stream::LEFT_RECTIFIED;
} else {
stream_match_ = Stream::LAST;
}
EnableStreamMatch();
auto framerate = device_->GetOptionValue(Option::FRAME_RATE);
stream_interval_us_ = 1000000.f / framerate;
stream_interval_us_half_ = 0.5f * stream_interval_us_;
VLOG(2) << "framerate: " << framerate
<< ", interval_us: " << stream_interval_us_;
}
Correspondence::~Correspondence() {
VLOG(2) << __func__;
}
bool Correspondence::Watch(const Stream &stream) const {
if (stream == stream_) return true;
if (stream_match_enabled_ && stream == stream_match_) return true;
return false;
}
void Correspondence::OnStreamDataCallback(
const Stream &stream, const api::StreamData &data) {
if (!Watch(stream)) {
return; // unwatched
}
// LOG(INFO) << __func__ << ", " << stream
// << ", id: " << data.frame_id << ", stamp: " << data.img->timestamp;
// if (data.img == nullptr) {
// LOG(FATAL) << "stream data image info is empty!";
// }
std::lock_guard<std::recursive_mutex> _(mtx_stream_datas_);
if (stream == stream_) {
stream_datas_.push_back(std::move(data));
} else if (/*stream_match_enabled_ && */stream == stream_match_) {
stream_datas_match_.push_back(std::move(data));
}
NotifyStreamDataReady();
}
void Correspondence::OnMotionDataCallback(const device::MotionData &data) {
// LOG(INFO) << __func__ << ", id: " << data.imu->frame_id
// << ", stamp: " << data.imu->timestamp;
{
std::lock_guard<std::recursive_mutex> _(mtx_motion_datas_);
motion_datas_.push_back(data);
}
if (motion_callback_) {
motion_callback_({data.imu});
}
}
void Correspondence::SetMotionCallback(API::motion_callback_t callback) {
// LOG(INFO) << __func__;
motion_callback_ = callback;
}
void Correspondence::WaitForStreams() {
if (stream_ == Stream::LEFT || stream_ == Stream::RIGHT) {
// Wait native stream ready, avoid get these stream empty
// Todo: determine native stream according to device
WaitStreamDataReady();
return;
}
device_->WaitForStreams();
}
api::StreamData Correspondence::GetStreamData(const Stream &stream) {
auto datas = GetStreamDatas(stream);
return datas.empty() ? api::StreamData{} : datas.back();
}
std::vector<api::StreamData> Correspondence::GetStreamDatas(
const Stream &stream) {
if (!Watch(stream)) {
LOG(ERROR) << "Get unwatched stream data of " << stream;
return {};
}
std::lock_guard<std::recursive_mutex> _(mtx_stream_datas_);
static std::uint32_t stream_count_ = 0;
static std::uint32_t stream_match_count_ = 0;
if (stream == stream_) {
auto datas = GetReadyStreamData(false);
if (stream_count_ < 10) {
++stream_count_;
} else {
// get stream, but not get matched stream, disable it
if (stream_match_count_ == 0) {
DisableStreamMatch();
}
}
return datas;
} else if (/*stream_match_enabled_ && */stream == stream_match_) {
auto datas = GetReadyStreamData(true);
if (stream_match_count_ < 10) {
++stream_match_count_;
}
return datas;
}
return {};
}
std::vector<api::MotionData> Correspondence::GetMotionDatas() {
return GetReadyMotionDatas();
}
void Correspondence::EnableStreamMatch() {
stream_match_enabled_ = true;
}
void Correspondence::DisableStreamMatch() {
stream_match_enabled_ = false;
stream_datas_match_.clear();
}
void Correspondence::WaitStreamDataReady() {
std::unique_lock<std::recursive_mutex> lock(mtx_stream_datas_);
auto ready = std::bind(&Correspondence::IsStreamDataReady, this);
bool ok = cond_stream_datas_.wait_for(lock, std::chrono::seconds(3), ready);
if (!ok) {
LOG(FATAL) << "Timeout waiting for key frames. Please use USB 3.0, and not "
"in virtual machine.";
}
}
void Correspondence::NotifyStreamDataReady() {
cond_stream_datas_.notify_one();
}
bool Correspondence::IsStreamDataReady() {
if (stream_datas_.empty()) return false;
if (stream_match_enabled_) {
if (stream_datas_match_.empty()) return false;
}
if (motion_datas_.empty()) return false;
std::uint64_t img_stamp = 0;
std::uint64_t img_macth_stamp = 0;
{
std::lock_guard<std::recursive_mutex> _(mtx_stream_datas_);
auto data = stream_datas_.front();
if (data.img == nullptr) {
LOG(FATAL) << "stream data image info is empty!";
}
img_stamp = data.img->timestamp;
if (stream_match_enabled_) {
img_macth_stamp = stream_datas_match_.front().img->timestamp;
}
}
std::uint64_t imu_stamp = 0;
{
std::lock_guard<std::recursive_mutex> _(mtx_motion_datas_);
auto data = motion_datas_.back();
if (data.imu == nullptr) {
LOG(FATAL) << "motion data imu info is empty!";
}
imu_stamp = data.imu->timestamp;
}
if (stream_match_enabled_) {
return img_stamp + stream_interval_us_half_ < imu_stamp
&& img_macth_stamp + stream_interval_us_half_ < imu_stamp;
} else {
return img_stamp + stream_interval_us_half_ < imu_stamp;
}
}
std::vector<api::StreamData> Correspondence::GetReadyStreamData(bool matched) {
std::uint64_t imu_stamp = 0;
{
std::lock_guard<std::recursive_mutex> _(mtx_motion_datas_);
if (motion_datas_.empty()) {
LOG(WARNING) << "motion data is unexpected empty!"
"\n\n Please ensure Start(Source::MOTION_TRACKING) "
"or Start(Source::ALL)\n";
std::lock_guard<std::recursive_mutex> _(mtx_stream_datas_);
return std::move(matched ? stream_datas_match_ : stream_datas_);
}
imu_stamp = motion_datas_.back().imu->timestamp;
}
std::lock_guard<std::recursive_mutex> _(mtx_stream_datas_);
std::vector<api::StreamData> &datas =
matched ? stream_datas_match_ : stream_datas_;
// LOG(INFO) << "datas.size: " << datas.size() << ", matched: " << matched;
std::vector<api::StreamData> result;
for (auto it = datas.begin(); it != datas.end(); ) {
// LOG(INFO) << "data.id: " << it->frame_id;
auto img_stamp = it->img->timestamp;
if (img_stamp + stream_interval_us_half_ < imu_stamp) {
// LOG(INFO) << "data.id: " << it->frame_id << " > result";
result.push_back(std::move(*it));
it = datas.erase(it);
} else {
// ++it;
break;
}
}
// LOG(INFO) << "datas.size: " << datas.size()
// << ", result.size: " << result.size();
if (!matched && !result.empty()) {
// last match stream timestamp
ready_image_timestamp_ = result.back().img->timestamp;
}
return result;
}
std::vector<api::MotionData> Correspondence::GetReadyMotionDatas() {
if (ready_image_timestamp_ == 0) return {};
std::lock_guard<std::recursive_mutex> _(mtx_motion_datas_);
std::vector<api::MotionData> result;
auto &&datas = motion_datas_;
for (auto it = datas.begin(); it != datas.end(); ) {
auto imu_stamp = it->imu->timestamp;
if (imu_stamp < ready_image_timestamp_ - stream_interval_us_half_) {
it = datas.erase(it);
} else if (imu_stamp > ready_image_timestamp_ + stream_interval_us_half_) {
// ++it;
break;
} else {
result.push_back({it->imu});
it = datas.erase(it);
}
}
return result;
}
MYNTEYE_END_NAMESPACE

80
src/mynteye/api/correspondence.h Normal file
View File

@ -0,0 +1,80 @@
// 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_API_CONFIG_H_
#define MYNTEYE_API_CONFIG_H_
#pragma once
#include <atomic>
#include <condition_variable>
#include <memory>
#include <mutex>
#include <vector>
#include "mynteye/api/api.h"
#include "mynteye/device/callbacks.h"
MYNTEYE_BEGIN_NAMESPACE
class Correspondence {
public:
Correspondence(const std::shared_ptr<Device> &device, const Stream &stream);
~Correspondence();
bool Watch(const Stream &stream) const;
void OnStreamDataCallback(const Stream &stream, const api::StreamData &data);
void OnMotionDataCallback(const device::MotionData &data);
void SetMotionCallback(API::motion_callback_t callback);
void WaitForStreams();
api::StreamData GetStreamData(const Stream &stream);
std::vector<api::StreamData> GetStreamDatas(const Stream &stream);
std::vector<api::MotionData> GetMotionDatas();
private:
void EnableStreamMatch();
void DisableStreamMatch();
void WaitStreamDataReady();
void NotifyStreamDataReady();
bool IsStreamDataReady();
std::vector<api::StreamData> GetReadyStreamData(bool matched);
std::vector<api::MotionData> GetReadyMotionDatas();
std::shared_ptr<Device> device_;
Stream stream_;
Stream stream_match_;
std::atomic_bool stream_match_enabled_;
float stream_interval_us_;
float stream_interval_us_half_;
API::motion_callback_t motion_callback_;
std::vector<device::MotionData> motion_datas_;
std::recursive_mutex mtx_motion_datas_;
std::vector<api::StreamData> stream_datas_;
std::vector<api::StreamData> stream_datas_match_;
std::recursive_mutex mtx_stream_datas_;
std::condition_variable_any cond_stream_datas_;
std::uint64_t ready_image_timestamp_;
};
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_API_CONFIG_H_

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

@ -74,6 +74,44 @@ void process_childs(
}
}
// 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() {
@ -105,7 +143,8 @@ Synthetic::Synthetic(API *api, CalibrationModel calib_model)
: api_(api),
plugin_(nullptr),
calib_model_(calib_model),
calib_default_tag_(false) {
calib_default_tag_(false),
stream_data_listener_(nullptr) {
VLOG(2) << __func__;
CHECK_NOTNULL(api_);
InitCalibInfo();
@ -121,6 +160,10 @@ Synthetic::~Synthetic() {
}
}
void Synthetic::SetStreamDataListener(stream_data_listener_t listener) {
stream_data_listener_ = listener;
}
void Synthetic::NotifyImageParamsChanged() {
if (!calib_default_tag_) {
intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT);
@ -195,27 +238,6 @@ bool Synthetic::checkControlDateWithStream(const Stream& stream) const {
return false;
}
void Synthetic::EnableStreamData(const Stream &stream) {
// Activate processors of synthetic stream
auto processor = getProcessorWithStream(stream);
iterate_processor_CtoP_before(processor,
[](std::shared_ptr<Processor> proce){
auto streams = proce->getTargetStreams();
int act_tag = 0;
for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) {
if (proce->target_streams_[i].enabled_mode_ == MODE_LAST) {
act_tag++;
proce->target_streams_[i].enabled_mode_ = MODE_SYNTHETIC;
}
}
if (act_tag > 0 && !proce->IsActivated()) {
// std::cout << proce->Name() << " Active now" << std::endl;
proce->Activate();
}
});
}
bool Synthetic::Supports(const Stream &stream) const {
return checkControlDateWithStream(stream);
}
@ -228,16 +250,45 @@ Synthetic::mode_t Synthetic::SupportsMode(const Stream &stream) const {
return MODE_LAST;
}
void Synthetic::DisableStreamData(const Stream &stream) {
void Synthetic::EnableStreamData(
const Stream &stream, stream_switch_callback_t callback,
bool try_tag) {
// Activate processors of synthetic stream
auto processor = getProcessorWithStream(stream);
iterate_processor_CtoP_before(processor,
[callback, try_tag](std::shared_ptr<Processor> proce){
auto streams = proce->getTargetStreams();
int act_tag = 0;
for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) {
if (proce->target_streams_[i].enabled_mode_ == MODE_LAST) {
callback(proce->target_streams_[i].stream);
if (!try_tag) {
act_tag++;
proce->target_streams_[i].enabled_mode_ = MODE_SYNTHETIC;
}
}
}
if (act_tag > 0 && !proce->IsActivated()) {
// std::cout << proce->Name() << " Active now" << std::endl;
proce->Activate();
}
});
}
void Synthetic::DisableStreamData(
const Stream &stream, stream_switch_callback_t callback,
bool try_tag) {
auto processor = getProcessorWithStream(stream);
iterate_processor_PtoC_before(processor,
[](std::shared_ptr<Processor> proce){
[callback, try_tag](std::shared_ptr<Processor> proce){
auto streams = proce->getTargetStreams();
int act_tag = 0;
for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) {
if (proce->target_streams_[i].enabled_mode_ == MODE_SYNTHETIC) {
act_tag++;
proce->target_streams_[i].enabled_mode_ = MODE_LAST;
callback(proce->target_streams_[i].stream);
if (!try_tag) {
act_tag++;
proce->target_streams_[i].enabled_mode_ = MODE_LAST;
}
}
}
if (act_tag > 0 && proce->IsActivated()) {
@ -247,6 +298,20 @@ void Synthetic::DisableStreamData(const Stream &stream) {
});
}
void Synthetic::EnableStreamData(const Stream &stream) {
EnableStreamData(stream, [](const Stream &stream){
// std::cout << stream << "enabled in callback" << std::endl;
MYNTEYE_UNUSED(stream);
}, false);
}
void Synthetic::DisableStreamData(const Stream &stream) {
DisableStreamData(stream, [](const Stream &stream){
// std::cout << stream << "disabled in callback" << std::endl;
MYNTEYE_UNUSED(stream);
}, false);
}
bool Synthetic::IsStreamDataEnabled(const Stream &stream) const {
if (checkControlDateWithStream(stream)) {
auto data = getControlDateWithStream(stream);
@ -335,7 +400,7 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) {
if (out != nullptr) {
auto &&output = Object::Cast<ObjMat>(out);
if (output != nullptr) {
return {output->data, output->value, nullptr, output->id};
return obj_data(output);
}
VLOG(2) << "Rectify not ready now";
}
@ -349,15 +414,9 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) {
for (auto it : streams) {
if (it.stream == stream) {
if (num == 1) {
return {output->first_data,
output->first,
nullptr,
output->first_id};
return obj_data_first(output);
} else {
return {output->second_data,
output->second,
nullptr,
output->second_id};
return obj_data_second(output);
}
}
num++;
@ -452,66 +511,60 @@ bool Synthetic::IsStreamEnabledSynthetic(const Stream &stream) const {
void Synthetic::InitProcessors() {
std::shared_ptr<Processor> rectify_processor = nullptr;
#ifdef WITH_CAM_MODELS
std::shared_ptr<RectifyProcessor> rectify_processor_imp = nullptr;
#endif
cv::Mat Q;
if (calib_model_ == CalibrationModel::PINHOLE) {
auto &&rectify_processor_ocv =
std::make_shared<RectifyProcessorOCV>(intr_left_, intr_right_, extr_,
RECTIFY_PROC_PERIOD);
Q = rectify_processor_ocv->Q;
rectify_processor = rectify_processor_ocv;
#ifdef WITH_CAM_MODELS
} else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
rectify_processor_imp =
std::make_shared<RectifyProcessor>(intr_left_, intr_right_, extr_,
RECTIFY_PROC_PERIOD);
rectify_processor = rectify_processor_imp;
#endif
} else {
LOG(ERROR) << "Unknow calib model type in device: "
<< calib_model_ << ", use default pinhole model";
auto &&rectify_processor_ocv =
std::make_shared<RectifyProcessorOCV>(intr_left_, intr_right_, extr_,
RECTIFY_PROC_PERIOD);
rectify_processor = rectify_processor_ocv;
}
std::shared_ptr<Processor> points_processor = nullptr;
std::shared_ptr<Processor> depth_processor = nullptr;
auto &&disparity_processor =
std::make_shared<DisparityProcessor>(DisparityComputingMethod::SGBM,
DISPARITY_PROC_PERIOD);
auto &&disparitynormalized_processor =
std::make_shared<DisparityNormalizedProcessor>(
DISPARITY_NORM_PROC_PERIOD);
std::shared_ptr<Processor> points_processor = nullptr;
if (calib_model_ == CalibrationModel::PINHOLE) {
auto root_processor =
std::make_shared<RootProcessor>(ROOT_PROC_PERIOD);
if (calib_model_ == CalibrationModel::PINHOLE) {
// PINHOLE
auto &&rectify_processor_ocv =
std::make_shared<RectifyProcessorOCV>(intr_left_, intr_right_, extr_,
RECTIFY_PROC_PERIOD);
rectify_processor = rectify_processor_ocv;
points_processor = std::make_shared<PointsProcessorOCV>(
Q, POINTS_PROC_PERIOD);
rectify_processor_ocv->Q, POINTS_PROC_PERIOD);
depth_processor = std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD);
root_processor->AddChild(rectify_processor);
rectify_processor->AddChild(disparity_processor);
disparity_processor->AddChild(disparitynormalized_processor);
disparity_processor->AddChild(points_processor);
points_processor->AddChild(depth_processor);
#ifdef WITH_CAM_MODELS
} else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
// KANNALA_BRANDT
auto rectify_processor_imp =
std::make_shared<RectifyProcessor>(intr_left_, intr_right_, extr_,
RECTIFY_PROC_PERIOD);
rectify_processor = rectify_processor_imp;
points_processor = std::make_shared<PointsProcessor>(
rectify_processor_imp -> getCalibInfoPair(),
POINTS_PROC_PERIOD);
#endif
} else {
points_processor = std::make_shared<PointsProcessorOCV>(
Q, POINTS_PROC_PERIOD);
}
std::shared_ptr<Processor> depth_processor = nullptr;
if (calib_model_ == CalibrationModel::PINHOLE) {
depth_processor = std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD);
#ifdef WITH_CAM_MODELS
} else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
depth_processor = std::make_shared<DepthProcessor>(
rectify_processor_imp -> getCalibInfoPair(),
DEPTH_PROC_PERIOD);
root_processor->AddChild(rectify_processor);
rectify_processor->AddChild(disparity_processor);
disparity_processor->AddChild(disparitynormalized_processor);
disparity_processor->AddChild(depth_processor);
depth_processor->AddChild(points_processor);
#endif
} else {
depth_processor = std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD);
// UNKNOW
LOG(ERROR) << "Unknow calib model type in device: "
<< calib_model_;
return;
}
auto root_processor =
std::make_shared<RootProcessor>(ROOT_PROC_PERIOD);
root_processor->AddChild(rectify_processor);
rectify_processor->addTargetStreams(
{Stream::LEFT_RECTIFIED, Mode::MODE_LAST, Mode::MODE_LAST, nullptr});
@ -559,30 +612,14 @@ void Synthetic::InitProcessors() {
depth_processor->SetPostProcessCallback(
std::bind(&Synthetic::OnDepthPostProcess, this, _1));
if (calib_model_ == CalibrationModel::PINHOLE) {
// PINHOLE
rectify_processor->AddChild(disparity_processor);
disparity_processor->AddChild(disparitynormalized_processor);
disparity_processor->AddChild(points_processor);
points_processor->AddChild(depth_processor);
} else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
// KANNALA_BRANDT
rectify_processor->AddChild(disparity_processor);
disparity_processor->AddChild(disparitynormalized_processor);
disparity_processor->AddChild(depth_processor);
depth_processor->AddChild(points_processor);
} else {
// UNKNOW
LOG(ERROR) << "Unknow calib model type in device: "
<< calib_model_;
}
processor_ = rectify_processor;
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;
@ -603,9 +640,7 @@ void Synthetic::ProcessNativeStream(
<< calib_model_ << ", use default pinhole model";
processor = find_processor<RectifyProcessorOCV>(processor_);
}
processor->Process(ObjMat2{
left_data.frame, left_data.frame_id, left_data.img,
right_data.frame, right_data.frame_id, right_data.img});
processor->Process(data_obj(left_data, right_data));
}
return;
}
@ -627,34 +662,28 @@ void Synthetic::ProcessNativeStream(
name = RectifyProcessor::NAME;
#endif
}
process_childs(
processor_, name, ObjMat2{
left_rect_data.frame, left_rect_data.frame_id, left_rect_data.img,
right_rect_data.frame, right_rect_data.frame_id,
right_rect_data.img});
process_childs(processor_, name,
data_obj(left_rect_data, right_rect_data));
}
return;
}
switch (stream) {
case Stream::DISPARITY: {
process_childs(processor_, DisparityProcessor::NAME,
ObjMat{data.frame, data.frame_id, data.img});
process_childs(processor_, DisparityProcessor::NAME, data_obj(data));
} break;
case Stream::DISPARITY_NORMALIZED: {
process_childs(processor_, DisparityNormalizedProcessor::NAME,
ObjMat{data.frame, data.frame_id, data.img});
data_obj(data));
} break;
case Stream::POINTS: {
if (calib_model_ == CalibrationModel::PINHOLE) {
// PINHOLE
process_childs(processor_, PointsProcessorOCV::NAME,
ObjMat{data.frame, data.frame_id, data.img});
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,
ObjMat{data.frame, data.frame_id, data.img});
process_childs(processor_, PointsProcessor::NAME, data_obj(data));
#endif
} else {
// UNKNOW
@ -665,13 +694,11 @@ void Synthetic::ProcessNativeStream(
case Stream::DEPTH: {
if (calib_model_ == CalibrationModel::PINHOLE) {
// PINHOLE
process_childs(processor_, DepthProcessorOCV::NAME,
ObjMat{data.frame, data.frame_id, data.img});
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,
ObjMat{data.frame, data.frame_id, data.img});
process_childs(processor_, DepthProcessor::NAME, data_obj(data));
#endif
} else {
// UNKNOW
@ -737,51 +764,51 @@ bool Synthetic::OnDepthProcess(
void Synthetic::OnRectifyPostProcess(Object *const out) {
const ObjMat2 *output = Object::Cast<ObjMat2>(out);
NotifyStreamData(Stream::LEFT_RECTIFIED, obj_data_first(output));
NotifyStreamData(Stream::RIGHT_RECTIFIED, obj_data_second(output));
if (HasStreamCallback(Stream::LEFT_RECTIFIED)) {
auto data = getControlDateWithStream(Stream::LEFT_RECTIFIED);
data.stream_callback(
{output->first_data, output->first, nullptr, output->first_id});
data.stream_callback(obj_data_first(output));
}
if (HasStreamCallback(Stream::RIGHT_RECTIFIED)) {
auto data = getControlDateWithStream(Stream::RIGHT_RECTIFIED);
data.stream_callback(
{output->second_data, output->second, nullptr, output->second_id});
data.stream_callback(obj_data_second(output));
}
}
void Synthetic::OnDisparityPostProcess(Object *const out) {
const ObjMat *output = Object::Cast<ObjMat>(out);
NotifyStreamData(Stream::DISPARITY, obj_data(output));
if (HasStreamCallback(Stream::DISPARITY)) {
auto data = getControlDateWithStream(Stream::DISPARITY);
data.stream_callback(
{output->data, output->value, nullptr, output->id});
data.stream_callback(obj_data(output));
}
}
void Synthetic::OnDisparityNormalizedPostProcess(Object *const out) {
const ObjMat *output = Object::Cast<ObjMat>(out);
NotifyStreamData(Stream::DISPARITY_NORMALIZED, obj_data(output));
if (HasStreamCallback(Stream::DISPARITY_NORMALIZED)) {
auto data = getControlDateWithStream(Stream::DISPARITY_NORMALIZED);
data.stream_callback(
{output->data, output->value, nullptr, output->id});
data.stream_callback(obj_data(output));
}
}
void Synthetic::OnPointsPostProcess(Object *const out) {
const ObjMat *output = Object::Cast<ObjMat>(out);
NotifyStreamData(Stream::POINTS, obj_data(output));
if (HasStreamCallback(Stream::POINTS)) {
auto data = getControlDateWithStream(Stream::POINTS);
data.stream_callback(
{output->data, output->value, nullptr, output->id});
data.stream_callback(obj_data(output));
}
}
void Synthetic::OnDepthPostProcess(Object *const out) {
const ObjMat *output = Object::Cast<ObjMat>(out);
NotifyStreamData(Stream::DEPTH, obj_data(output));
if (HasStreamCallback(Stream::DEPTH)) {
auto data = getControlDateWithStream(Stream::DEPTH);
data.stream_callback(
{output->data, output->value, nullptr, output->id});
data.stream_callback(obj_data(output));
}
}
@ -796,4 +823,11 @@ void Synthetic::SetDisparityComputingMethodType(
LOG(ERROR) << "ERROR: no suited processor for disparity computing.";
}
void Synthetic::NotifyStreamData(
const Stream &stream, const api::StreamData &data) {
if (stream_data_listener_) {
stream_data_listener_(stream, data);
}
}
MYNTEYE_END_NAMESPACE

16
src/mynteye/api/synthetic.h
View File

@ -19,6 +19,7 @@
#include <memory>
#include <string>
#include <vector>
#include <mutex>
#include "mynteye/api/api.h"
#include "mynteye/api/config.h"
@ -34,6 +35,9 @@ struct Object;
class Synthetic {
public:
using stream_callback_t = API::stream_callback_t;
using stream_data_listener_t =
std::function<void(const Stream &stream, const api::StreamData &data)>;
using stream_switch_callback_t = API::stream_switch_callback_t;
typedef enum Mode {
MODE_NATIVE, // Native stream
@ -51,6 +55,8 @@ class Synthetic {
explicit Synthetic(API *api, CalibrationModel calib_model);
~Synthetic();
void SetStreamDataListener(stream_data_listener_t listener);
void NotifyImageParamsChanged();
bool Supports(const Stream &stream) const;
@ -58,6 +64,11 @@ class Synthetic {
void EnableStreamData(const Stream &stream);
void DisableStreamData(const Stream &stream);
void EnableStreamData(
const Stream &stream, stream_switch_callback_t callback, bool try_tag);
void DisableStreamData(
const Stream &stream, stream_switch_callback_t callback, bool try_tag);
bool IsStreamDataEnabled(const Stream &stream) const;
void SetStreamCallback(const Stream &stream, stream_callback_t callback);
@ -125,6 +136,8 @@ class Synthetic {
void OnPointsPostProcess(Object *const out);
void OnDepthPostProcess(Object *const out);
void NotifyStreamData(const Stream &stream, const api::StreamData &data);
API *api_;
std::shared_ptr<Processor> processor_;
@ -132,6 +145,7 @@ class Synthetic {
std::shared_ptr<Plugin> plugin_;
CalibrationModel calib_model_;
std::mutex mtx_left_right_ready_;
std::shared_ptr<IntrinsicsBase> intr_left_;
std::shared_ptr<IntrinsicsBase> intr_right_;
@ -139,6 +153,8 @@ class Synthetic {
bool calib_default_tag_;
std::vector<std::shared_ptr<Processor>> processors_;
stream_data_listener_t stream_data_listener_;
};
class SyntheticProcessorPart {

136
src/mynteye/api/version_checker.cc Normal file
View File

@ -0,0 +1,136 @@
// 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.
#include "mynteye/api/version_checker.h"
#include "mynteye/device/utils.h"
#include "mynteye/logger.h"
#include "mynteye/types.h"
MYNTEYE_BEGIN_NAMESPACE
typedef struct {
const std::string device_type;
const std::string sdk_version;
const std::string firmware_version;
const std::string status;
}firmware_version_match_table_unit;
const char* ERRO_DESCRIPTION_F =
"Please update the firmware at first";
const char* ERRO_DESCRIPTION_S =
"Please update the SDK at first";
const char* WARN_DESCRIPTION_F =
"We suggest that you should update the firmware";
const char* WARN_DESCRIPTION_S =
"We suggest that you should update the SDK";
const char* PASS_DESCRIPTION = "pass";
/** firmware/sdk version matched table */
/**----device type-----sdk version---firmware version-----pass tag-----*/
static const firmware_version_match_table_unit FSVM_TABLE[] ={
/** S1030 */
{"MYNT-EYE-S1030", ">2.3.0", ">2.2.0", PASS_DESCRIPTION},
{"MYNT-EYE-S1030", ">2.3.0", "2.2.0", WARN_DESCRIPTION_F},
{"MYNT-EYE-S1030", ">2.3.0", "<2.2.0", ERRO_DESCRIPTION_F},
{"MYNT-EYE-S1030", "<2.3.1", "<2.2.0", WARN_DESCRIPTION_S},
/** S2100 */
{"MYNT-EYE-S2100", ">2.3.0", "1.0", PASS_DESCRIPTION},
{"MYNT-EYE-S2100", "<2.3.1", "1.0", ERRO_DESCRIPTION_S},
/** S210A */
{"MYNT-EYE-S210A", ">2.3.0", "1.0", PASS_DESCRIPTION},
{"MYNT-EYE-S210A", "<2.3.1", "1.0", ERRO_DESCRIPTION_S},
};
void getVersion(const std::string &str, char *version) {
std::string st1("");
int j = 0;
for (size_t i = 0; i < str.size(); i++) {
if (str[i] == '.') {
version[j++] = atoi(st1.c_str());
st1 = "";
} else {
st1 += str[i];
}
}
version[j++] = atoi(st1.c_str());
}
bool conditionMatch(const std::string& condition, const std::string& target) {
char version[4] = {0};
char version_c[4] = {0};
getVersion(target, version);
int tag_c = 0;
std::string condition_c;
if (condition[0] == '>') {
tag_c = 1;
condition_c = condition.substr(1);
} else if (condition[0] == '<') {
tag_c = -1;
condition_c = condition.substr(1);
} else {
tag_c = 0;
condition_c = condition;
}
getVersion(condition_c, version_c);
int tag_big = memcmp(version, version_c, 4);
if (tag_big * tag_c > 0 || (tag_big == 0 && tag_c == 0)) return true;
return false;
}
enum STATUS_UNIT {
ST_PASS,
ST_ERRO_F,
ST_ERRO_S,
ST_NOT_PASS
};
STATUS_UNIT checkUnit(const std::string& sdkv,
const std::string& devn,
const std::string& firmv,
const firmware_version_match_table_unit& condition) {
if (condition.device_type == devn &&
conditionMatch(condition.sdk_version, sdkv) &&
conditionMatch(condition.firmware_version, firmv)) {
if (condition.status == ERRO_DESCRIPTION_F) return ST_ERRO_F;
if (condition.status == ERRO_DESCRIPTION_S) return ST_ERRO_S;
if (condition.status == WARN_DESCRIPTION_F ||
condition.status == WARN_DESCRIPTION_S) {
LOG(WARNING) << condition.status;
}
return ST_PASS;
}
return ST_NOT_PASS;
}
bool checkFirmwareVersion(const std::shared_ptr<API> api) {
auto sdkv = api->GetInfo(Info::SDK_VERSION);
auto devn = api->GetInfo(Info::DEVICE_NAME);
auto firmv = api->GetInfo(Info::FIRMWARE_VERSION);
for (size_t i =0;
i < sizeof(FSVM_TABLE)/sizeof(firmware_version_match_table_unit);
i++) {
auto res = checkUnit(sdkv, devn, firmv, FSVM_TABLE[i]);
if (res == ST_PASS) {
return true;
} else if (res == ST_ERRO_S || res == ST_ERRO_F) {
LOG(ERROR) << FSVM_TABLE[i].status;
return false;
}
}
LOG(ERROR) << ERRO_DESCRIPTION_S;
return false;
}
MYNTEYE_END_NAMESPACE

25
src/mynteye/api/version_checker.h Normal file
View File

@ -0,0 +1,25 @@
// 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_API_VERSION_CHECKER_H_
#define MYNTEYE_API_VERSION_CHECKER_H_
#pragma once
#include <string>
#include "mynteye/api/api.h"
MYNTEYE_BEGIN_NAMESPACE
bool checkFirmwareVersion(const std::shared_ptr<API> api);
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_API_VERSION_CHECKER_H_

1
src/mynteye/device/channel/channels.cc
View File

@ -460,6 +460,7 @@ bool Channels::GetFiles(
while (i < end) {
std::uint8_t file_id = *(data + i);
std::uint16_t file_size = bytes::_from_data<std::uint16_t>(data + i + 1);
LOG(INFO) << "GetFiles:data_size : " << file_size;
VLOG(2) << "GetFiles id: " << static_cast<int>(file_id)
<< ", size: " << file_size;
i += 3;

32
src/mynteye/device/channel/file_channel.cc
View File

@ -32,6 +32,7 @@ std::size_t FileChannel::GetDeviceInfoFromData(
const std::uint8_t *data, const std::uint16_t &data_size,
device_info_t *info) {
auto n = dev_info_parser_->GetFromData(data, data_size, info);
LOG(INFO) << "GetDeviceInfoFromData:data_size : " << data_size;
auto spec_version = info->spec_version;
img_params_parser_->SetSpecVersion(spec_version);
imu_params_parser_->SetSpecVersion(spec_version);
@ -113,6 +114,22 @@ std::size_t DeviceInfoParser::GetFromData(
info->nominal_baseline = bytes::_from_data<std::uint16_t>(data + i);
i += 2;
if (info->spec_version >= Version(1, 2)) {
// auxiliary_chip_version, 2
info->auxiliary_chip_version.set_major(data[i]);
info->auxiliary_chip_version.set_minor(data[i + 1]);
i += 2;
// isp_version, 2
info->isp_version.set_major(data[i]);
info->isp_version.set_minor(data[i + 1]);
i += 2;
} else {
info->auxiliary_chip_version.set_major(0);
info->auxiliary_chip_version.set_minor(0);
info->isp_version.set_major(0);
info->isp_version.set_minor(0);
}
// get other infos according to spec_version
MYNTEYE_UNUSED(data_size)
@ -155,6 +172,17 @@ std::size_t DeviceInfoParser::SetToData(
bytes::_to_data(info->nominal_baseline, data + i);
i += 2;
if (info->spec_version >= Version(1, 2)) {
// auxiliary_chip_version, 2
data[i] = info->auxiliary_chip_version.major();
data[i + 1] = info->auxiliary_chip_version.minor();
i += 2;
// isp_version, 2
data[i] = info->isp_version.major();
data[i + 1] = info->isp_version.minor();
i += 2;
}
// set other infos according to spec_version
// others
@ -181,7 +209,7 @@ std::size_t ImgParamsParser::GetFromData(
return GetFromData_v1_0(data, data_size, img_params);
}
// s210a old params
if (spec_version_ == Version(1, 1) && data_size == 404) {
if (spec_version_ >= Version(1, 1) && data_size == 404) {
return GetFromData_v1_1(data, data_size, img_params);
}
// get img params with new version format
@ -406,7 +434,7 @@ std::size_t ImuParamsParser::GetFromData(
return GetFromData_old(data, data_size, imu_params);
}
// s210a old params
if (spec_version_ == Version(1, 1) && data_size == 384) {
if (spec_version_ >= Version(1, 1) && data_size == 384) {
return GetFromData_old(data, data_size, imu_params);
}
// get imu params with new version format

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

@ -234,6 +234,10 @@ std::string Device::GetInfo(const Info &info) const {
return device_info_->imu_type.to_string();
case Info::NOMINAL_BASELINE:
return std::to_string(device_info_->nominal_baseline);
case Info::AUXILIARY_CHIP_VERSION:
return device_info_->auxiliary_chip_version.to_string();
case Info::ISP_VERSION:
return device_info_->isp_version.to_string();
default:
LOG(WARNING) << "Unknown device info";
return "";
@ -349,6 +353,9 @@ OptionInfo Device::GetOptionInfo(const Option &option) const {
std::int32_t Device::GetOptionValue(const Option &option) const {
if (!Supports(option)) {
if (option == Option::FRAME_RATE) {
return GetStreamRequest().fps;
}
LOG(WARNING) << "Unsupported option: " << option;
return -1;
}
@ -466,6 +473,11 @@ std::vector<device::StreamData> Device::GetStreamDatas(const Stream &stream) {
return streams_->GetStreamDatas(stream);
}
void Device::DisableMotionDatas() {
CHECK_NOTNULL(motions_);
motions_->DisableMotionDatas();
}
void Device::EnableMotionDatas() {
EnableMotionDatas(std::numeric_limits<std::size_t>::max());
}

15
src/mynteye/device/motions.cc
View File

@ -66,7 +66,10 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
std::lock_guard<std::mutex> _(mtx_datas_);
motion_data_t data = {imu};
if (motion_datas_enabled_) {
if (motion_datas_enabled_ && motion_datas_max_size_ > 0) {
if (motion_datas_.size() >= motion_datas_max_size_) {
motion_datas_.erase(motion_datas_.begin());
}
motion_datas_.push_back(data);
}
@ -98,13 +101,21 @@ void Motions::StopMotionTracking() {
}
}
void Motions::DisableMotionDatas() {
std::lock_guard<std::mutex> _(mtx_datas_);
motion_datas_enabled_ = false;
motion_datas_max_size_ = 0;
motion_datas_.clear();
}
void Motions::EnableMotionDatas(std::size_t max_size) {
if (max_size <= 0) {
LOG(WARNING) << "Could not enable motion datas with max_size <= 0";
return;
}
std::lock_guard<std::mutex> _(mtx_datas_);
motion_datas_enabled_ = true;
motion_datas_max_size = max_size;
motion_datas_max_size_ = max_size;
}
Motions::motion_datas_t Motions::GetMotionDatas() {

3
src/mynteye/device/motions.h
View File

@ -42,6 +42,7 @@ class Motions {
void StartMotionTracking();
void StopMotionTracking();
void DisableMotionDatas();
void EnableMotionDatas(std::size_t max_size);
motion_datas_t GetMotionDatas();
@ -52,7 +53,7 @@ class Motions {
motion_datas_t motion_datas_;
bool motion_datas_enabled_;
std::size_t motion_datas_max_size;
std::size_t motion_datas_max_size_;
bool is_imu_tracking;

2
src/mynteye/types.cc
View File

@ -92,6 +92,8 @@ const char *to_string(const Info &value) {
CASE(LENS_TYPE)
CASE(IMU_TYPE)
CASE(NOMINAL_BASELINE)
CASE(AUXILIARY_CHIP_VERSION)
CASE(ISP_VERSION)
default:
CHECK(is_valid(value));
return "Info::UNKNOWN";

2
test/types_test.cc
View File

@ -53,6 +53,8 @@ TEST(Info, VerifyToString) {
EXPECT_STREQ("Info::LENS_TYPE", to_string(Info::LENS_TYPE));
EXPECT_STREQ("Info::IMU_TYPE", to_string(Info::IMU_TYPE));
EXPECT_STREQ("Info::NOMINAL_BASELINE", to_string(Info::NOMINAL_BASELINE));
EXPECT_STREQ("Info::AUXILIARY_CHIP_VERSION", to_string(Info::AUXILIARY_CHIP_VERSION));
EXPECT_STREQ("Info::ISP_VERSION", to_string(Info::ISP_VERSION));
}
TEST(Option, VerifyToString) {

10
tools/writer/config/S210A/device.info
View File

@ -2,9 +2,11 @@
---
device_name: MYNT-EYE-S210A
serial_number: "07C40D1C0009071F"
firmware_version: "0.1"
firmware_version: "1.1"
hardware_version: "1.0"
spec_version: "1.1"
lens_type: "0000"
imu_type: "0000"
spec_version: "1.2"
lens_type: "0001"
imu_type: "0001"
nominal_baseline: 0
auxiliary_chip_version: "1.0"
isp_version: "1.0"

8
tools/writer/device_writer.cc
View File

@ -52,7 +52,11 @@ bool DeviceWriter::WriteDeviceInfo(const dev_info_t &info) {
<< ", spec_version: " << dev_info->spec_version.to_string()
<< ", lens_type: " << dev_info->lens_type.to_string()
<< ", imu_type: " << dev_info->imu_type.to_string()
<< ", nominal_baseline: " << dev_info->nominal_baseline << "}";
<< ", nominal_baseline: " << dev_info->nominal_baseline
<< ", auxiliary_chip_version: "
<< dev_info->auxiliary_chip_version.to_string()
<< ", isp_version: "
<< dev_info->isp_version.to_string()<< "}";
return true;
} else {
LOG(ERROR) << "Write device info failed";
@ -215,6 +219,8 @@ bool DeviceWriter::SaveDeviceInfo(
fs << "lens_type" << info.lens_type.to_string();
fs << "imu_type" << info.imu_type.to_string();
fs << "nominal_baseline" << info.nominal_baseline;
fs << "auxiliary_chip_version" << info.auxiliary_chip_version.to_string();
fs << "isp_version" << info.isp_version.to_string();
// save other infos according to spec_version
fs.release();
return true;

2
wrappers/python/src/mynteye_py.cc
View File

@ -247,6 +247,8 @@ BOOST_PYTHON_MODULE(mynteye_py) {
.value("LENS_TYPE", Info::LENS_TYPE)
.value("IMU_TYPE", Info::IMU_TYPE)
.value("NOMINAL_BASELINE", Info::NOMINAL_BASELINE)
.value("AUXILIARY_CHIP_VERSION", Info::AUXILIARY_CHIP_VERSION)
.value("ISP_VERSION", Info::ISP_VERSION)
#ifdef ENUM_EXPORT_VALUES
.export_values()
#endif

8
wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
View File

@ -153,7 +153,7 @@
<!-- <arg name="standard2/ir_control" default="0" /> -->
<!-- standard2/accel_range range: [6,48] -->
<arg name="standard2/accel_range" default="-1" />
<arg name="standard2/accel_range" default="24" />
<!-- <arg name="standard2/accel_range" default="6" /> -->
<!-- standard2/gyro_range range: [250,4000] -->
@ -347,6 +347,10 @@
- 'image_transport/compressedDepth'
</rosparam>
</group>
<group ns="$(arg depth_topic)">
<rosparam param="disable_pub_plugins">
- 'image_transport/compressedDepth'
</rosparam>
</group>
</group> <!-- mynteye -->
</launch>

2
wrappers/ros/src/mynt_eye_ros_wrapper/scripts/get_device_info.py
View File

@ -54,6 +54,8 @@ def main():
'LENS_TYPE': GetInfoRequest.LENS_TYPE,
'IMU_TYPE': GetInfoRequest.IMU_TYPE,
'NOMINAL_BASELINE': GetInfoRequest.NOMINAL_BASELINE,
'AUXILIARY_CHIP_VERSION': GetInfoRequest.AUXILIARY_CHIP_VERSION,
'ISP_VERSION': GetInfoRequest.ISP_VERSION,
}
for k, v in get_device_info(**keys).items():
print('{}: {}'.format(k, v))

275
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
View File

@ -315,7 +315,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
for (auto &&it = mono_topics.begin(); it != mono_topics.end(); ++it) {
auto &&topic = mono_topics[it->first];
if (it->first == Stream::LEFT || it->first == Stream::RIGHT) {
mono_publishers_[it->first] = it_mynteye.advertiseCamera(topic, 1);
mono_publishers_[it->first] = it_mynteye.advertise(topic, 1);
}
NODELET_INFO_STREAM("Advertized on topic " << topic);
}
@ -407,6 +407,12 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
case Request::NOMINAL_BASELINE:
res.value = api_->GetInfo(Info::NOMINAL_BASELINE);
break;
case Request::AUXILIARY_CHIP_VERSION:
res.value = api_->GetInfo(Info::AUXILIARY_CHIP_VERSION);
break;
case Request::ISP_VERSION:
res.value = api_->GetInfo(Info::ISP_VERSION);
break;
case Request::IMG_INTRINSICS:
{
auto intri_left = api_->GetIntrinsicsBase(Stream::LEFT);
@ -554,75 +560,41 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
return true;
}
void SetIsPublished(const Stream &stream) {
is_published_[stream] = false;
switch (stream) {
case Stream::LEFT_RECTIFIED: {
if (is_published_[Stream::RIGHT_RECTIFIED]) {
SetIsPublished(Stream::RIGHT_RECTIFIED);
}
if (is_published_[Stream::DISPARITY]) {
SetIsPublished(Stream::DISPARITY);
}
} break;
case Stream::RIGHT_RECTIFIED: {
if (is_published_[Stream::LEFT_RECTIFIED]) {
SetIsPublished(Stream::LEFT_RECTIFIED);
}
if (is_published_[Stream::DISPARITY]) {
SetIsPublished(Stream::DISPARITY);
}
} break;
case Stream::DISPARITY: {
if (is_published_[Stream::DISPARITY_NORMALIZED]) {
SetIsPublished(Stream::DISPARITY_NORMALIZED);
}
if (is_published_[Stream::POINTS]) {
SetIsPublished(Stream::POINTS);
}
} break;
case Stream::DISPARITY_NORMALIZED: {
} break;
case Stream::POINTS: {
if (is_published_[Stream::DEPTH]) {
SetIsPublished(Stream::DEPTH);
}
} break;
case Stream::DEPTH: {
} break;
default:
return;
void publishData(
const Stream &stream, const api::StreamData &data, std::uint32_t seq,
ros::Time stamp) {
if (stream == Stream::LEFT || stream == Stream::RIGHT) {
return;
} else if (stream == Stream::POINTS) {
publishPoints(data, seq, stamp);
} else {
publishCamera(stream, data, seq, stamp);
}
}
void publishPoint(const Stream &stream) {
auto &&points_num = points_publisher_.getNumSubscribers();
if (points_num == 0 && is_published_[stream]) {
SetIsPublished(stream);
api_->DisableStreamData(stream);
} else if (points_num > 0 && !is_published_[Stream::POINTS]) {
api_->EnableStreamData(Stream::POINTS);
api_->SetStreamCallback(
Stream::POINTS, [this](const api::StreamData &data) {
// ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
ros::Time stamp = checkUpTimeStamp(
data.img->timestamp, Stream::POINTS);
static std::size_t count = 0;
++count;
publishPoints(data, count, stamp);
});
is_published_[Stream::POINTS] = true;
int getStreamSubscribers(const Stream &stream) {
if (stream == Stream::POINTS) {
return points_publisher_.getNumSubscribers();
}
auto pub = camera_publishers_[stream];
if (pub)
return pub.getNumSubscribers();
return -1;
}
void publishOthers(const Stream &stream) {
auto stream_num = camera_publishers_[stream].getNumSubscribers();
if (stream_num == 0 && is_published_[stream]) {
// Stop computing when was not subcribed
SetIsPublished(stream);
api_->DisableStreamData(stream);
} else if (stream_num > 0 && !is_published_[stream]) {
// Start computing and publishing when was subcribed
// std::cout << stream << "===============================" << std::endl;
// int enable_tag = 0;
// api_->EnableStreamData(stream, [&](const Stream &stream) {
// enable_tag += getStreamSubscribers(stream);
// std::cout << "EnableStreamData callback test"
// << stream << "|| Sum:"
// << getStreamSubscribers(stream) << std::endl;
// }, true);
if (getStreamSubscribers(stream) > 0 && !is_published_[stream]) {
// std::cout << stream
// <<" enableStreamData tag = 0 return" << std::endl;
// std::cout << "enable " << stream << std::endl;
api_->EnableStreamData(stream);
api_->SetStreamCallback(
stream, [this, stream](const api::StreamData &data) {
@ -631,81 +603,121 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
data.img->timestamp, stream);
static std::size_t count = 0;
++count;
publishCamera(stream, data, count, stamp);
publishData(stream, data, count, stamp);
});
is_published_[stream] = true;
return;
}
int disable_tag = 0;
api_->DisableStreamData(stream, [&](const Stream &stream) {
disable_tag += getStreamSubscribers(stream);
// std::cout << "DisableStreamData callback test: "
// << stream << "|| Sum:"<< getStreamSubscribers(stream) << std::endl;
}, true);
if (disable_tag == 0 && is_published_[stream]) {
api_->DisableStreamData(stream, [&](const Stream &stream) {
// std::cout << "disable " << stream << std::endl;
api_->SetStreamCallback(stream, nullptr);
is_published_[stream] = false;
});
return;
}
}
void publishTopics() {
if ((camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 ||
mono_publishers_[Stream::LEFT].getNumSubscribers() > 0) &&
!is_published_[Stream::LEFT]) {
api_->SetStreamCallback(
Stream::LEFT, [&](const api::StreamData &data) {
++left_count_;
if (left_count_ > 10) {
// ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
ros::Time stamp = checkUpTimeStamp(
data.img->timestamp, Stream::LEFT);
std::vector<Stream> all_streams{
Stream::RIGHT,
Stream::LEFT,
Stream::LEFT_RECTIFIED,
Stream::RIGHT_RECTIFIED,
Stream::DISPARITY,
Stream::DISPARITY_NORMALIZED,
Stream::POINTS,
Stream::DEPTH
};
// static double img_time_prev = -1;
// NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION <<
// ros_time_beg
// << ", img_time_elapsed: " << FULL_PRECISION
// << ((data.img->timestamp - img_time_beg) * 0.00001f)
// << ", img_time_diff: " << FULL_PRECISION
// << ((img_time_prev < 0) ? 0
// : (data.img->timestamp - img_time_prev) * 0.01f) << "
// ms");
// img_time_prev = data.img->timestamp;
publishCamera(Stream::LEFT, data, left_count_, stamp);
publishMono(Stream::LEFT, data, left_count_, stamp);
NODELET_DEBUG_STREAM(
Stream::LEFT << ", count: " << left_count_
<< ", frame_id: " << data.img->frame_id
<< ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time);
}
});
left_time_beg_ = ros::Time::now().toSec();
is_published_[Stream::LEFT] = true;
static int sum = 0;
int sum_c = 0;
for (auto &&stream : all_streams) {
sum_c += getStreamSubscribers(stream);
}
if ((camera_publishers_[Stream::RIGHT].getNumSubscribers() > 0 ||
mono_publishers_[Stream::RIGHT].getNumSubscribers() > 0) &&
!is_published_[Stream::RIGHT]) {
api_->SetStreamCallback(
Stream::RIGHT, [&](const api::StreamData &data) {
++right_count_;
if (right_count_ > 10) {
// ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
ros::Time stamp = checkUpTimeStamp(
data.img->timestamp, Stream::RIGHT);
publishCamera(Stream::RIGHT, data, right_count_, stamp);
publishMono(Stream::RIGHT, data, right_count_, stamp);
NODELET_DEBUG_STREAM(
Stream::RIGHT << ", count: " << right_count_
<< ", frame_id: " << data.img->frame_id
<< ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time);
}
});
right_time_beg_ = ros::Time::now().toSec();
is_published_[Stream::RIGHT] = true;
}
std::vector<Stream> other_streams{
Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED,
Stream::DISPARITY, Stream::DISPARITY_NORMALIZED,
Stream::POINTS, Stream::DEPTH};
for (auto &&stream : other_streams) {
if (stream != Stream::POINTS) {
publishOthers(stream);
if (sum_c != sum) {
if (sum_c == 0) {
api_->Stop(Source::VIDEO_STREAMING);
for (auto &&stream : all_streams) {
is_published_[stream] = false;
}
api_->Start(Source::VIDEO_STREAMING);
} else {
publishPoint(stream);
if ((camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 ||
mono_publishers_[Stream::LEFT].getNumSubscribers() > 0) &&
!is_published_[Stream::LEFT]) {
api_->SetStreamCallback(
Stream::LEFT, [&](const api::StreamData &data) {
++left_count_;
if (left_count_ > 10) {
// ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
ros::Time stamp = checkUpTimeStamp(
data.img->timestamp, Stream::LEFT);
// static double img_time_prev = -1;
// NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION <<
// ros_time_beg
// << ", img_time_elapsed: " << FULL_PRECISION
// << ((data.img->timestamp - img_time_beg) * 0.00001f)
// << ", img_time_diff: " << FULL_PRECISION
// << ((img_time_prev < 0) ? 0
// : (data.img->timestamp - img_time_prev) * 0.01f) << "
// ms");
// img_time_prev = data.img->timestamp;
publishCamera(Stream::LEFT, data, left_count_, stamp);
publishMono(Stream::LEFT, data, left_count_, stamp);
NODELET_DEBUG_STREAM(
Stream::LEFT << ", count: " << left_count_
<< ", frame_id: " << data.img->frame_id
<< ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time);
}
});
left_time_beg_ = ros::Time::now().toSec();
is_published_[Stream::LEFT] = true;
}
if ((camera_publishers_[Stream::RIGHT].getNumSubscribers() > 0 ||
mono_publishers_[Stream::RIGHT].getNumSubscribers() > 0) &&
!is_published_[Stream::RIGHT]) {
api_->SetStreamCallback(
Stream::RIGHT, [&](const api::StreamData &data) {
++right_count_;
if (right_count_ > 10) {
// ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
ros::Time stamp = checkUpTimeStamp(
data.img->timestamp, Stream::RIGHT);
publishCamera(Stream::RIGHT, data, right_count_, stamp);
publishMono(Stream::RIGHT, data, right_count_, stamp);
NODELET_DEBUG_STREAM(
Stream::RIGHT << ", count: " << right_count_
<< ", frame_id: " << data.img->frame_id
<< ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time);
}
});
right_time_beg_ = ros::Time::now().toSec();
is_published_[Stream::RIGHT] = true;
}
std::vector<Stream> other_streams{
Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED,
Stream::DISPARITY, Stream::DISPARITY_NORMALIZED,
Stream::POINTS, Stream::DEPTH
};
for (auto &&stream : other_streams) {
publishOthers(stream);
}
}
sum = sum_c;
}
if (!is_motion_published_) {
@ -822,9 +834,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
cv::cvtColor(data.frame, mono, CV_RGB2GRAY);
auto &&msg = cv_bridge::CvImage(header, enc::MONO8, mono).toImageMsg();
pthread_mutex_unlock(&mutex_data_);
auto &&info = getCameraInfo(stream);
info->header.stamp = msg->header.stamp;
mono_publishers_[stream].publish(msg, info);
mono_publishers_[stream].publish(msg);
}
void publishPoints(
@ -1284,7 +1294,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
}
for (int i = 0; i < p.rows; i++) {
for (int j = 0; j < p.cols; j++) {
camera_info->P.at(i * p.cols + j) = p.at<double>(i, j);
int scale = (i == 2 && j == 2)?1:1000;
camera_info->P.at(i * p.cols + j) = p.at<double>(i, j) / scale;
}
}
@ -1510,7 +1521,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
std::map<Stream, std::string> image_encodings_;
// mono: LEFT, RIGHT
std::map<Stream, image_transport::CameraPublisher> mono_publishers_;
std::map<Stream, image_transport::Publisher> mono_publishers_;
// pointcloud: POINTS
ros::Publisher points_publisher_;

10
wrappers/ros/src/mynt_eye_ros_wrapper/srv/GetInfo.srv
View File

@ -6,10 +6,12 @@ uint32 SPEC_VERSION=4
uint32 LENS_TYPE=5
uint32 IMU_TYPE=6
uint32 NOMINAL_BASELINE=7
uint32 IMG_INTRINSICS=8
uint32 IMG_EXTRINSICS_RTOL=9
uint32 IMU_INTRINSICS=10
uint32 IMU_EXTRINSICS=11
uint32 AUXILIARY_CHIP_VERSION=8
uint32 ISP_VERSION=9
uint32 IMG_INTRINSICS=10
uint32 IMG_EXTRINSICS_RTOL=11
uint32 IMU_INTRINSICS=12
uint32 IMU_EXTRINSICS=13
uint32 key
---
string value