diff --git a/samples/device/camera.cc b/samples/device/camera.cc index 8dea9d9..7f3784c 100644 --- a/samples/device/camera.cc +++ b/samples/device/camera.cc @@ -100,10 +100,13 @@ int main(int argc, char *argv[]) { << ", temperature: " << data.imu->temperature; }); + // Enable this will cache the motion datas until you get them. + device->EnableMotionDatas(); device->Start(Source::ALL); cv::namedWindow("frame"); + std::size_t motion_count = 0; auto &&time_beg = times::now(); while (true) { device->WaitForStreams(); @@ -111,6 +114,20 @@ int main(int argc, char *argv[]) { device::StreamData left_data = device->GetLatestStreamData(Stream::LEFT); device::StreamData right_data = device->GetLatestStreamData(Stream::RIGHT); + auto &&motion_datas = device->GetMotionDatas(); + motion_count += motion_datas.size(); + for (auto &&data : motion_datas) { + LOG(INFO) << " 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 left_img( left_data.frame->height(), left_data.frame->width(), CV_8UC1, left_data.frame->data()); @@ -142,5 +159,7 @@ int main(int argc, char *argv[]) { << ", 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/internal/motions.cc b/src/internal/motions.cc index 091d14f..e8da952 100644 --- a/src/internal/motions.cc +++ b/src/internal/motions.cc @@ -45,8 +45,12 @@ void Motions::StartMotionTracking() { imu->gyro[1] = seg.gyro[1] * 1000.f / 0x10000; imu->gyro[2] = seg.gyro[2] * 1000.f / 0x10000; imu->temperature = seg.temperature / 326.8f + 25; + + std::lock_guard _(mtx_datas_); + motion_data_t data = {imu}; + motion_datas_.push_back(data); if (motion_callback_) { - motion_callback_({imu}); + motion_callback_(data); } } }); @@ -77,7 +81,10 @@ Motions::motion_datas_t Motions::GetMotionDatas() { LOG(FATAL) << "Must enable motion datas before getting them, or you set " "motion callback instead"; } - return motion_datas_; + std::lock_guard _(mtx_datas_); + motion_datas_t datas = motion_datas_; + motion_datas_.clear(); + return datas; } MYNTEYE_END_NAMESPACE diff --git a/src/internal/motions.h b/src/internal/motions.h index 4e3d715..607f8a8 100644 --- a/src/internal/motions.h +++ b/src/internal/motions.h @@ -3,6 +3,7 @@ #pragma once #include +#include #include #include "mynteye/mynteye.h" @@ -41,6 +42,8 @@ class Motions { std::size_t motion_datas_max_size; bool is_imu_tracking; + + std::mutex mtx_datas_; }; MYNTEYE_END_NAMESPACE