Add tutorial get_imu
This commit is contained in:
John Zhao
parent
90bb1cdd9a
commit
63105b5ae4
|
|
@ -58,6 +58,11 @@ LIST(APPEND CMAKE_MODULE_PATH ${PRO_DIR}/cmake)
|
|||
|
||||
find_package(OpenCV REQUIRED)
|
||||
message(STATUS "Found OpenCV: ${OpenCV_VERSION}")
|
||||
if(OpenCV_VERSION VERSION_LESS 3.0)
|
||||
add_definitions(-DUSE_OPENCV2)
|
||||
else()
|
||||
add_definitions(-DUSE_OPENCV3)
|
||||
endif()
|
||||
|
||||
if(OS_WIN)
|
||||
get_filename_component(OpenCV_LIB_SEARCH_PATH "${OpenCV_LIB_PATH}/../bin" ABSOLUTE)
|
||||
|
|
|
|||
|
|
@ -102,6 +102,7 @@ if(PCL_FOUND)
|
|||
WITH_OPENCV WITH_PCL
|
||||
)
|
||||
endif()
|
||||
make_executable(get_imu SRCS data/get_imu.cc data/cv_painter.cc WITH_OPENCV)
|
||||
make_executable(get_with_plugin SRCS data/get_with_plugin.cc WITH_OPENCV)
|
||||
|
||||
# control
|
||||
|
|
|
|||
150
samples/tutorials/data/cv_painter.cc
Normal file
150
samples/tutorials/data/cv_painter.cc
Normal file
|
|
@ -0,0 +1,150 @@
|
|||
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
#include "data/cv_painter.h"
|
||||
|
||||
#include <glog/logging.h>
|
||||
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
|
||||
#define FONT_FACE cv::FONT_HERSHEY_PLAIN
|
||||
#define FONT_SCALE 1
|
||||
#define FONT_COLOR cv::Scalar(255, 255, 255)
|
||||
#define THICKNESS 1
|
||||
|
||||
namespace {
|
||||
|
||||
std::shared_ptr<std::ios> NewFormat(int width, int prec, char fillch = ' ') {
|
||||
auto fmt = std::make_shared<std::ios>(nullptr);
|
||||
fmt->setf(std::ios::fixed);
|
||||
fmt->width(std::move(width));
|
||||
fmt->precision(std::move(prec));
|
||||
fmt->fill(std::move(fillch));
|
||||
return fmt;
|
||||
}
|
||||
|
||||
std::ostringstream &Clear(std::ostringstream &os) {
|
||||
os.str("");
|
||||
os.clear();
|
||||
return os;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
std::ostream &operator<<(
|
||||
std::ostream &os, const std::shared_ptr<std::ios> &fmt) {
|
||||
if (fmt)
|
||||
os.copyfmt(*fmt);
|
||||
return os;
|
||||
}
|
||||
|
||||
CVPainter::CVPainter() {
|
||||
VLOG(2) << __func__;
|
||||
}
|
||||
|
||||
CVPainter::~CVPainter() {
|
||||
VLOG(2) << __func__;
|
||||
}
|
||||
|
||||
cv::Rect CVPainter::DrawSize(const cv::Mat &img, const gravity_t &gravity) {
|
||||
std::ostringstream ss;
|
||||
ss << img.cols << "x" << img.rows;
|
||||
return DrawText(img, ss.str(), gravity, 5);
|
||||
}
|
||||
|
||||
cv::Rect CVPainter::DrawImuData(
|
||||
const cv::Mat &img, const mynteye::ImuData &data,
|
||||
const gravity_t &gravity) {
|
||||
static std::ostringstream ss;
|
||||
static auto fmt_imu = NewFormat(8, 4);
|
||||
static auto fmt_temp = NewFormat(6, 4);
|
||||
|
||||
int sign = 1;
|
||||
if (gravity == BOTTOM_LEFT || gravity == BOTTOM_RIGHT)
|
||||
sign = -1;
|
||||
|
||||
Clear(ss) << "frame_id: " << data.frame_id << ", stamp: " << data.timestamp
|
||||
<< ", temp: " << fmt_temp << data.temperature;
|
||||
cv::Rect rect_i = DrawText(img, ss.str(), gravity, 5);
|
||||
|
||||
Clear(ss) << "accel(x,y,z): " << fmt_imu << data.accel[0] << "," << fmt_imu
|
||||
<< data.accel[1] << "," << fmt_imu << data.accel[2];
|
||||
cv::Rect rect_a =
|
||||
DrawText(img, ss.str(), gravity, 5, 0, sign * (5 + rect_i.height));
|
||||
|
||||
Clear(ss) << "gyro(x,y,z): " << fmt_imu << data.gyro[0] << "," << fmt_imu
|
||||
<< data.gyro[1] << "," << fmt_imu << data.gyro[2];
|
||||
cv::Rect rect_g = DrawText(
|
||||
img, ss.str(), gravity, 5, 0,
|
||||
sign * (10 + rect_i.height + rect_a.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));
|
||||
} else {
|
||||
return cv::Rect(rect_g.tl(), rect_i.br());
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
int w = img.cols, h = img.rows;
|
||||
|
||||
int baseline = 0;
|
||||
cv::Size textSize =
|
||||
cv::getTextSize(text, FONT_FACE, FONT_SCALE, THICKNESS, &baseline);
|
||||
|
||||
int x, y;
|
||||
switch (gravity) {
|
||||
case TOP_LEFT:
|
||||
x = margin;
|
||||
y = margin + textSize.height;
|
||||
break;
|
||||
case TOP_RIGHT:
|
||||
x = w - margin - textSize.width;
|
||||
y = margin + textSize.height;
|
||||
break;
|
||||
case BOTTOM_LEFT:
|
||||
x = margin;
|
||||
y = h - margin;
|
||||
break;
|
||||
case BOTTOM_RIGHT:
|
||||
x = w - margin - textSize.width;
|
||||
y = h - margin;
|
||||
break;
|
||||
default: // TOP_LEFT
|
||||
x = margin;
|
||||
y = margin + textSize.height;
|
||||
break;
|
||||
}
|
||||
x += offset_x;
|
||||
y += offset_y;
|
||||
|
||||
cv::Point org(x, y);
|
||||
#ifdef USE_OPENCV2
|
||||
cv::putText(
|
||||
const_cast<cv::Mat &>(img), text, org, FONT_FACE, FONT_SCALE, FONT_COLOR,
|
||||
THICKNESS);
|
||||
#else
|
||||
cv::putText(img, text, org, FONT_FACE, FONT_SCALE, FONT_COLOR, THICKNESS);
|
||||
#endif
|
||||
return cv::Rect(org, textSize);
|
||||
}
|
||||
47
samples/tutorials/data/cv_painter.h
Normal file
47
samples/tutorials/data/cv_painter.h
Normal file
|
|
@ -0,0 +1,47 @@
|
|||
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
#ifndef MYNTEYE_TUTORIALS_CV_PAINTER_H_ // NOLINT
|
||||
#define MYNTEYE_TUTORIALS_CV_PAINTER_H_
|
||||
#pragma once
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "mynteye/types.h"
|
||||
|
||||
class CVPainter {
|
||||
public:
|
||||
typedef enum Gravity {
|
||||
TOP_LEFT,
|
||||
TOP_RIGHT,
|
||||
BOTTOM_LEFT,
|
||||
BOTTOM_RIGHT
|
||||
} gravity_t;
|
||||
|
||||
CVPainter();
|
||||
~CVPainter();
|
||||
|
||||
cv::Rect DrawSize(const cv::Mat &img, const gravity_t &gravity = TOP_LEFT);
|
||||
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,
|
||||
const int &offset_x = 0, const int &offset_y = 0);
|
||||
};
|
||||
|
||||
#endif // MYNTEYE_TUTORIALS_CV_PAINTER_H_ NOLINT
|
||||
75
samples/tutorials/data/get_imu.cc
Normal file
75
samples/tutorials/data/get_imu.cc
Normal file
|
|
@ -0,0 +1,75 @@
|
|||
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
#include <glog/logging.h>
|
||||
|
||||
#include "mynteye/api.h"
|
||||
|
||||
#include "data/cv_painter.h"
|
||||
|
||||
MYNTEYE_USE_NAMESPACE
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
auto &&api = API::Create(argc, argv);
|
||||
|
||||
// Enable this will cache the motion datas until you get them.
|
||||
api->EnableMotionDatas();
|
||||
|
||||
api->Start(Source::ALL);
|
||||
|
||||
CVPainter painter;
|
||||
|
||||
cv::namedWindow("frame");
|
||||
|
||||
while (true) {
|
||||
api->WaitForStreams();
|
||||
|
||||
auto &&left_data = api->GetStreamData(Stream::LEFT);
|
||||
auto &&right_data = api->GetStreamData(Stream::RIGHT);
|
||||
|
||||
cv::Mat img;
|
||||
cv::hconcat(left_data.frame, right_data.frame, img);
|
||||
|
||||
auto &&motion_datas = api->GetMotionDatas();
|
||||
/*
|
||||
for (auto &&data : motion_datas) {
|
||||
LOG(INFO) << "Imu 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;
|
||||
}
|
||||
*/
|
||||
|
||||
painter.DrawSize(img);
|
||||
if (!motion_datas.empty()) {
|
||||
painter.DrawImuData(img, *motion_datas[0].imu);
|
||||
}
|
||||
|
||||
cv::imshow("frame", img);
|
||||
|
||||
char key = static_cast<char>(cv::waitKey(1));
|
||||
if (key == 27 || key == 'q' || key == 'Q') { // ESC/Q
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
api->Stop(Source::ALL);
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user