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

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

Browse Source
This commit is contained in:
kalman 2019-09-30 16:39:21 +08:00
parent db4f1908ed
commit abd16236ca
7 changed files with 111 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
samples/CMakeLists.txt
View File

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

4
samples/ctrl_auto_exposure.cc
View File

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

39
samples/ctrl_imu_range.cc
View File

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

4
samples/ctrl_manual_exposure.cc
View File

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

63
samples/util_cv.cc
View File

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

12
samples/util_cv.h
View File

@ -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();
@ -39,8 +45,10 @@ class CVPainter {
const gravity_t &gravity = TOP_LEFT);
cv::Rect DrawImuData(
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,

4
wrappers/android/mynteye/libmynteye/src/main/cpp/mynteye/cpp/option.hpp
View File

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