diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 807c203..36a3b89 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -127,7 +127,7 @@ include_directories( # tutorials if(WITH_API) - + # make_executable2(NAME # [SRCS src1 src2 ...] # [WITH_OPENCV] @@ -219,7 +219,7 @@ if(WITH_API) make_executable2(ctrl_framerate SRCS ctrl_framerate.cc WITH_OPENCV) make_executable2(ctrl_imu_low_pass_filter SRCS ctrl_imu_low_pass_filter.cc WITH_OPENCV) - make_executable2(ctrl_imu_range SRCS ctrl_imu_range.cc WITH_OPENCV) + make_executable2(ctrl_imu_range SRCS ctrl_imu_range.cc util_cv.cc WITH_OPENCV) make_executable2(ctrl_infrared SRCS ctrl_infrared.cc WITH_OPENCV) make_executable2(ctrl_iic_address SRCS ctrl_iic_address.cc WITH_OPENCV) make_executable2(ctrl_sync_timestamp SRCS ctrl_sync_timestamp.cc WITH_OPENCV) diff --git a/samples/ctrl_auto_exposure.cc b/samples/ctrl_auto_exposure.cc index 1a78c97..53f18c3 100644 --- a/samples/ctrl_auto_exposure.cc +++ b/samples/ctrl_auto_exposure.cc @@ -57,8 +57,8 @@ int main(int argc, char *argv[]) { } // Set auto exposure options fo S2000/S2100/S210A/S200B - if (model == Model::STANDARD2 || - model == Model::STANDARD210A || model == Model::STANDARD200B) { + if (model == Model::STANDARD2 || model == Model::STANDARD210A || + model == Model::STANDARD200B) { // auto-exposure: 0 api->SetOptionValue(Option::EXPOSURE_MODE, 0); diff --git a/samples/ctrl_imu_range.cc b/samples/ctrl_imu_range.cc index 1a0292c..f16004a 100644 --- a/samples/ctrl_imu_range.cc +++ b/samples/ctrl_imu_range.cc @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. #include +#include +#include +#include #include @@ -19,6 +22,8 @@ #include "mynteye/api/api.h" #include "mynteye/util/times.h" +#include "util_cv.h" + MYNTEYE_USE_NAMESPACE int main(int argc, char *argv[]) { @@ -31,18 +36,19 @@ int main(int argc, char *argv[]) { api->ConfigStreamRequest(request); Model model = api->GetModel(); - + int imu_frequency = 200; // Set imu range for S1030 if (model == Model::STANDARD) { // ACCELEROMETER_RANGE values: 4, 8, 16, 32 api->SetOptionValue(Option::ACCELEROMETER_RANGE, 8); // GYROSCOPE_RANGE values: 500, 1000, 2000, 4000 api->SetOptionValue(Option::GYROSCOPE_RANGE, 1000); + imu_frequency = api->GetOptionValue(Option::IMU_FREQUENCY); } // Set imu range for S2000/S2100/S210A/S200B - if (model == Model::STANDARD2 || - model == Model::STANDARD210A || model == Model::STANDARD200B) { + if (model == Model::STANDARD2 || model == Model::STANDARD210A || + model == Model::STANDARD200B) { // ACCELEROMETER_RANGE values: 6, 12, 24, 48 api->SetOptionValue(Option::ACCELEROMETER_RANGE, 6); // GYROSCOPE_RANGE values: 250, 500, 1000, 2000, 4000 @@ -64,13 +70,26 @@ int main(int argc, char *argv[]) { // Count imu std::atomic_uint imu_count(0); - api->SetMotionCallback([&imu_count](const api::MotionData &data) { + std::shared_ptr imu; + std::mutex imu_mtx; + CVPainter::angle_t gyro_offset; + api->SetMotionCallback([&imu_count, &imu, &imu_mtx, + &gyro_offset, &imu_frequency](const api::MotionData &data) { CHECK_NOTNULL(data.imu); ++imu_count; + { + std::lock_guard _(imu_mtx); + imu = data.imu; + gyro_offset.angle_x += data.imu->gyro[0] / (1.0 * imu_frequency); + gyro_offset.angle_y += data.imu->gyro[1] / (1.0 * imu_frequency); + gyro_offset.angle_z += data.imu->gyro[2] / (1.0 * imu_frequency); + } }); api->Start(Source::ALL); + CVPainter painter; + cv::namedWindow("frame"); auto &&time_beg = times::now(); @@ -82,6 +101,18 @@ int main(int argc, char *argv[]) { cv::Mat img; cv::hconcat(left_data.frame, right_data.frame, img); + + // Draw imu data + if (imu) { + std::lock_guard _(imu_mtx); + painter.DrawImuData(img, *imu, gyro_offset); + } + + // Draw counts + std::ostringstream ss; + ss << "imu: " << imu_count; + painter.DrawText(img, ss.str(), CVPainter::BOTTOM_RIGHT); + cv::imshow("frame", img); char key = static_cast(cv::waitKey(1)); diff --git a/samples/ctrl_manual_exposure.cc b/samples/ctrl_manual_exposure.cc index b843742..489b2f3 100644 --- a/samples/ctrl_manual_exposure.cc +++ b/samples/ctrl_manual_exposure.cc @@ -55,8 +55,8 @@ int main(int argc, char *argv[]) { } // Set manual exposure options fo S2000/S2100/S210A/S200B - if (model == Model::STANDARD2 || - model == Model::STANDARD210A || model == Model::STANDARD200B) { + if (model == Model::STANDARD2 || model == Model::STANDARD210A || + model == Model::STANDARD200B) { // manual-exposure: 1 api->SetOptionValue(Option::EXPOSURE_MODE, 1); diff --git a/samples/util_cv.cc b/samples/util_cv.cc index 1911d78..e7d8645 100644 --- a/samples/util_cv.cc +++ b/samples/util_cv.cc @@ -13,6 +13,8 @@ // limitations under the License. #include "util_cv.h" +#include + #include #include #include @@ -28,6 +30,8 @@ #define FONT_COLOR cv::Scalar(255, 255, 255) #define THICKNESS 1 +#define PI 3.1415926 + namespace { std::shared_ptr NewFormat(int width, int prec, char fillch = ' ') { @@ -108,7 +112,7 @@ cv::Rect CVPainter::DrawImgData( cv::Rect CVPainter::DrawImuData( const cv::Mat &img, const mynteye::ImuData &data, - const gravity_t &gravity) { + const angle_t &offset, const gravity_t &gravity) { static std::ostringstream ss; static auto fmt_imu = NewFormat(8, 4); static auto fmt_temp = NewFormat(6, 4); @@ -124,23 +128,45 @@ cv::Rect CVPainter::DrawImuData( static double accel1_s = 0.0; static double accel2_s = 0.0; static double accel3_s = 0.0; - + static double vector_sum = 0.0; + static double pitch = 0.0; + static double roll = 0.0; if (data.accel[0] > 0.000001 || data.accel[1] > 0.000001 || data.accel[2] > 0.000001 || data.accel[0] < -0.000001 || data.accel[1] < -0.000001 || data.accel[2] < -0.000001) { + double accel1_s_square, accel2_s_square, accel3_s_square; + accel1_s = data.accel[0]; accel2_s = data.accel[1]; accel3_s = data.accel[2]; + accel1_s_square = accel1_s * accel1_s; + accel2_s_square = accel2_s * accel2_s; + accel3_s_square = accel3_s * accel3_s; + vector_sum = sqrt(accel1_s_square + accel2_s_square + accel3_s_square); + pitch = atan(accel1_s / sqrt(accel2_s_square + accel3_s_square)) + * 180.0 / PI; + roll = atan(accel2_s / sqrt(accel1_s_square + accel3_s_square)) + * 180.0 / PI; } - Clear(ss) << "accel(x,y,z): " << fmt_imu << accel1_s << "," << fmt_imu - << accel2_s << "," << fmt_imu << accel3_s; + Clear(ss) << "accel(x,y,z,sum): " + << fmt_imu << accel1_s << "," + << fmt_imu << accel2_s << "," + << fmt_imu << accel3_s << "," + << fmt_imu << vector_sum; cv::Rect rect_a = DrawText(img, ss.str(), gravity, 5, 0, sign * (5 + rect_i.height)); + Clear(ss) << "posture(pitch,roll): " + << fmt_imu << pitch << "," + << fmt_imu << roll; + cv::Rect rect_p = DrawText( + img, ss.str(), gravity, 5, 0, + sign * (10 + rect_i.height + rect_a.height)); + static double gyro1_s = 0.0; static double gyro2_s = 0.0; static double gyro3_s = 0.0; @@ -160,18 +186,43 @@ cv::Rect CVPainter::DrawImuData( << gyro2_s << "," << fmt_imu << gyro3_s; cv::Rect rect_g = DrawText( img, ss.str(), gravity, 5, 0, - sign * (10 + rect_i.height + rect_a.height)); + sign * (15 + rect_i.height + rect_a.height + rect_p.height)); + if (offset.angle_x != 0 || + offset.angle_y != 0 || + offset.angle_z != 0) { + Clear(ss) << "angle offset(x,y,z): " + << fmt_imu << offset.angle_x << "," + << fmt_imu << offset.angle_y << "," + << fmt_imu << offset.angle_z; + } else { + Clear(ss) << "angle offset(x,y,z): " + << fmt_imu << "x," + << fmt_imu << "x," + << fmt_imu << "x"; + } + + cv::Rect rect_o = DrawText( + img, ss.str(), gravity, 5, 0, + sign * (20 + rect_i.height + rect_a.height + + rect_p.height + rect_g.height)); // rect_i.width is the max one if (sign > 0) { return cv::Rect( rect_i.tl(), - cv::Point(rect_i.x + rect_i.width, rect_g.y + rect_g.height)); + cv::Point(rect_i.x + rect_i.width, rect_o.y + rect_o.height)); } else { return cv::Rect(rect_g.tl(), rect_i.br()); } } +cv::Rect CVPainter::DrawImuData( + const cv::Mat &img, const mynteye::ImuData &data, + const gravity_t &gravity) { + angle_t offset = {0, 0, 0}; + return DrawImuData(img, data, offset, gravity); +} + cv::Rect CVPainter::DrawText( const cv::Mat &img, const std::string &text, const gravity_t &gravity, const int &margin, const int &offset_x, const int &offset_y) { diff --git a/samples/util_cv.h b/samples/util_cv.h index decde17..e8b07f6 100644 --- a/samples/util_cv.h +++ b/samples/util_cv.h @@ -30,6 +30,12 @@ class CVPainter { BOTTOM_RIGHT } gravity_t; + typedef struct Angle { + double angle_x; + double angle_y; + double angle_z; + } angle_t; + explicit CVPainter(std::int32_t frame_rate = 0); ~CVPainter(); @@ -38,9 +44,11 @@ class CVPainter { const cv::Mat &img, const mynteye::ImgData &data, const gravity_t &gravity = TOP_LEFT); cv::Rect DrawImuData( - const cv::Mat &img, const mynteye::ImuData &data, - const gravity_t &gravity = TOP_RIGHT); - + const cv::Mat &img, const mynteye::ImuData &data, + const angle_t &offset, const gravity_t &gravity = TOP_RIGHT); + cv::Rect DrawImuData( + const cv::Mat &img, + const mynteye::ImuData &data, const gravity_t &gravity = TOP_RIGHT); cv::Rect DrawText( const cv::Mat &img, const std::string &text, const gravity_t &gravity = TOP_LEFT, const int &margin = 5, diff --git a/wrappers/android/mynteye/libmynteye/src/main/cpp/mynteye/cpp/option.hpp b/wrappers/android/mynteye/libmynteye/src/main/cpp/mynteye/cpp/option.hpp index 6790ba4..9c519c7 100644 --- a/wrappers/android/mynteye/libmynteye/src/main/cpp/mynteye/cpp/option.hpp +++ b/wrappers/android/mynteye/libmynteye/src/main/cpp/mynteye/cpp/option.hpp @@ -24,12 +24,12 @@ enum class Option : int { */ CONTRAST, /** - * Image frame rate, must set IMU_FREQUENCY together + * Image frame rate, must set FRAME_RATE together * values: {10,15,20,25,30,35,40,45,50,55,60}, default: 25 */ FRAME_RATE, /** - * IMU frequency, must set FRAME_RATE together + * IMU frequency, must set IMU_FREQUENCY together * values: {100,200,250,333,500}, default: 200 */ IMU_FREQUENCY,