feat(correspondence): add get correspondence data

This commit is contained in:
John Zhao 2019-02-22 14:00:23 +08:00
parent ab52fd5280
commit 85acd7b915
4 changed files with 94 additions and 18 deletions

View File

@ -38,22 +38,30 @@ int main(int argc, char *argv[]) {
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);
LOG(INFO) << "Img frame_id: " << left_data.img->frame_id
<< ", timestamp: " << left_data.img->timestamp;
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) {
LOG(INFO) << "Imu frame_id: " << data.imu->frame_id
<< ", timestamp: " << data.imu->timestamp;
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);

View File

@ -20,7 +20,7 @@ MYNTEYE_BEGIN_NAMESPACE
Correspondence::Correspondence(const std::shared_ptr<Device> &device,
const Stream &stream)
: device_(device), 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
@ -56,16 +56,19 @@ bool Correspondence::Watch(const Stream &stream) const {
void Correspondence::OnStreamDataCallback(
const Stream &stream, const api::StreamData &data) {
// LOG(INFO) << __func__ << ", " << stream
// << ", id: " << data.frame_id << ", stamp: " << data.img->timestamp;
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(data);
stream_datas_.push_back(std::move(data));
} else if (/*stream_match_enabled_ && */stream == stream_match_) {
stream_datas_match_.push_back(data);
stream_datas_match_.push_back(std::move(data));
}
NotifyStreamDataReady();
}
@ -114,7 +117,7 @@ std::vector<api::StreamData> Correspondence::GetStreamDatas(
static std::uint32_t stream_match_count_ = 0;
if (stream == stream_) {
auto datas = std::move(stream_datas_);
auto datas = GetReadyStreamData(false);
if (stream_count_ < 10) {
++stream_count_;
@ -127,7 +130,7 @@ std::vector<api::StreamData> Correspondence::GetStreamDatas(
return datas;
} else if (/*stream_match_enabled_ && */stream == stream_match_) {
auto datas = std::move(stream_datas_match_);
auto datas = GetReadyStreamData(true);
if (stream_match_count_ < 10) {
++stream_match_count_;
@ -140,13 +143,7 @@ std::vector<api::StreamData> Correspondence::GetStreamDatas(
}
std::vector<api::MotionData> Correspondence::GetMotionDatas() {
std::lock_guard<std::recursive_mutex> _(mtx_motion_datas_);
std::vector<api::MotionData> datas;
for (auto &&data : motion_datas_) {
datas.push_back({data.imu});
}
motion_datas_.clear();
return datas;
return GetReadyMotionDatas();
}
void Correspondence::EnableStreamMatch() {
@ -198,4 +195,67 @@ bool Correspondence::IsStreamDataReady() {
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!";
return {};
}
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

View File

@ -51,6 +51,9 @@ class Correspondence {
bool IsStreamDataReady();
std::vector<api::StreamData> GetReadyStreamData(bool matched);
std::vector<api::MotionData> GetReadyMotionDatas();
std::shared_ptr<Device> device_;
Stream stream_;
Stream stream_match_;
@ -67,6 +70,8 @@ class Correspondence {
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

View File

@ -349,6 +349,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;
}