feat(*):add the display of the posture in imu range setting sample

samples/CMakeLists.txt

@@ -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)

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 ||
+  if (model == Model::STANDARD2 || model == Model::STANDARD210A ||
-   model == Model::STANDARD210A || model == Model::STANDARD200B) {
+    model == Model::STANDARD200B) {
     // auto-exposure: 0
     api->SetOptionValue(Option::EXPOSURE_MODE, 0);
 

samples/ctrl_imu_range.cc

@@ -12,6 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 #include <atomic>
+#include <memory>
+#include <mutex>
+#include <sstream>
 
 #include <opencv2/highgui/highgui.hpp>
 
@@ -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 ||
+  if (model == Model::STANDARD2 || model == Model::STANDARD210A ||
-   model == Model::STANDARD210A || model == Model::STANDARD200B) {
+    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<mynteye::ImuData> 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<std::mutex> _(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<std::mutex> _(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<char>(cv::waitKey(1));

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 ||
+  if (model == Model::STANDARD2 || model == Model::STANDARD210A ||
-   model == Model::STANDARD210A || model == Model::STANDARD200B) {
+    model == Model::STANDARD200B) {
     // manual-exposure: 1
     api->SetOptionValue(Option::EXPOSURE_MODE, 1);
 

samples/util_cv.cc

@@ -13,6 +13,8 @@
 // limitations under the License.
 #include "util_cv.h"
 
+#include <math.h>
+
 #include <iomanip>
 #include <iostream>
 #include <memory>
@@ -28,6 +30,8 @@
 #define FONT_COLOR cv::Scalar(255, 255, 255)
 #define THICKNESS 1
 
+#define PI 3.1415926
+
 namespace {
 
 std::shared_ptr<std::ios> 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
+  Clear(ss) << "accel(x,y,z,sum): "
-            << accel2_s << "," << fmt_imu << accel3_s;
+            << 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) {

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 cv::Mat &img, const mynteye::ImuData &data,
-      const gravity_t &gravity = TOP_RIGHT);
+    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,

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,