diff --git a/3rdparty/fmt b/3rdparty/fmt
index 02cae7e..b3bf23f 160000
--- a/3rdparty/fmt
+++ b/3rdparty/fmt
@@ -1 +1 @@
-Subproject commit 02cae7e48ac9b1d90acb0ee07c76fbde2462a99e
+Subproject commit b3bf23f3c4e2e9f552ae2e74c33a6754f4a7dd29
diff --git a/base/RobotVision_Sirius.cpp b/base/RobotVision_Sirius.cpp
index e256545..0b3dce2 100644
--- a/base/RobotVision_Sirius.cpp
+++ b/base/RobotVision_Sirius.cpp
@@ -6,46 +6,53 @@
* @copyright Copyright (c) 2023 Sirius
*/
#include "RobotVision_Sirius.hpp"
-//#define VIDEO_DEBUG
+
+// 视频调试
+// #define VIDEO_DEBUG
+
+// 手动开火
#define MANUAL_FIRE
-//#define PARM_EDIT
-#define RELEASE
+// 调参模式
+// #define PARM_EDIT
+
+// 发布模式
+#define RELEASE
-int main()
+int main()
{
// 调试信息
fmt::print("[{}] RobotVision_Sirius built on g++ version: {}\n", idntifier, __VERSION__);
fmt::print("[{}] RobotVision_Sirius config file path: {}\n", idntifier, CONFIG_FILE_PATH);
-
+
cv::Mat src_img, roi_img; // src_img 读取图像 roi_img 截取图像
-
+
#ifndef VIDEO_DEBUG
- mindvision::VideoCapture* mv_capture_ = new mindvision::VideoCapture(
- mindvision::CameraParam(0, mindvision::RESOLUTION_1280_X_1024, mindvision::EXPOSURE_40000));
+ mindvision::VideoCapture *mv_capture_ = new mindvision::VideoCapture(
+ mindvision::CameraParam(0, mindvision::RESOLUTION_1280_X_1024, mindvision::EXPOSURE_40000));
cv::VideoCapture cap_ = cv::VideoCapture(0);
#else
-
+
cv::VideoCapture cap_(fmt::format("{}{}", SOURCE_PATH, "/video/new-final-v2.mp4"));
-
+
#endif
- fmt::print("pass\n");
+ fmt::print("Capture init pass.\n");
// 配置文件
-
+
uart::SerialPort serial_ = uart::SerialPort(
- fmt::format("{}{}", CONFIG_FILE_PATH, "/serial/uart_serial_config.xml"));
+ fmt::format("{}{}", CONFIG_FILE_PATH, "/serial/uart_serial_config.xml"));
basic_armor::Detector basic_armor_ = basic_armor::Detector(
- fmt::format("{}{}", CONFIG_FILE_PATH, "/armor/basic_armor_config.xml"));
+ fmt::format("{}{}", CONFIG_FILE_PATH, "/armor/basic_armor_config.xml"));
basic_buff::Detector basic_buff_ = basic_buff::Detector(
- fmt::format("{}{}", CONFIG_FILE_PATH, "/buff/basic_buff_config.xml"));
+ fmt::format("{}{}", CONFIG_FILE_PATH, "/buff/basic_buff_config.xml"));
basic_pnp::PnP pnp_ = basic_pnp::PnP(
- fmt::format("{}{}", CONFIG_FILE_PATH, "/camera/mv_camera_config_407.xml"), fmt::format("{}{}", CONFIG_FILE_PATH, "/angle_solve/basic_pnp_config.xml"));
+ fmt::format("{}{}", CONFIG_FILE_PATH, "/camera/mv_camera_config_407.xml"), fmt::format("{}{}", CONFIG_FILE_PATH, "/angle_solve/basic_pnp_config.xml"));
onnx_inferring::model model_ = onnx_inferring::model(
- fmt::format("{}{}", SOURCE_PATH, "/module/ml/mnist-8.onnx"));
+ fmt::format("{}{}", SOURCE_PATH, "/module/ml/mnist-8.onnx"));
angle_solve::solve solution;
solution.set_config(fmt::format("{}{}", CONFIG_FILE_PATH, "/angle_solve/angle_solve_config.xml"));
@@ -53,59 +60,70 @@ int main()
angle_solve::solve buff_solution;
buff_solution.set_config(fmt::format("{}{}", CONFIG_FILE_PATH, "/angle_solve/buff_angle_solve_config.xml"));
-
+ // 内录，目前弃用，存在保存失败的问题
RecordMode::Record record_ = RecordMode::Record(
- fmt::format("{}{}", CONFIG_FILE_PATH, "/record/recordpath_save.yaml"),
- fmt::format("{}{}", CONFIG_FILE_PATH, "/record/record_packeg/record.avi"),
- cv::Size(1280, 1024)); // 记得修改分辨率
- /* cv::VideoWriter vw_src;
- cv::FileStorage re_config_get(record_.video_save_path_, cv::FileStorage::READ);
- std::string save_path_ = "/record/video/";
- vw_src.open(CONFIG_FILE_PATH+save_path_+ to_string(time_now) + ".mp4", cv::VideoWriter::fourcc('X', '2', '6', '4'), 30, cv::Size(1280, 1024), true);
- assert(vw_src.isOpened());*/
+ fmt::format("{}{}", CONFIG_FILE_PATH, "/record/recordpath_save.yaml"),
+ fmt::format("{}{}", CONFIG_FILE_PATH, "/record/record_packeg/record.avi"),
+ cv::Size(1280, 1024)); // 记得修改分辨率
+ // 内录，目前弃用，存在保存失败的问题
+ /* cv::VideoWriter vw_src;
+ cv::FileStorage re_config_get(record_.video_save_path_, cv::FileStorage::READ);
+ std::string save_path_ = "/record/video/";
+ vw_src.open(CONFIG_FILE_PATH+save_path_+ to_string(time_now) + ".mp4", cv::VideoWriter::fourcc('X', '2', '6', '4'), 30, cv::Size(1280, 1024), true);
+ assert(vw_src.isOpened());*/
basic_roi::RoI save_roi;
- fps::FPS global_fps_;
+ fps::FPS global_fps_;
basic_roi::RoI roi_;
-
- while (true) {
+
+ // 主循环
+ while (true)
+ {
rec_time++;
- fmt::print("[time] {}\n",rec_time);
- if(rec_time>5000)
+ fmt::print("[time] {}\n", rec_time);
+#ifdef RELEASE
+ if (rec_time > 5000)
{
- int i [[maybe_unused]] = std::system("reboot");
+ int i [[maybe_unused]] = std::system("reboot"); // 镜头掉线重启系统
}
+#endif
global_fps_.getTick();
new_buff::new_buff_fps.getTick();
#ifndef VIDEO_DEBUG
- if (mv_capture_->isindustryimgInput()) {
+ if (mv_capture_->isindustryimgInput())
+ {
src_img = mv_capture_->image();
- } else {
- cap_.read(src_img);
}
-#else
+ else
+ {
cap_.read(src_img);
- cv::waitKey(30);
-
+ }
+#else
+ cap_.read(src_img);
+ cv::waitKey(30);
#endif
- if (!src_img.empty()) {
-
- //vw_src << src_img*2;
- //cv::imshow("new",src_img*2);
-
- src_img = src_img*2;//*2
+ if (!src_img.empty())
+ {
+
+ // vw_src << src_img*2;
+ // cv::imshow("new",src_img*2);
+
+ src_img = src_img * 2; // 提升亮度
- //cv::line(src_img,{1024,0},{1024,1024},cv::Scalar(255,0,255),2);
- //cv::imshow("[new]",src_img);
+ // 装甲板参数校准线
+ // cv::line(src_img,{1024,0},{1024,1024},cv::Scalar(255,0,255),2);
+ // cv::imshow("[new]",src_img);
fire = false;
serial_.updateReceiveInformation();
- fmt::print("[MODE] {}\n",serial_.returnReceiveMode());
- switch (serial_.returnReceiveMode()) {
+ fmt::print("[MODE] {}\n", serial_.returnReceiveMode());
+ switch (serial_.returnReceiveMode())
+ {
// 基础自瞄模式
case uart::AUTO_AIM:
- if (basic_armor_.runBasicArmor(src_img, serial_.returnReceive())) {
+ if (basic_armor_.runBasicArmor(src_img, serial_.returnReceive()))
+ {
solution.angleSolve(basic_armor_.returnFinalArmorRotatedRect(0), src_img.size().height, src_img.size().width, serial_);
// 旧的调用
- //pnp_.solvePnP(serial_.returnReceiveBulletVelocity(), basic_armor_.returnFinalArmorDistinguish(0), basic_armor_.returnFinalArmorRotatedRect(0));
+ // pnp_.solvePnP(serial_.returnReceiveBulletVelocity(), basic_armor_.returnFinalArmorDistinguish(0), basic_armor_.returnFinalArmorRotatedRect(0));
}
// 旧的调用
/*serial_.updataWriteData(basic_armor_.returnArmorNum(), fire,
@@ -121,24 +139,26 @@ int main()
break;
// 能量机关
case uart::ENERGY_BUFF:
- //serial_.writeData(basic_buff_.runTask(src_img, serial_.returnReceive()));
- if(basic_buff_.runTask(src_img, serial_.returnReceive())){
- fmt::print("[buff] PASS to\n");
+ // serial_.writeData(basic_buff_.runTask(src_img, serial_.returnReceive()));
+ if (basic_buff_.runTask(src_img, serial_.returnReceive()))
+ {
+ fmt::print("[buff] PASS\n");
buff_solution.angleSolve(basic_buff_.returnObjectRect(), src_img.size().height, src_img.size().width, serial_);
serial_.updataWriteData(1, basic_buff_.isfire(),
- buff_solution.returnYawAngle(),
- buff_solution.returnPitchAngle(),
- basic_buff_.returnObjectforUart(),
- 0);
- } else {
- serial_.updataWriteData(0, 0, 0, 0, {0,0}, 0);
+ buff_solution.returnYawAngle(),
+ buff_solution.returnPitchAngle(),
+ basic_buff_.returnObjectforUart(),
+ 0);
+ }
+ else
+ {
+ serial_.updataWriteData(0, 0, 0, 0, {0, 0}, 0);
}
-
-
break;
- // 击打哨兵模式
+ // 击打哨兵模式
case uart::SENTRY_STRIKE_MODE:
- if (basic_armor_.sentryMode(src_img, serial_.returnReceive())) {
+ if (basic_armor_.sentryMode(src_img, serial_.returnReceive()))
+ {
pnp_.solvePnP(serial_.returnReceiveBulletVelocity(),
basic_armor_.returnFinalArmorDistinguish(0),
basic_armor_.returnFinalArmorRotatedRect(0));
@@ -147,7 +167,9 @@ int main()
pnp_.returnPitchAngle(),
basic_armor_.returnArmorCenter(0),
pnp_.returnDepth());
- } else {
+ }
+ else
+ {
serial_.updataWriteData(basic_armor_.returnLostCnt() > 0 ? 1 : 0, 0,
-pnp_.returnYawAngle(),
pnp_.returnPitchAngle(),
@@ -159,7 +181,8 @@ int main()
// 反小陀螺模式（暂未完善）
case uart::TOP_MODE:
roi_img = roi_.returnROIResultMat(src_img);
- if (basic_armor_.runBasicArmor(roi_img, serial_.returnReceive())) {
+ if (basic_armor_.runBasicArmor(roi_img, serial_.returnReceive()))
+ {
basic_armor_.fixFinalArmorCenter(0, roi_.returnRectTl());
roi_.setLastRoiRect(basic_armor_.returnFinalArmorRotatedRect(0),
basic_armor_.returnFinalArmorDistinguish(0));
@@ -171,7 +194,9 @@ int main()
pnp_.returnPitchAngle(),
basic_armor_.returnArmorCenter(0),
pnp_.returnDepth());
- } else {
+ }
+ else
+ {
serial_.updataWriteData(basic_armor_.returnLostCnt() > 0 ? 1 : 0, 0,
-pnp_.returnYawAngle(),
pnp_.returnPitchAngle(),
@@ -183,28 +208,36 @@ int main()
break;
// 录制视频
case uart::RECORD_MODE:
- //vw_src << src_img;
+ // vw_src << src_img;
break;
// 无人机模式（空缺）
case uart::PLANE_MODE:
break;
// 哨兵模式（添加数字识别便于区分工程和其他车辆）
case uart::SENTINEL_AUTONOMOUS_MODE:
- if (basic_armor_.runBasicArmor(src_img, serial_.returnReceive())) {
- if (basic_armor_.returnFinalArmorDistinguish(0) == 1) {
+ if (basic_armor_.runBasicArmor(src_img, serial_.returnReceive()))
+ {
+ if (basic_armor_.returnFinalArmorDistinguish(0) == 1)
+ {
pnp_.solvePnP(serial_.returnReceiveBulletVelocity(),
- basic_armor_.returnFinalArmorDistinguish(0),
- basic_armor_.returnFinalArmorRotatedRect(0));
+ basic_armor_.returnFinalArmorDistinguish(0),
+ basic_armor_.returnFinalArmorRotatedRect(0));
serial_.updataWriteData(basic_armor_.returnArmorNum(), 0,
pnp_.returnYawAngle(),
pnp_.returnPitchAngle(),
basic_armor_.returnArmorCenter(0),
pnp_.returnDepth());
- } else {
- for (int i = 0; i < basic_armor_.returnArmorNum(); i++) {
+ }
+ else
+ {
+ for (int i = 0; i < basic_armor_.returnArmorNum(); i++)
+ {
if (model_.inferring(save_roi.cutRoIRotatedRect(src_img,
- basic_armor_.returnFinalArmorRotatedRect(i)), 0, 0, src_img) == 2) {
- if (basic_armor_.returnArmorNum() > 1) {
+ basic_armor_.returnFinalArmorRotatedRect(i)),
+ 0, 0, src_img) == 2)
+ {
+ if (basic_armor_.returnArmorNum() > 1)
+ {
pnp_.solvePnP(serial_.returnReceiveBulletVelocity(),
basic_armor_.returnFinalArmorDistinguish(i + 1),
basic_armor_.returnFinalArmorRotatedRect(i + 1));
@@ -214,7 +247,9 @@ int main()
basic_armor_.returnArmorCenter(0),
pnp_.returnDepth());
break;
- } else {
+ }
+ else
+ {
serial_.updataWriteData(0, 0,
pnp_.returnYawAngle(),
pnp_.returnPitchAngle(),
@@ -222,7 +257,9 @@ int main()
pnp_.returnDepth());
break;
}
- } else {
+ }
+ else
+ {
pnp_.solvePnP(serial_.returnReceiveBulletVelocity(),
basic_armor_.returnFinalArmorDistinguish(0),
basic_armor_.returnFinalArmorRotatedRect(0));
@@ -242,13 +279,13 @@ int main()
break;
// 相机标定
case uart::CAMERA_CALIBRATION:
- //cam::create_images(src_img);
- //cam::calibrate();
- //cam::auto_create_images(src_img);
+ // cam::create_images(src_img);
+ // cam::calibrate();
+ // cam::auto_create_images(src_img);
cam::assess(src_img);
- //cam::CalibrationEvaluate();
+ // cam::CalibrationEvaluate();
break;
- default: // 默认进入基础自瞄
+ default: // 默认进入基础自瞄
/*if (basic_armor_.runBasicArmor(src_img, serial_.returnReceive())) {
solution.angleSolve(basic_armor_.returnFinalArmorRotatedRect(0), src_img.size().height, src_img.size().width, serial_);
//pnp_.solvePnP(serial_.returnReceiveBulletVelocity(), basic_armor_.returnFinalArmorDistinguish(0), basic_armor_.returnFinalArmorRotatedRect(0));
@@ -268,17 +305,20 @@ int main()
break;
}
}
- else{
+ else
+ {
#ifdef VIDEO_DEBUG
- //cap_.open(fmt::format("{}{}", SOURCE_PATH, "/video/1080.mp4"));
+ // cap_.open(fmt::format("{}{}", SOURCE_PATH, "/video/1080.mp4"));
#endif
}
- if (record_.last_mode_ != uart::RECORD_MODE && serial_.returnReceiveMode() == uart::RECORD_MODE) {
- //vw_src.release();
+ if (record_.last_mode_ != uart::RECORD_MODE && serial_.returnReceiveMode() == uart::RECORD_MODE)
+ {
+ // vw_src.release();
}
record_.last_mode_ = serial_.returnReceiveMode();
// 非击打哨兵模式时初始化
- if (serial_.returnReceiveMode() != uart::SENTRY_STRIKE_MODE) {
+ if (serial_.returnReceiveMode() != uart::SENTRY_STRIKE_MODE)
+ {
basic_armor_.initializationSentryMode();
}
#ifndef VIDEO_DEBUG
@@ -300,16 +340,20 @@ int main()
*/
// 看门狗放置相机掉线
/*global_fps_.calculateFPSGlobal();*/
- if (global_fps_.returnFps() > 500) {
+ if (global_fps_.returnFps() > 500)
+ {
#ifndef VIDEO_DEBUG
mv_capture_->~VideoCapture();
#endif
- static int counter_for_dev {100};
- static int counter_for_new {30};
- while (!utils::resetMVCamera()) {
- if (!--counter_for_dev) {
- //return 0;
- int i [[maybe_unused]] = std::system("reboot");
+ static int counter_for_dev{100};
+ static int counter_for_new{30};
+ while (!utils::resetMVCamera())
+ {
+ if (!--counter_for_dev)
+ {
+ // return 0;
+ // 下行为掉线重启
+ // int i [[maybe_unused]] = std::system("reboot");
}
usleep(100);
}
@@ -318,12 +362,13 @@ int main()
mv_capture_ = new mindvision::VideoCapture(mindvision::CameraParam(
0, mindvision::RESOLUTION_1280_X_800, mindvision::EXPOSURE_40000));
#endif
- if (!--counter_for_new) {
- //return 0;
- int i [[maybe_unused]] = std::system("reboot");
+ if (!--counter_for_new)
+ {
+ // return 0;
+ // 下行为掉线重启
+ // int i [[maybe_unused]] = std::system("reboot");
}
}
-
}
return 0;
}
diff --git a/base/RobotVision_Sirius.hpp b/base/RobotVision_Sirius.hpp
index e43e72a..1585df7 100644
--- a/base/RobotVision_Sirius.hpp
+++ b/base/RobotVision_Sirius.hpp
@@ -29,5 +29,8 @@
auto idntifier = fmt::format(fg(fmt::color::green) | fmt::emphasis::bold, "RobotVision");
time_t time_now = time(0);
bool fire;
-long long rec_time=0;
-int rec_cnt=0;
\ No newline at end of file
+
+// 已处理帧数个数
+long long rec_time = 0;
+
+int rec_cnt = 0;
\ No newline at end of file
diff --git a/configs/armor/basic_armor_config_new.xml b/configs/armor/basic_armor_config_new.xml
index ae7d4d5..e69de29 100644
--- a/configs/armor/basic_armor_config_new.xml
+++ b/configs/armor/basic_armor_config_new.xml
@@ -1,45 +0,0 @@
-
-
-0
-0
-0
-80
-45
-110
-110
-10
-240
-0
-60
-130
-255
-30
-255
-90
-160
-130
-255
-30
-255
-0
-1
-1
-15
-45
-45
-16
-1000
-0
-1
-1
-2
-20
-5
-30
-10
-10
-80
-11
-33
-42
-
diff --git a/devices/camera/mv_video_capture.cpp b/devices/camera/mv_video_capture.cpp
index 7d4ef64..30d4635 100755
--- a/devices/camera/mv_video_capture.cpp
+++ b/devices/camera/mv_video_capture.cpp
@@ -9,128 +9,151 @@
*/
#include "mv_video_capture.hpp"
-namespace mindvision {
+namespace mindvision
+{
-VideoCapture::VideoCapture(const CameraParam &_camera_param) {
- if (_camera_param.camera_mode == 0) {
- cameraInit(_camera_param.resolution.cols,
- _camera_param.resolution.rows,
- _camera_param.camera_exposuretime);
+ VideoCapture::VideoCapture(const CameraParam &_camera_param)
+ {
+ if (_camera_param.camera_mode == 0)
+ {
+ cameraInit(_camera_param.resolution.cols,
+ _camera_param.resolution.rows,
+ _camera_param.camera_exposuretime);
- iscamera0_open = true;
+ iscamera0_open = true;
fmt::print("[{}] Using mindvision industrial camera: {}\n", idntifier_green, _camera_param.camera_mode);
- } else {
- iscamera0_open = false;
+ }
+ else
+ {
+ iscamera0_open = false;
- fmt::print("[{}] Not using mindvision industrial camera: {}\n", idntifier_green, _camera_param.camera_mode);
+ fmt::print("[{}] Not using mindvision industrial camera: {}\n", idntifier_green, _camera_param.camera_mode);
+ }
}
-}
-VideoCapture::~VideoCapture() {
- if (iscamera0_open) {
- CameraUnInit(hCamera);
- free(g_pRgbBuffer);
+ VideoCapture::~VideoCapture()
+ {
+ if (iscamera0_open)
+ {
+ CameraUnInit(hCamera);
+ free(g_pRgbBuffer);
- fmt::print("[{}] Released mindvision industrial camera: {}\n", idntifier_green, iStatus);
+ fmt::print("[{}] Released mindvision industrial camera: {}\n", idntifier_green, iStatus);
+ }
}
-}
-bool VideoCapture::isindustryimgInput() {
- bool isindustry_camera_open = false;
+ bool VideoCapture::isindustryimgInput()
+ {
+ bool isindustry_camera_open = false;
- if (iscamera0_open == 1) {
- if (CameraGetImageBuffer(hCamera, &sFrameInfo, &pbyBuffer, 1000) == CAMERA_STATUS_SUCCESS) {
- CameraImageProcess(hCamera, pbyBuffer, g_pRgbBuffer, &sFrameInfo);
+ if (iscamera0_open == 1)
+ {
+ if (CameraGetImageBuffer(hCamera, &sFrameInfo, &pbyBuffer, 1000) == CAMERA_STATUS_SUCCESS)
+ {
+ CameraImageProcess(hCamera, pbyBuffer, g_pRgbBuffer, &sFrameInfo);
- if (iplImage) {
- cvReleaseImageHeader(&iplImage);
- }
+ if (iplImage)
+ {
+ cvReleaseImageHeader(&iplImage);
+ }
+
+ iplImage =
+ cvCreateImageHeader(cvSize(sFrameInfo.iWidth, sFrameInfo.iHeight), IPL_DEPTH_8U, channel);
- iplImage =
- cvCreateImageHeader(cvSize(sFrameInfo.iWidth, sFrameInfo.iHeight), IPL_DEPTH_8U, channel);
+ cvSetData(iplImage, g_pRgbBuffer, sFrameInfo.iWidth * channel);
+ }
- cvSetData(iplImage, g_pRgbBuffer, sFrameInfo.iWidth * channel);
+ isindustry_camera_open = true;
+ }
+ else
+ {
+ isindustry_camera_open = false;
}
- isindustry_camera_open = true;
- } else {
- isindustry_camera_open = false;
+ return isindustry_camera_open;
}
- return isindustry_camera_open;
-}
+ int VideoCapture::cameraInit(const int _CAMERA_RESOLUTION_COLS,
+ const int _CAMERA_RESOLUTION_ROWS,
+ const int _CAMERA_EXPOSURETIME)
+ {
+ CameraSdkInit(1);
-int VideoCapture::cameraInit(const int _CAMERA_RESOLUTION_COLS,
- const int _CAMERA_RESOLUTION_ROWS,
- const int _CAMERA_EXPOSURETIME) {
- CameraSdkInit(1);
+ iStatus = CameraEnumerateDevice(&tCameraEnumList, &iCameraCounts);
- iStatus = CameraEnumerateDevice(&tCameraEnumList, &iCameraCounts);
+ if (iCameraCounts == 0)
+ {
+ fmt::print("[{}] Error, no mindvision industrial camera detected: {}\n", idntifier_red, iCameraCounts);
- if (iCameraCounts == 0) {
- fmt::print("[{}] Error, no mindvision industrial camera detected: {}\n", idntifier_red, iCameraCounts);
+ return -1;
+ }
- return -1;
- }
+ // 相机初始化
+ iStatus = CameraInit(&tCameraEnumList, -1, -1, &hCamera);
- // 相机初始化
- iStatus = CameraInit(&tCameraEnumList, -1, -1, &hCamera);
+ if (iStatus != CAMERA_STATUS_SUCCESS)
+ {
+ fmt::print("[{}] Error, Init mindvision industrial camera failed: {}\n", idntifier_red, iStatus);
- if (iStatus != CAMERA_STATUS_SUCCESS) {
- fmt::print("[{}] Error, Init mindvision industrial camera failed: {}\n", idntifier_red, iStatus);
+ return -1;
+ }
- return -1;
- }
+ fmt::print("[{}] Info, Init mindvision industrial camera success: {}\n", idntifier_green, iStatus);
- fmt::print("[{}] Info, Init mindvision industrial camera success: {}\n", idntifier_green, iStatus);
-
- CameraGetCapability(hCamera, &tCapability);
-
- g_pRgbBuffer =
- static_cast(
- malloc(tCapability.sResolutionRange.iHeightMax *
- tCapability.sResolutionRange.iWidthMax * 3));
-
- // 设置相机分辨率
- CameraGetImageResolution(hCamera, &pImageResolution);
-
- pImageResolution.iIndex = 0xFF;
- pImageResolution.iWidthFOV = _CAMERA_RESOLUTION_COLS;
- pImageResolution.iHeightFOV = _CAMERA_RESOLUTION_ROWS;
- pImageResolution.iWidth = _CAMERA_RESOLUTION_COLS;
- pImageResolution.iHeight = _CAMERA_RESOLUTION_ROWS;
- pImageResolution.iHOffsetFOV = static_cast((1280 - _CAMERA_RESOLUTION_COLS) * 0.5);
- pImageResolution.iVOffsetFOV = static_cast((1024 - _CAMERA_RESOLUTION_ROWS) * 0.5);
-
- CameraSetImageResolution(hCamera, &pImageResolution);
-
- // 设置曝光时间
- CameraGetAeState(hCamera, &AEstate);
- CameraSetAeState(hCamera, FALSE);
- CameraSetExposureTime(hCamera, _CAMERA_EXPOSURETIME);
- // 设置颜色增益
- CameraSetGain(hCamera, 145, 130, 105);
-
- // 让SDK进入工作模式
- CameraPlay(hCamera);
- CameraReleaseImageBuffer(hCamera, pbyBuffer);
-
- if (tCapability.sIspCapacity.bMonoSensor) {
- channel = 1;
- CameraSetIspOutFormat(hCamera, CAMERA_MEDIA_TYPE_MONO8);
- } else {
- channel = 3;
- CameraSetIspOutFormat(hCamera, CAMERA_MEDIA_TYPE_BGR8);
- }
+ CameraGetCapability(hCamera, &tCapability);
- return 1;
-}
+ g_pRgbBuffer =
+ static_cast(
+ malloc(tCapability.sResolutionRange.iHeightMax *
+ tCapability.sResolutionRange.iWidthMax * 3));
-void VideoCapture::cameraReleasebuff() {
- // After successfully calling CameraGetImageBuffer, CameraReleaseImageBuffer must be called to release the acquired buffer
- // Otherwise, when CameraGetImageBuffer is called again, the program will hang and block until CameraReleaseImageBuffer is called in another thread
- if (iscamera0_open) { CameraReleaseImageBuffer(hCamera, pbyBuffer); }
-}
+ // 设置相机分辨率
+ CameraGetImageResolution(hCamera, &pImageResolution);
+
+ pImageResolution.iIndex = 0xFF;
+ pImageResolution.iWidthFOV = _CAMERA_RESOLUTION_COLS;
+ pImageResolution.iHeightFOV = _CAMERA_RESOLUTION_ROWS;
+ pImageResolution.iWidth = _CAMERA_RESOLUTION_COLS;
+ pImageResolution.iHeight = _CAMERA_RESOLUTION_ROWS;
+ pImageResolution.iHOffsetFOV = static_cast((1280 - _CAMERA_RESOLUTION_COLS) * 0.5);
+ pImageResolution.iVOffsetFOV = static_cast((1024 - _CAMERA_RESOLUTION_ROWS) * 0.5);
+
+ CameraSetImageResolution(hCamera, &pImageResolution);
+
+ // 设置曝光时间
+ CameraGetAeState(hCamera, &AEstate);
+ CameraSetAeState(hCamera, FALSE);
+ CameraSetExposureTime(hCamera, _CAMERA_EXPOSURETIME);
+ // 设置颜色增益
+ CameraSetGain(hCamera, 145, 130, 105);
+
+ // 让SDK进入工作模式
+ CameraPlay(hCamera);
+ CameraReleaseImageBuffer(hCamera, pbyBuffer);
+
+ if (tCapability.sIspCapacity.bMonoSensor)
+ {
+ channel = 1;
+ CameraSetIspOutFormat(hCamera, CAMERA_MEDIA_TYPE_MONO8);
+ }
+ else
+ {
+ channel = 3;
+ CameraSetIspOutFormat(hCamera, CAMERA_MEDIA_TYPE_BGR8);
+ }
+
+ return 1;
+ }
+
+ void VideoCapture::cameraReleasebuff()
+ {
+ // After successfully calling CameraGetImageBuffer, CameraReleaseImageBuffer must be called to release the acquired buffer
+ // Otherwise, when CameraGetImageBuffer is called again, the program will hang and block until CameraReleaseImageBuffer is called in another thread
+ if (iscamera0_open)
+ {
+ CameraReleaseImageBuffer(hCamera, pbyBuffer);
+ }
+ }
-} // namespace mindvision
+} // namespace mindvision
diff --git a/devices/camera/mv_video_capture.hpp b/devices/camera/mv_video_capture.hpp
index 94581d2..0cee4de 100755
--- a/devices/camera/mv_video_capture.hpp
+++ b/devices/camera/mv_video_capture.hpp
@@ -17,40 +17,46 @@
#include
-namespace mindvision {
-
-auto idntifier_green = fmt::format(fg(fmt::color::green) | fmt::emphasis::bold, "mv_video_capture");
-auto idntifier_red = fmt::format(fg(fmt::color::red) | fmt::emphasis::bold, "mv_video_capture");
-
-enum EXPOSURETIME {
- // 相机曝光时间
- EXPOSURE_100000 = 100000,
- EXPOSURE_80000 = 80000,
- EXPOSURE_60000 = 60000,
- EXPOSURE_40000 = 40000,
- EXPOSURE_20000 = 20000,
- EXPOSURE_10000 = 10000,
- EXPOSURE_5000 = 5000,
- EXPOSURE_2500 = 2500,
- EXPOSURE_1200 = 1200,
- EXPOSURE_800 = 800,
- EXPOSURE_600 = 600,
- EXPOSURE_400 = 400,
-};
-
-enum RESOLUTION {
- // 相机分辨率
- RESOLUTION_1280_X_1024,
- RESOLUTION_1280_X_800,
- RESOLUTION_640_X_480,
-};
-
-struct Camera_Resolution {
- int cols;
- int rows;
- // 设置相机分辨率
- explicit Camera_Resolution(const mindvision::RESOLUTION _resolution) {
- switch (_resolution) {
+namespace mindvision
+{
+
+ auto idntifier_green = fmt::format(fg(fmt::color::green) | fmt::emphasis::bold, "mv_video_capture");
+ auto idntifier_red = fmt::format(fg(fmt::color::red) | fmt::emphasis::bold, "mv_video_capture");
+
+ enum EXPOSURETIME
+ {
+ // 相机曝光时间
+ EXPOSURE_100000 = 100000,
+ EXPOSURE_80000 = 80000,
+ EXPOSURE_60000 = 60000,
+ EXPOSURE_40000 = 40000,
+ EXPOSURE_20000 = 20000,
+ EXPOSURE_10000 = 10000,
+ EXPOSURE_5000 = 5000,
+ EXPOSURE_2500 = 2500,
+ EXPOSURE_1200 = 1200,
+ EXPOSURE_800 = 800,
+ EXPOSURE_600 = 600,
+ EXPOSURE_400 = 400,
+ };
+
+ enum RESOLUTION
+ {
+ // 相机分辨率
+ RESOLUTION_1280_X_1024,
+ RESOLUTION_1280_X_800,
+ RESOLUTION_640_X_480,
+ };
+
+ struct Camera_Resolution
+ {
+ int cols;
+ int rows;
+ // 设置相机分辨率
+ explicit Camera_Resolution(const mindvision::RESOLUTION _resolution)
+ {
+ switch (_resolution)
+ {
case mindvision::RESOLUTION::RESOLUTION_1280_X_1024:
cols = 1280;
rows = 1024;
@@ -67,76 +73,78 @@ struct Camera_Resolution {
cols = 1280;
rows = 800;
break;
+ }
}
- }
-};
-
-struct CameraParam {
- int camera_mode;
- int camera_exposuretime;
-
- mindvision::Camera_Resolution resolution;
-
- CameraParam(const int _camera_mode,
- const mindvision::RESOLUTION _resolution,
- const mindvision::EXPOSURETIME _camera_exposuretime)
- : camera_mode(_camera_mode),
- camera_exposuretime(_camera_exposuretime),
- resolution(_resolution) {}
-};
-
-class VideoCapture {
- public:
- VideoCapture() = default;
- explicit VideoCapture(const mindvision::CameraParam &_camera_param);
-
- ~VideoCapture();
- /**
- * @brief 判断工业相机是否在线
- *
- * @return true 检测到工业相机
- * @return false 没检测到工业相机
- */
- bool isindustryimgInput();
- /**
- * @brief 清空相机内存（每次读取相机后进行清空）
- *
- */
- void cameraReleasebuff();
- /**
- * @brief 设置相机参数
- *
- * @param _CAMERA_RESOLUTION_COLS 设置相机宽度
- * @param _CAMERA_RESOLUTION_ROWS 设置相机高度
- * @param _CAMERA_EXPOSURETIME 设置相机曝光
- * @return int
- */
- int cameraInit(const int _CAMERA_RESOLUTION_COLS,
- const int _CAMERA_RESOLUTION_ROWS,
- const int _CAMERA_EXPOSURETIME);
- /**
- * @brief 返回相机读取图片
- *
- * @return cv::Mat
- */
- inline cv::Mat image() const { return cv::cvarrToMat(iplImage, true); }
-
- private:
- unsigned char* g_pRgbBuffer;
-
- int iCameraCounts = 1;
- int iStatus = -1;
- int hCamera;
- int channel = 3;
- bool iscamera0_open = false;
-
- tSdkCameraDevInfo tCameraEnumList;
- tSdkCameraCapbility tCapability;
- tSdkFrameHead sFrameInfo;
- tSdkImageResolution pImageResolution;
- BYTE* pbyBuffer;
- BOOL AEstate = FALSE;
- IplImage* iplImage = nullptr;
-};
-
-} // namespace mindvision
+ };
+
+ struct CameraParam
+ {
+ int camera_mode;
+ int camera_exposuretime;
+
+ mindvision::Camera_Resolution resolution;
+
+ CameraParam(const int _camera_mode,
+ const mindvision::RESOLUTION _resolution,
+ const mindvision::EXPOSURETIME _camera_exposuretime)
+ : camera_mode(_camera_mode),
+ camera_exposuretime(_camera_exposuretime),
+ resolution(_resolution) {}
+ };
+
+ class VideoCapture
+ {
+ public:
+ VideoCapture() = default;
+ explicit VideoCapture(const mindvision::CameraParam &_camera_param);
+
+ ~VideoCapture();
+ /**
+ * @brief 判断工业相机是否在线
+ *
+ * @return true 检测到工业相机
+ * @return false 没检测到工业相机
+ */
+ bool isindustryimgInput();
+ /**
+ * @brief 清空相机内存（每次读取相机后进行清空）
+ *
+ */
+ void cameraReleasebuff();
+ /**
+ * @brief 设置相机参数
+ *
+ * @param _CAMERA_RESOLUTION_COLS 设置相机宽度
+ * @param _CAMERA_RESOLUTION_ROWS 设置相机高度
+ * @param _CAMERA_EXPOSURETIME 设置相机曝光
+ * @return int
+ */
+ int cameraInit(const int _CAMERA_RESOLUTION_COLS,
+ const int _CAMERA_RESOLUTION_ROWS,
+ const int _CAMERA_EXPOSURETIME);
+ /**
+ * @brief 返回相机读取图片
+ *
+ * @return cv::Mat
+ */
+ inline cv::Mat image() const { return cv::cvarrToMat(iplImage, true); }
+
+ private:
+ unsigned char *g_pRgbBuffer;
+
+ int iCameraCounts = 1;
+ int iStatus = -1;
+ int hCamera;
+ int channel = 3;
+ bool iscamera0_open = false;
+
+ tSdkCameraDevInfo tCameraEnumList;
+ tSdkCameraCapbility tCapability;
+ tSdkFrameHead sFrameInfo;
+ tSdkImageResolution pImageResolution;
+ BYTE *pbyBuffer;
+ BOOL AEstate = FALSE;
+ IplImage *iplImage = nullptr;
+ };
+
+} // namespace mindvision
diff --git a/devices/serial/uart_serial.cpp b/devices/serial/uart_serial.cpp
index 43e8dcf..aa98cf7 100755
--- a/devices/serial/uart_serial.cpp
+++ b/devices/serial/uart_serial.cpp
@@ -4,40 +4,46 @@
* @brief 串口通讯
* @date 2023-01-18
* @copyright Copyright (c) 2023 Sirius
- *
+ *
*/
#include "uart_serial.hpp"
#define RELEASE
+namespace uart
+{
-namespace uart {
-
-SerialPort::SerialPort(std::string _serial_config) {
- cv::FileStorage fs_serial(_serial_config, cv::FileStorage::READ);
+ SerialPort::SerialPort(std::string _serial_config)
+ {
+ cv::FileStorage fs_serial(_serial_config, cv::FileStorage::READ);
- fs_serial["PREFERRED_DEVICE"] >> serial_config_.preferred_device;
- fs_serial["SET_BAUDRATE"] >> serial_config_.set_baudrate;
- fs_serial["SHOW_SERIAL_INFORMATION"] >> serial_config_.show_serial_information;
+ fs_serial["PREFERRED_DEVICE"] >> serial_config_.preferred_device;
+ fs_serial["SET_BAUDRATE"] >> serial_config_.set_baudrate;
+ fs_serial["SHOW_SERIAL_INFORMATION"] >> serial_config_.show_serial_information;
- const char* DeviceName[] = {serial_config_.preferred_device.c_str(), "/dev/ttyUSB1", "/dev/ttyUSB2", "/dev/ttyUSB3", "/dev/ttyACM0", "/dev/ttyACM1"};
+ const char *DeviceName[] = {serial_config_.preferred_device.c_str(), "/dev/ttyUSB1", "/dev/ttyUSB2", "/dev/ttyUSB3", "/dev/ttyACM0", "/dev/ttyACM1"};
- struct termios newstate;
- bzero(&newstate, sizeof(newstate));
+ struct termios newstate;
+ bzero(&newstate, sizeof(newstate));
- for (size_t i = 0; i != sizeof(DeviceName) / sizeof(char*); ++i) {
- fd = open(DeviceName[i], O_RDWR | O_NONBLOCK | O_NOCTTY | O_NDELAY);
- if (fd == -1) {
- fmt::print("[{}] Open serial device failed: {}\n", idntifier_red, DeviceName[i]);
- } else {
- fmt::print("[{}] Open serial device success: {}\n", idntifier_green, DeviceName[i]);
+ for (size_t i = 0; i != sizeof(DeviceName) / sizeof(char *); ++i)
+ {
+ fd = open(DeviceName[i], O_RDWR | O_NONBLOCK | O_NOCTTY | O_NDELAY);
+ if (fd == -1)
+ {
+ fmt::print("[{}] Open serial device failed: {}\n", idntifier_red, DeviceName[i]);
+ }
+ else
+ {
+ fmt::print("[{}] Open serial device success: {}\n", idntifier_green, DeviceName[i]);
- break;
+ break;
+ }
}
- }
- switch (serial_config_.set_baudrate) {
+ switch (serial_config_.set_baudrate)
+ {
case 1:
cfsetospeed(&newstate, B115200);
cfsetispeed(&newstate, B115200);
@@ -50,271 +56,312 @@ SerialPort::SerialPort(std::string _serial_config) {
cfsetospeed(&newstate, B115200);
cfsetispeed(&newstate, B115200);
break;
+ }
+
+ newstate.c_cflag |= CLOCAL | CREAD;
+ newstate.c_cflag &= ~CSIZE;
+ newstate.c_cflag &= ~CSTOPB;
+ newstate.c_cflag |= CS8;
+ newstate.c_cflag &= ~PARENB;
+
+ newstate.c_cc[VTIME] = 0;
+ newstate.c_cc[VMIN] = 0;
+
+ tcflush(fd, TCIOFLUSH);
+ tcsetattr(fd, TCSANOW, &newstate);
}
- newstate.c_cflag |= CLOCAL | CREAD;
- newstate.c_cflag &= ~CSIZE;
- newstate.c_cflag &= ~CSTOPB;
- newstate.c_cflag |= CS8;
- newstate.c_cflag &= ~PARENB;
-
- newstate.c_cc[VTIME] = 0;
- newstate.c_cc[VMIN] = 0;
-
- tcflush(fd, TCIOFLUSH);
- tcsetattr(fd, TCSANOW, &newstate);
-}
-
-SerialPort::~SerialPort(void) {
- if (!close(fd)) { fmt::print("[{}] Close serial device success: {}\n", idntifier_green, fd); }
-}
-
-void SerialPort::receiveData() {
- //get_flag = false;
- // 初始化接收数据为0
- memset(receive_buff_, 0, REC_INFO_LENGTH * 2);
- // 接收数据
- read_message_ = read(fd, receive_buff_temp_, sizeof(receive_buff_temp_));
- fmt::print("[info] in 10 {}\n", sizeof(receive_buff_temp_));
- //printf("%s\n",read_message_);
- fmt::print("[info] in 10 {} {}\n", receive_buff_temp_[0],receive_buff_temp_[1]);
- for (int i = 0; i != sizeof(receive_buff_temp_); ++i) {
- fmt::print("{:d} ",receive_buff_temp_[i]);
- if (receive_buff_temp_[i] == 'S' && receive_buff_temp_[i + sizeof(receive_buff_) - 1] == 'E') {
- if(!get_flag) {
- get_flag = true;
+ SerialPort::~SerialPort(void)
+ {
+ if (!close(fd))
+ {
+ fmt::print("[{}] Close serial device success: {}\n", idntifier_green, fd);
+ }
+ }
- }
- fmt::print("[info] insert {:d}\n", receive_buff_temp_[i]);
- if (serial_config_.show_serial_information == 1) {
- fmt::print("[{}] receiveData() ->", idntifier_green);
- for (size_t j = 0; j != sizeof(receive_buff_); ++j) {
- receive_buff_[j] = receive_buff_temp_[i + j];
- fmt::print(" {:d}", receive_buff_[j]);
+ void SerialPort::receiveData()
+ {
+ // get_flag = false;
+ // 初始化接收数据为0
+ memset(receive_buff_, 0, REC_INFO_LENGTH * 2);
+ // 接收数据
+ read_message_ = read(fd, receive_buff_temp_, sizeof(receive_buff_temp_));
+ fmt::print("[info] in 10 {}\n", sizeof(receive_buff_temp_));
+ // printf("%s\n",read_message_);
+ fmt::print("[info] in 10 {} {}\n", receive_buff_temp_[0], receive_buff_temp_[1]);
+ for (int i = 0; i != sizeof(receive_buff_temp_); ++i)
+ {
+ fmt::print("{:d} ", receive_buff_temp_[i]);
+ if (receive_buff_temp_[i] == 'S' && receive_buff_temp_[i + sizeof(receive_buff_) - 1] == 'E')
+ {
+ if (!get_flag)
+ {
+ get_flag = true;
+ }
+ fmt::print("[info] insert {:d}\n", receive_buff_temp_[i]);
+ if (serial_config_.show_serial_information == 1)
+ {
+ fmt::print("[{}] receiveData() ->", idntifier_green);
+ for (size_t j = 0; j != sizeof(receive_buff_); ++j)
+ {
+ receive_buff_[j] = receive_buff_temp_[i + j];
+ fmt::print(" {:d}", receive_buff_[j]);
+ }
+ fmt::print("\n");
}
- fmt::print("\n");
- } else {
- for (size_t j = 0; j != sizeof(receive_buff_); ++j) {
- receive_buff_[j] = receive_buff_temp_[i + j];
+ else
+ {
+ for (size_t j = 0; j != sizeof(receive_buff_); ++j)
+ {
+ receive_buff_[j] = receive_buff_temp_[i + j];
+ }
}
+ break;
}
- break;
}
-
+ fmt::print("\n");
+ tcflush(fd, TCIFLUSH);
}
- fmt::print("\n");
- tcflush(fd, TCIFLUSH);
-}
-
-void SerialPort::writeData(const int& data_type,
- const int& is_shooting,
- const int16_t& yaw,
- const int16_t& pitch,
- const Write_Data::node& cord,
- const int16_t& depth) {
- //getDataForCRC(data_type, is_shooting, _yaw, yaw, _pitch, pitch, depth, predict_cord, predict_length, predict_width);
- //uint8_t CRC = checksumCRC(crc_buff_, sizeof(crc_buff_))
- getDataForSend(data_type, is_shooting, yaw, pitch, cord, depth, 0);
- write_buff_[0]='S';
- write_buff_[14]='E';
- write_message_ = write(fd, write_buff_, sizeof(write_buff_));
- //memset(write_buff_, 0, sizeof(write_buff_));
- if (serial_config_.show_serial_information == 1) {
- yaw_reduction_ = mergeIntoBytes(write_buff_[4], write_buff_[3]);
- pitch_reduction_ = mergeIntoBytes(write_buff_[6], write_buff_[5]);
- cord_reduction_x = mergeIntoBytes(write_buff_[8], write_buff_[7]);
- cord_reduction_y = mergeIntoBytes(write_buff_[10], write_buff_[9]);
- depth_reduction_ = mergeIntoBytes(write_buff_[12], write_buff_[11]);
-
- fmt::print("[{}] writeData() ->", idntifier_green);
- for (size_t i = 0; i < 3; ++i) { fmt::print(" {}", write_buff_[i]); }
+
+ void SerialPort::writeData(const int &data_type,
+ const int &is_shooting,
+ const int16_t &yaw,
+ const int16_t &pitch,
+ const Write_Data::node &cord,
+ const int16_t &depth)
+ {
+ // getDataForCRC(data_type, is_shooting, _yaw, yaw, _pitch, pitch, depth, predict_cord, predict_length, predict_width);
+ // uint8_t CRC = checksumCRC(crc_buff_, sizeof(crc_buff_))
+ getDataForSend(data_type, is_shooting, yaw, pitch, cord, depth, 0);
+ write_buff_[0] = 'S';
+ write_buff_[14] = 'E';
+ write_message_ = write(fd, write_buff_, sizeof(write_buff_));
+ // memset(write_buff_, 0, sizeof(write_buff_));
+ if (serial_config_.show_serial_information == 1)
+ {
+ yaw_reduction_ = mergeIntoBytes(write_buff_[4], write_buff_[3]);
+ pitch_reduction_ = mergeIntoBytes(write_buff_[6], write_buff_[5]);
+ cord_reduction_x = mergeIntoBytes(write_buff_[8], write_buff_[7]);
+ cord_reduction_y = mergeIntoBytes(write_buff_[10], write_buff_[9]);
+ depth_reduction_ = mergeIntoBytes(write_buff_[12], write_buff_[11]);
+
+ fmt::print("[{}] writeData() ->", idntifier_green);
+ for (size_t i = 0; i < 3; ++i)
+ {
+ fmt::print(" {}", write_buff_[i]);
+ }
fmt::print(" {} {} {} {} {}",
- static_cast(yaw_reduction_) / 100,
- static_cast(pitch_reduction_) / 100,
- static_cast(cord_reduction_x),
- static_cast(cord_reduction_y),
- static_cast(depth_reduction_));
- for (size_t i = 13; i < 15; ++i) { fmt::print(" {}", write_buff_[i]); }
- fmt::print("\n");
+ static_cast(yaw_reduction_) / 100,
+ static_cast(pitch_reduction_) / 100,
+ static_cast(cord_reduction_x),
+ static_cast(cord_reduction_y),
+ static_cast(depth_reduction_));
+ for (size_t i = 13; i < 15; ++i)
+ {
+ fmt::print(" {}", write_buff_[i]);
+ }
+ fmt::print("\n");
- yaw_reduction_ = 0x0000;
- pitch_reduction_ = 0x0000;
- depth_reduction_ = 0x0000;
- //memset(write_buff_, 0, sizeof(write_buff_));
+ yaw_reduction_ = 0x0000;
+ pitch_reduction_ = 0x0000;
+ depth_reduction_ = 0x0000;
+ // memset(write_buff_, 0, sizeof(write_buff_));
+ }
}
-}
-
-void SerialPort::writeData(const Write_Data& _write_data) {
- write_data_.data_type = _write_data.data_type > 1 ? 1 : _write_data.data_type;
- write_data_.is_shooting = _write_data.is_shooting;
- write_data_.yaw = _write_data.yaw * 100;
- write_data_.pitch = _write_data.pitch * 100;
- write_data_.cord = _write_data.cord;
- write_data_.depth = 0;
-
- writeData(write_data_.data_type,
- write_data_.is_shooting,
- write_data_.yaw,
- write_data_.pitch,
- write_data_.cord,
- write_data_.depth);
-}
-
-void SerialPort::writeData() {
- writeData(write_data_.data_type,
- write_data_.is_shooting,
- write_data_.yaw,
- write_data_.pitch,
- write_data_.cord,
- write_data_.depth);
-}
-
-//this active
-void SerialPort::updataWriteData(const int _data_type,
- const int _is_shooting,
- const float _yaw,
- const float _pitch,
- const Write_Data::node _cord,
- const int _depth) {
- write_data_.data_type = _data_type > 1 ? 1 : _data_type;
- write_data_.is_shooting = _is_shooting;
- /*if(fabs(_yaw) < 2.0)
+
+ void SerialPort::writeData(const Write_Data &_write_data)
{
- write_data_.yaw = 0.0;
+ write_data_.data_type = _write_data.data_type > 1 ? 1 : _write_data.data_type;
+ write_data_.is_shooting = _write_data.is_shooting;
+ write_data_.yaw = _write_data.yaw * 100;
+ write_data_.pitch = _write_data.pitch * 100;
+ write_data_.cord = _write_data.cord;
+ write_data_.depth = 0;
+
+ writeData(write_data_.data_type,
+ write_data_.is_shooting,
+ write_data_.yaw,
+ write_data_.pitch,
+ write_data_.cord,
+ write_data_.depth);
}
- else
+
+ void SerialPort::writeData()
{
- write_data_.yaw = _yaw * 100;
+ writeData(write_data_.data_type,
+ write_data_.is_shooting,
+ write_data_.yaw,
+ write_data_.pitch,
+ write_data_.cord,
+ write_data_.depth);
}
- if(fabs(_pitch) < 2.0)
+
+ // this active
+ void SerialPort::updataWriteData(const int _data_type,
+ const int _is_shooting,
+ const float _yaw,
+ const float _pitch,
+ const Write_Data::node _cord,
+ const int _depth)
{
- write_data_.pitch = 0.0;
+ write_data_.data_type = _data_type > 1 ? 1 : _data_type;
+ write_data_.is_shooting = _is_shooting;
+ /*if(fabs(_yaw) < 2.0)
+ {
+ write_data_.yaw = 0.0;
+ }
+ else
+ {
+ write_data_.yaw = _yaw * 100;
+ }
+ if(fabs(_pitch) < 2.0)
+ {
+ write_data_.pitch = 0.0;
+ }
+ else
+ {
+ write_data_.pitch = -_pitch * 100;
+ }*/
+ write_data_.yaw = kal.run(_yaw) * 100;
+ write_data_.pitch = kal.run(-_pitch) * 100;
+ write_data_.cord = _cord;
+ write_data_.depth = _depth;
+ writeData();
}
- else
+
+ Write_Data SerialPort::gainWriteData(const int _data_type,
+ const int _is_shooting,
+ const float _yaw,
+ const float _pitch,
+ const Write_Data::node _cord,
+ const int _depth)
{
- write_data_.pitch = -_pitch * 100;
- }*/
- write_data_.yaw = kal.run(_yaw)*100;
- write_data_.pitch = kal.run(-_pitch)*100;
- write_data_.cord = _cord;
- write_data_.depth = _depth;
- writeData();
-}
-
-Write_Data SerialPort::gainWriteData(const int _data_type,
- const int _is_shooting,
- const float _yaw,
- const float _pitch,
- const Write_Data::node _cord,
- const int _depth) {
- Write_Data write_data;
- write_data.data_type = _data_type > 1 ? 1 : _data_type;
- write_data.is_shooting = _is_shooting;
- write_data.yaw = _yaw * 100;
- write_data.pitch = _pitch * 100;
- write_data_.cord = _cord;
- write_data.depth = _depth;
- return write_data;
-}
-
-uint8_t SerialPort::checksumCRC(unsigned char* buf, uint16_t len) {
- uint8_t check = 0;
-
- while (len--) { check = CRC8_Table[check ^ (*buf++)]; }
-
- return check;
-}
-
-void SerialPort::getDataForCRC(const int& data_type,
- const int& is_shooting,
- const int16_t& yaw,
- const int16_t& pitch,
- const Write_Data::node& cord,
- const int16_t& depth) {
- crc_buff_[0] = 0x53;
- crc_buff_[1] = static_cast(data_type);
- crc_buff_[2] = static_cast(is_shooting);
- crc_buff_[3] = returnLowBit(yaw);
- crc_buff_[4] = returnHighBit(yaw);
- crc_buff_[5] = returnLowBit(pitch);
- crc_buff_[6] = returnHighBit(pitch);
- crc_buff_[7] = returnLowBit(cord.x);
- crc_buff_[8] = returnHighBit(cord.x);
- crc_buff_[9] = returnLowBit(cord.y);
- crc_buff_[10] = returnHighBit(cord.y);
- crc_buff_[11] = returnLowBit(depth);
- crc_buff_[12] = returnHighBit(depth);
-}
-
-void SerialPort::getDataForSend(const int& data_type,
- const int& is_shooting,
- const int16_t& yaw,
- const int16_t& pitch,
- const Write_Data::node& cord,
- const int16_t& depth,
- const uint8_t& CRC) {
- write_buff_[0] = 0x53;
- write_buff_[1] = static_cast(data_type);
- write_buff_[2] = static_cast(is_shooting);
- fmt::print("[info] {}\n", pitch);
- if(data_type == 0) {
- write_buff_[3] = 0;
- write_buff_[4] = 0;
- write_buff_[5] = 0;
- write_buff_[6] = 0;
- write_buff_[7] = 0;
- write_buff_[8] = 0;
- write_buff_[9] = 0;
- write_buff_[10] = 0;
- write_buff_[11] = 0;
- write_buff_[12] = 0;
- } else {
- write_buff_[3] = returnLowBit(yaw);
- write_buff_[4] = returnHighBit(yaw);
- write_buff_[5] = returnLowBit(pitch);
- write_buff_[6] = returnHighBit(pitch);
- //write_buff_[5] = returnLowBit(0);
- //write_buff_[6] = returnHighBit(0);
- write_buff_[7] = returnLowBit(cord.x);
- write_buff_[8] = returnHighBit(cord.x);
- write_buff_[9] = returnLowBit(cord.y);
- write_buff_[10] = returnHighBit(cord.y);
- write_buff_[11] = returnLowBit(depth);
- write_buff_[12] = returnHighBit(depth);
+ Write_Data write_data;
+ write_data.data_type = _data_type > 1 ? 1 : _data_type;
+ write_data.is_shooting = _is_shooting;
+ write_data.yaw = _yaw * 100;
+ write_data.pitch = _pitch * 100;
+ write_data_.cord = _cord;
+ write_data.depth = _depth;
+ return write_data;
}
- write_buff_[13] = 0; //CRC & 0xff;
- write_buff_[14] = 0x45;
-}
-
-bool SerialPort::isEmpty() {
- if(get_flag) {
- receive_data_.old_flag = false;
- get_flag = false;
- return false;
+
+ uint8_t SerialPort::checksumCRC(unsigned char *buf, uint16_t len)
+ {
+ uint8_t check = 0;
+
+ while (len--)
+ {
+ check = CRC8_Table[check ^ (*buf++)];
+ }
+
+ return check;
}
- else {
- receive_data_.old_flag = true;
- get_flag = false;
- return true;
+
+ void SerialPort::getDataForCRC(const int &data_type,
+ const int &is_shooting,
+ const int16_t &yaw,
+ const int16_t &pitch,
+ const Write_Data::node &cord,
+ const int16_t &depth)
+ {
+ crc_buff_[0] = 0x53;
+ crc_buff_[1] = static_cast(data_type);
+ crc_buff_[2] = static_cast(is_shooting);
+ crc_buff_[3] = returnLowBit(yaw);
+ crc_buff_[4] = returnHighBit(yaw);
+ crc_buff_[5] = returnLowBit(pitch);
+ crc_buff_[6] = returnHighBit(pitch);
+ crc_buff_[7] = returnLowBit(cord.x);
+ crc_buff_[8] = returnHighBit(cord.x);
+ crc_buff_[9] = returnLowBit(cord.y);
+ crc_buff_[10] = returnHighBit(cord.y);
+ crc_buff_[11] = returnLowBit(depth);
+ crc_buff_[12] = returnHighBit(depth);
}
-}
-
-void SerialPort::updateReceiveInformation() {
- //cv::waitKey(50);
- receiveData();
- fmt::print("[info] in 1\n");
- if (!isEmpty()) {
- if(pre_mode != receive_buff_[2]) {
+
+ void SerialPort::getDataForSend(const int &data_type,
+ const int &is_shooting,
+ const int16_t &yaw,
+ const int16_t &pitch,
+ const Write_Data::node &cord,
+ const int16_t &depth,
+ const uint8_t &CRC)
+ {
+ write_buff_[0] = 0x53;
+ write_buff_[1] = static_cast(data_type);
+ write_buff_[2] = static_cast(is_shooting);
+ fmt::print("[info] {}\n", pitch);
+ if (data_type == 0)
+ {
+ write_buff_[3] = 0;
+ write_buff_[4] = 0;
+ write_buff_[5] = 0;
+ write_buff_[6] = 0;
+ write_buff_[7] = 0;
+ write_buff_[8] = 0;
+ write_buff_[9] = 0;
+ write_buff_[10] = 0;
+ write_buff_[11] = 0;
+ write_buff_[12] = 0;
+ }
+ else
+ {
+ write_buff_[3] = returnLowBit(yaw);
+ write_buff_[4] = returnHighBit(yaw);
+ write_buff_[5] = returnLowBit(pitch);
+ write_buff_[6] = returnHighBit(pitch);
+ // write_buff_[5] = returnLowBit(0);
+ // write_buff_[6] = returnHighBit(0);
+ write_buff_[7] = returnLowBit(cord.x);
+ write_buff_[8] = returnHighBit(cord.x);
+ write_buff_[9] = returnLowBit(cord.y);
+ write_buff_[10] = returnHighBit(cord.y);
+ write_buff_[11] = returnLowBit(depth);
+ write_buff_[12] = returnHighBit(depth);
+ }
+ write_buff_[13] = 0; // CRC & 0xff;
+ write_buff_[14] = 0x45;
+ }
+
+ bool SerialPort::isEmpty()
+ {
+ if (get_flag)
+ {
+ receive_data_.old_flag = false;
+ get_flag = false;
+ return false;
+ }
+ else
+ {
+ receive_data_.old_flag = true;
+ get_flag = false;
+ return true;
+ }
+ }
+
+ void SerialPort::updateReceiveInformation()
+ {
+ // cv::waitKey(50);
+ receiveData();
+ fmt::print("[info] in 1\n");
+ if (!isEmpty())
+ {
+ if (pre_mode != receive_buff_[2])
+ {
#ifndef RELEASE
- cv::destroyAllWindows();
+ cv::destroyAllWindows();
#endif
- pre_mode = receive_buff_[2];
+ pre_mode = receive_buff_[2];
+ }
}
- } else {
- return;
- }
- fmt::print("[info] in 2\n");
- switch (receive_buff_[1]) {
+ else
+ {
+ return;
+ }
+ fmt::print("[info] in 2\n");
+ switch (receive_buff_[1])
+ {
case RED:
receive_data_.my_color = RED;
fmt::print("[info] My color is RED\n");
@@ -327,42 +374,44 @@ void SerialPort::updateReceiveInformation() {
receive_data_.my_color = ALL;
fmt::print("[info] My color is ALL\n");
break;
- }
- fmt::print("[rec info] GET\n");
- switch (receive_buff_[2]) {
- case AUTO_AIM:
- receive_data_.now_run_mode = AUTO_AIM;
- break;
- case ENERGY_BUFF:
- receive_data_.now_run_mode = ENERGY_BUFF;
- break;
- case SENTRY_STRIKE_MODE:
- receive_data_.now_run_mode = SENTRY_STRIKE_MODE;
- break;
- case TOP_MODE:
- receive_data_.now_run_mode = TOP_MODE;
- break;
- case RECORD_MODE:
- receive_data_.now_run_mode = RECORD_MODE;
- break;
- case PLANE_MODE:
- receive_data_.now_run_mode = PLANE_MODE;
- break;
- case SENTINEL_AUTONOMOUS_MODE:
- receive_data_.now_run_mode = SENTINEL_AUTONOMOUS_MODE;
- break;
- case RADAR_MODE:
- receive_data_.now_run_mode = RADAR_MODE;
- break;
- case CAMERA_CALIBRATION:
- receive_data_.now_run_mode = CAMERA_CALIBRATION;
- break;
- default:
- receive_data_.now_run_mode = AUTO_AIM;
- break;
- }
+ }
+ fmt::print("[rec info] GET\n");
+ switch (receive_buff_[2])
+ {
+ case AUTO_AIM:
+ receive_data_.now_run_mode = AUTO_AIM;
+ break;
+ case ENERGY_BUFF:
+ receive_data_.now_run_mode = ENERGY_BUFF;
+ break;
+ case SENTRY_STRIKE_MODE:
+ receive_data_.now_run_mode = SENTRY_STRIKE_MODE;
+ break;
+ case TOP_MODE:
+ receive_data_.now_run_mode = TOP_MODE;
+ break;
+ case RECORD_MODE:
+ receive_data_.now_run_mode = RECORD_MODE;
+ break;
+ case PLANE_MODE:
+ receive_data_.now_run_mode = PLANE_MODE;
+ break;
+ case SENTINEL_AUTONOMOUS_MODE:
+ receive_data_.now_run_mode = SENTINEL_AUTONOMOUS_MODE;
+ break;
+ case RADAR_MODE:
+ receive_data_.now_run_mode = RADAR_MODE;
+ break;
+ case CAMERA_CALIBRATION:
+ receive_data_.now_run_mode = CAMERA_CALIBRATION;
+ break;
+ default:
+ receive_data_.now_run_mode = AUTO_AIM;
+ break;
+ }
- switch (receive_buff_[3]) {
+ switch (receive_buff_[3])
+ {
case HERO:
receive_data_.my_robot_id = HERO;
break;
@@ -381,45 +430,46 @@ void SerialPort::updateReceiveInformation() {
default:
receive_data_.my_robot_id = INFANTRY;
break;
- }
+ }
- for (size_t i = 0; i != sizeof(receive_data_.raw_yaw_angle.arr_yaw); ++i) {
- receive_data_.raw_yaw_angle.arr_yaw[i] = receive_buff_[4 + i];
- }
- //fmt::print("[info] yaw:{}\n",receive_data_.raw_yaw_angle.yaw);
- receive_data_.yaw = receive_data_.raw_yaw_angle.yaw / 100.0;
- fmt::print("[rec info] yaw:{}\n",receive_data_.yaw);
+ for (size_t i = 0; i != sizeof(receive_data_.raw_yaw_angle.arr_yaw); ++i)
+ {
+ receive_data_.raw_yaw_angle.arr_yaw[i] = receive_buff_[4 + i];
+ }
+ // fmt::print("[info] yaw:{}\n",receive_data_.raw_yaw_angle.yaw);
+ receive_data_.yaw = receive_data_.raw_yaw_angle.yaw / 100.0;
+ fmt::print("[rec info] yaw:{}\n", receive_data_.yaw);
- for (size_t i = 0; i != sizeof(receive_data_.raw_pitch_angle.pitch); ++i) {
- receive_data_.raw_pitch_angle.arr_pitch[i] = receive_buff_[6 + i];
- }
- //fmt::print("[info] pitch:{}\n",receive_data_.raw_pitch_angle.pitch);
- receive_data_.pitch = receive_data_.raw_pitch_angle.pitch / 100.0;
- fmt::print("[rec info] pitch:{}\n",receive_data_.pitch);
+ for (size_t i = 0; i != sizeof(receive_data_.raw_pitch_angle.pitch); ++i)
+ {
+ receive_data_.raw_pitch_angle.arr_pitch[i] = receive_buff_[6 + i];
+ }
+ // fmt::print("[info] pitch:{}\n",receive_data_.raw_pitch_angle.pitch);
+ receive_data_.pitch = receive_data_.raw_pitch_angle.pitch / 100.0;
+ fmt::print("[rec info] pitch:{}\n", receive_data_.pitch);
- for (size_t i = 0; i != sizeof(receive_data_.raw_yaw_velocity.veloctiy); ++i) {
- receive_data_.raw_yaw_velocity.arr_yaw_velocity[i] = receive_buff_[8 + i];
- }
- fmt::print("[rec info] yaw_velocity:{}\n",receive_data_.raw_yaw_velocity.veloctiy);
- receive_data_.yaw_velocity = receive_data_.raw_yaw_velocity.veloctiy / 100.0;
- //fmt::print("[info] yaw_velocity:{}\n",receive_data_.raw_yaw_velocity.veloctiy);
+ for (size_t i = 0; i != sizeof(receive_data_.raw_yaw_velocity.veloctiy); ++i)
+ {
+ receive_data_.raw_yaw_velocity.arr_yaw_velocity[i] = receive_buff_[8 + i];
+ }
+ fmt::print("[rec info] yaw_velocity:{}\n", receive_data_.raw_yaw_velocity.veloctiy);
+ receive_data_.yaw_velocity = receive_data_.raw_yaw_velocity.veloctiy / 100.0;
+ // fmt::print("[info] yaw_velocity:{}\n",receive_data_.raw_yaw_velocity.veloctiy);
- for (size_t i = 0; i != sizeof(receive_data_.raw_pitch_velocity.veloctiy); ++i) {
- receive_data_.raw_pitch_velocity.arr_pitch_velocity[i] = receive_buff_[10 + i];
- }
- fmt::print("[rec info] pitch_velocity:{}\n",receive_data_.raw_pitch_velocity.veloctiy);
- receive_data_.pitch_velocity = receive_data_.raw_pitch_velocity.veloctiy / 100.0;
- //receive_data_.pitch_velocity = 20;
- //fmt::print("[rec info] pitch_velocity:{}\n",receive_data_.pitch_velocity);
- //fmt::print("[info] pitch_velocity:{}\n",receive_data_.raw_pitch_velocity.veloctiy);
-
- receive_data_.raw_bullet_velocity.arr_veloctiy = receive_buff_[12];
- if(receive_data_.raw_bullet_velocity.veloctiy==0)
- {
- receive_data_.raw_bullet_velocity.veloctiy=200;
+ for (size_t i = 0; i != sizeof(receive_data_.raw_pitch_velocity.veloctiy); ++i)
+ {
+ receive_data_.raw_pitch_velocity.arr_pitch_velocity[i] = receive_buff_[10 + i];
+ }
+ fmt::print("[rec info] pitch_velocity:{}\n", receive_data_.raw_pitch_velocity.veloctiy);
+ receive_data_.pitch_velocity = receive_data_.raw_pitch_velocity.veloctiy / 100.0;
+ // fmt::print("[info] pitch_velocity:{}\n",receive_data_.raw_pitch_velocity.veloctiy);
+
+ receive_data_.raw_bullet_velocity.arr_veloctiy = receive_buff_[12];
+ if (receive_data_.raw_bullet_velocity.veloctiy == 0)
+ {
+ receive_data_.raw_bullet_velocity.veloctiy = 200;
+ }
+ fmt::print("[rec info] bullet_velocity:{}\n", receive_data_.raw_bullet_velocity.arr_veloctiy);
+ receive_data_.bullet_velocity = receive_data_.raw_bullet_velocity.veloctiy / 10.0;
}
- fmt::print("[rec info] bullet_velocity:{}\n",receive_data_.raw_bullet_velocity.arr_veloctiy);
- receive_data_.bullet_velocity = receive_data_.raw_bullet_velocity.veloctiy / 10.0;
-
-}
-} // namespace uart
+} // namespace uart
diff --git a/devices/serial/uart_serial.hpp b/devices/serial/uart_serial.hpp
index 7ed3e34..4d7ff26 100755
--- a/devices/serial/uart_serial.hpp
+++ b/devices/serial/uart_serial.hpp
@@ -3,9 +3,9 @@
* @author dgsyrc (yrcminecraft@foxmail.com)
* @brief 串口通讯
* @date 2023-01-18
- *
+ *
* @copyright Copyright (c) 2023 Sirius
- *
+ *
*/
#pragma once
@@ -21,421 +21,438 @@
#include "module/filter/basic_kalman.hpp"
#include
-
-
-namespace uart {
-
-auto idntifier_green = fmt::format(fg(fmt::color::green) | fmt::emphasis::bold, "uart_serial");
-auto idntifier_red = fmt::format(fg(fmt::color::red) | fmt::emphasis::bold, "uart_serial");
-
-basic_kalman::firstKalman kal;
-
-enum BufferLength {
- // 接收数据字节数
- REC_INFO_LENGTH = 14,
-
- // 计算CRC校验码的发送数据的长度
- CRC_BUFF_LENGTH = 13,
-
- // The send length of the array after append CRC auth code
- WRITE_BUFF_LENGTH = 15
-};
-
-
-// 我方颜色
-enum Color {
- ALL,
- RED,
- BLUE
-};
-
-// 当前模式
-enum RunMode {
- // 默认模式（基础自瞄）
- DEFAULT_MODE,
- // 基础自瞄模式
- AUTO_AIM,
- // 能量机关模式
- ENERGY_BUFF,
- // 击打哨兵模式
- SENTRY_STRIKE_MODE,
- // 反小陀螺模式
- TOP_MODE,
- // 内录模式
- RECORD_MODE,
- // 无人机模式
- PLANE_MODE,
- // 哨兵模式
- SENTINEL_AUTONOMOUS_MODE,
- // 雷达模式
- RADAR_MODE,
- // 相机标定模式
- CAMERA_CALIBRATION
-};
-
-// 机器人id
-enum RobotID {
- HERO, // 英雄
- UAV, // 无人机
- ENGINEERING, // 工程
- INFANTRY, // 步兵
- SENTRY // 哨兵
-};
-
-// 串口信息
-struct Serial_Config {
- std::string preferred_device = "/dev/ttyACM0";
- int set_baudrate = 0;
- int show_serial_information = 0;
-};
-
-// 接收的数据结构 头帧0x53 尾帧0x45
-struct Receive_Data {
- int my_color; // 01 颜色
- int now_run_mode; // 02 模式
- int my_robot_id; // 03 机器人ID
-
- float yaw; // 0405 yaw轴
- float pitch; // 0607 pitch轴
-
- float yaw_velocity; // 0809 yaw轴加速度
- float pitch_velocity; // 1011 pitch轴加速度
-
- float bullet_velocity; // 12 子弹速度
-
- bool old_flag;
-
- union Bullet_Velocity_Info {
- short veloctiy; // 子弹速度（未解码）
- uint8_t arr_veloctiy = 0;
- } raw_bullet_velocity; // 子弹速度（未解码）
-
- union Yaw_Angle_Info {
- short yaw; // yaw轴（未解码）
- uint8_t arr_yaw[2] = {0};
- } raw_yaw_angle; // yaw轴（未解码）
-
- union Yaw_Velocity_Info {
- short veloctiy; // yaw轴加速度（未解码）
- uint8_t arr_yaw_velocity[2] = {0};
- } raw_yaw_velocity; // yaw轴加速度（未解码）
-
- union Pitch_Angle_Info {
- short pitch; // pitch轴（未解码）
- uint8_t arr_pitch[2] = {0};
- } raw_pitch_angle; // pitch轴（未解码）
-
- union Pitch_Velocity_Info {
- short veloctiy; // pitch轴加速度（未解码）
- uint8_t arr_pitch_velocity[2] = {0};
- } raw_pitch_velocity; // pitch轴加速度（未解码）
-
- Receive_Data() {
- my_color = ALL;
- now_run_mode = AUTO_AIM;
- my_robot_id = INFANTRY;
- raw_bullet_velocity.veloctiy = 0;
- raw_yaw_angle.yaw = 0;
- raw_yaw_velocity.veloctiy = 0;
- raw_pitch_angle.pitch = 0;
- raw_pitch_velocity.veloctiy = 0;
- yaw = 0.f; // yaw轴
- pitch = 0.f; // pitch轴
- yaw_velocity = 0.f; // yaw轴加速度
- pitch_velocity = 0.f; // pitch轴加速度
- bullet_velocity = 0.f; // 子弹速度
- old_flag = false;
- }
-};
-
-// 发送的数据结构
-struct Write_Data {
- int data_type; // 01 识别信息
- int is_shooting; // 02 射击信息 1发射 0不发射
-
- float yaw; // 0304 yaw轴数据
-
- float pitch; // 0506 pitch轴数据
-
- struct node {
- int x; // 0708 预测坐标x轴
- int y; // 0910 预测坐标y轴
- } cord;
-
- int depth; // 1112 深度
-
- Write_Data() {
- data_type = 0; // 01
- is_shooting = 0; // 02
- yaw = 0.f; // 0304
- pitch = 0.f; // 0506
- cord.x = 0; // 0708
- cord.y = 0; // 0910
- depth = 0; // 1112
- }
-};
-
-class SerialPort {
- public:
- SerialPort() = default;
- explicit SerialPort(std::string _serial_config);
-
- ~SerialPort();
- /**
- * @brief 返回接受数据的结构体
- *
- * @return Receive_Data
- */
- inline Receive_Data returnReceive() { return receive_data_; }
- /**
- * @brief 返回子弹速度
- *
- * @return float
- */
- inline float returnReceiveBulletVelocity() { return receive_data_.bullet_velocity; }
- /**
- * @brief 返回机器人 ID
- *
- * @return int
- */
- inline int returnReceiveRobotId() { return receive_data_.my_robot_id; }
- /**
- * @brief 返回自身颜色
- *
- * @return int
- */
- inline int returnReceiceColor() { return receive_data_.my_color; }
- /**
- * @brief 返回模式选择
- *
- * @return int
- */
- inline int returnReceiveMode() { return /*receive_data_.now_run_mode;*/1; }
- /**
- * @brief 返回陀螺仪 Pitch 轴数据
- *
- * @return float
- */
- inline float returnReceivePitch() { return receive_data_.pitch; }
- /**
- * @brief 返回陀螺仪 Yaw 轴数据
- *
- * @return float
- */
- inline float returnReceiveYaw() { return receive_data_.yaw; }
- /**
- * @brief 返回陀螺仪Yaw轴速度数据
- *
- * @return float
- */
- inline float returnReceiveYawVelocity() { return receive_data_.yaw_velocity; }
- /**
- * @brief 返回陀螺仪Pitch轴速度数据
- *
- * @return float
- */
- inline float returnReceivePitchVelocity() { return receive_data_.pitch_velocity;}
-
- inline bool returnOldFlag() {
- return receive_data_.old_flag;
- }
-
- /**
- * @brief 返回高八位数据
- * @brief 发送数据
- * @param _yaw yaw 符号
- * @param yaw yaw 绝对值
- * @param _pitch pitch 符号
- * @param pitch pitch 绝对值
- * @param depth 深度
- * @param data_type 是否发现目标
- * @param is_shooting 开火命令
- *
- * @param Byte
- * @return unsigned char
- */
- inline unsigned char returnHighBit(const int& Byte) {
- exchangebyte_ = (Byte >> 8) & 0xff;
-
- return exchangebyte_;
- }
- /**
- * @brief 返回低八位数据
- *
- * @param Byte
- * @return unsigned char
- */
- inline unsigned char returnLowBit(const int& Byte) {
- exchangebyte_ = Byte & 0xff;
-
- return exchangebyte_;
- }
- /**
- * @brief 合并数据
- *
- * @param highbit 高八位数据
- * @param lowbit 低八位数据
- * @return int16_t 合并后数据
- */
- inline int16_t mergeIntoBytes(const unsigned char& highbit,
- const unsigned char& lowbit) {
- exchangebit_ = (highbit << 8) | lowbit;
-
- return exchangebit_;
- }
-
- /**
- * @brief 写入发送数据
- * @param data_type 是否发现目标
- * @param is_shooting 开火命令
- * @param yaw yaw轴
- * @param pitch pitch轴
- * @param cord 预测坐标
- * @param depth 深度信息
- */
- void writeData(const int& data_type,
- const int& is_shooting,
- const int16_t& yaw,
- const int16_t& pitch,
- const Write_Data::node& cord,
- const int16_t& depth);
-
- void writeData();
- /**
- * @brief 发送数据
- *
- * @param _write_data 需要发送的 Write_Data 结构体
- */
- void writeData(const Write_Data& _write_data);
-
- /**
- * @brief 发送数据
- *
- * @param _data_type 是否发现目标（装甲板数量）
- * @param _is_shooting 开火命令
- * @param _yaw yaw 数据
- * @param _pitch pitch 数据
- * @param _cord 预测坐标
- * @param _depth 深度
- */
- void updataWriteData(const int _data_type, const int _is_shooting,
- const float _yaw, const float _pitch,
- const Write_Data::node _cord, const int _depth);
-
- /**
- * @brief 数据转换为结构体
- *
- * @param _yaw yaw 数据
- * @param _pitch pitch 数据
- * @param _depth 深度
- * @param _data_type 是否发现目标
- * @param _is_shooting 开火命令
- * @return Write_Data 返回写入数据结构体
- */
- Write_Data gainWriteData(const int _data_type, const int _is_shooting,
- const float _yaw, const float _pitch,
- const Write_Data::node _cord, const int _depth);
-
- /**
- * @brief 接收数据
- */
- void receiveData();
-
- /**
- * @brief 接收数据是否正常
- * @return true 不正常
- * @return false 正常
- */
- bool isEmpty();
-
- /**
- * @brief 更新数据信息
- */
- void updateReceiveInformation();
-
- private:
- Serial_Config serial_config_;
- Receive_Data receive_data_;
- Receive_Data last_receive_data_;
- Write_Data write_data_;
-
- int fd;
- int transform_arr_[4];
- unsigned char write_buff_[WRITE_BUFF_LENGTH];
- unsigned char crc_buff_[CRC_BUFF_LENGTH];
- unsigned char receive_buff_[REC_INFO_LENGTH];
- unsigned char receive_buff_temp_[REC_INFO_LENGTH * 2];
- unsigned char exchangebyte_;
-
- int16_t yaw_reduction_;
- int16_t pitch_reduction_;
- int16_t depth_reduction_;
- int16_t cord_reduction_x;
- int16_t cord_reduction_y;
-
- int16_t angle_reduction_;
- int16_t acceleration_reduction_;
-
- int16_t exchangebit_;
-
- ssize_t read_message_;
- ssize_t write_message_;
-
- bool get_flag;
-
- inline uint8_t checksumCRC(unsigned char* buf, uint16_t len);
-
- int pre_mode = 0;
-
-/**
- * @brief Get the Data For CRC object
- * @param data_type 是否发现目标
- * @param is_shooting 开火命令
- * @param _yaw yaw 符号
- * @param yaw yaw 绝对值
- * @param _pitch pitch 符号
- * @param pitch pitch 绝对值
- * @param depth 深度
- */
- void getDataForCRC(const int& data_type, const int& is_shooting,
- const int16_t& yaw, const int16_t& pitch,
- const Write_Data::node& cord, const int16_t& depth);
-
- /**
- * @brief 获取发送信息
- * @param data_type 是否发现目标
- * @param is_shooting 开火命令
- * @param yaw yaw轴
- * @param pitch pitch轴
- * @param cord 预测坐标
- * @param depth 深度
- * @param CRC CRC 校验码
- */
- void getDataForSend(const int& data_type, const int& is_shooting,
- const int16_t& yaw, const int16_t& pitch,
- const Write_Data::node& cord, const int16_t& depth,
- const uint8_t& CRC);
-};
-
-static constexpr unsigned char CRC8_Table[] = {
- 0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31,
- 65, 157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96,
- 130, 220, 35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128,
- 222, 60, 98, 190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158,
- 29, 67, 161, 255, 70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56,
- 102, 229, 187, 89, 7, 219, 133, 103, 57, 186, 228, 6, 88, 25, 71,
- 165, 251, 120, 38, 196, 154, 101, 59, 217, 135, 4, 90, 184, 230, 167,
- 249, 27, 69, 198, 152, 122, 36, 248, 166, 68, 26, 153, 199, 37, 123,
- 58, 100, 134, 216, 91, 5, 231, 185, 140, 210, 48, 110, 237, 179, 81,
- 15, 78, 16, 242, 172, 47, 113, 147, 205, 17, 79, 173, 243, 112, 46,
- 204, 146, 211, 141, 111, 49, 178, 236, 14, 80, 175, 241, 19, 77, 206,
- 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238, 50, 108, 142, 208,
- 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115, 202, 148, 118,
- 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139, 87, 9,
- 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22, 233,
- 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
- 116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107,
- 53
-};
-
-} // namespace uart
+namespace uart
+{
+
+ auto idntifier_green = fmt::format(fg(fmt::color::green) | fmt::emphasis::bold, "uart_serial");
+ auto idntifier_red = fmt::format(fg(fmt::color::red) | fmt::emphasis::bold, "uart_serial");
+
+ basic_kalman::firstKalman kal;
+
+ enum BufferLength
+ {
+ // 接收数据字节数
+ REC_INFO_LENGTH = 14,
+
+ // 计算CRC校验码的发送数据的长度
+ CRC_BUFF_LENGTH = 13,
+
+ // The send length of the array after append CRC auth code
+ WRITE_BUFF_LENGTH = 15
+ };
+
+ // 我方颜色
+ enum Color
+ {
+ ALL,
+ RED,
+ BLUE
+ };
+
+ // 当前模式
+ enum RunMode
+ {
+ // 默认模式（基础自瞄）
+ DEFAULT_MODE,
+ // 基础自瞄模式
+ AUTO_AIM,
+ // 能量机关模式
+ ENERGY_BUFF,
+ // 击打哨兵模式（未启用）
+ SENTRY_STRIKE_MODE,
+ // 反小陀螺模式（未启用）
+ TOP_MODE,
+ // 内录模式（弃用）
+ RECORD_MODE,
+ // 无人机模式（未启用）
+ PLANE_MODE,
+ // 哨兵模式（未启用）
+ SENTINEL_AUTONOMOUS_MODE,
+ // 雷达模式（未启用）
+ RADAR_MODE,
+ // 相机标定模式
+ CAMERA_CALIBRATION
+ };
+
+ // 机器人id
+ enum RobotID
+ {
+ HERO, // 英雄
+ UAV, // 无人机
+ ENGINEERING, // 工程
+ INFANTRY, // 步兵
+ SENTRY // 哨兵
+ };
+
+ // 串口信息
+ struct Serial_Config
+ {
+ std::string preferred_device = "/dev/ttyACM0";
+ int set_baudrate = 0;
+ int show_serial_information = 0;
+ };
+
+ // 接收的数据结构 头帧0x53 尾帧0x45
+ struct Receive_Data
+ {
+ int my_color; // 01 颜色
+ int now_run_mode; // 02 模式
+ int my_robot_id; // 03 机器人ID
+
+ float yaw; // 0405 yaw轴
+ float pitch; // 0607 pitch轴
+
+ float yaw_velocity; // 0809 yaw轴加速度
+ float pitch_velocity; // 1011 pitch轴加速度
+
+ float bullet_velocity; // 12 子弹速度
+
+ bool old_flag;
+
+ union Bullet_Velocity_Info
+ {
+ short veloctiy; // 子弹速度（未解码）
+ uint8_t arr_veloctiy = 0;
+ } raw_bullet_velocity; // 子弹速度（未解码）
+
+ union Yaw_Angle_Info
+ {
+ short yaw; // yaw轴（未解码）
+ uint8_t arr_yaw[2] = {0};
+ } raw_yaw_angle; // yaw轴（未解码）
+
+ union Yaw_Velocity_Info
+ {
+ short veloctiy; // yaw轴加速度（未解码）
+ uint8_t arr_yaw_velocity[2] = {0};
+ } raw_yaw_velocity; // yaw轴加速度（未解码）
+
+ union Pitch_Angle_Info
+ {
+ short pitch; // pitch轴（未解码）
+ uint8_t arr_pitch[2] = {0};
+ } raw_pitch_angle; // pitch轴（未解码）
+
+ union Pitch_Velocity_Info
+ {
+ short veloctiy; // pitch轴加速度（未解码）
+ uint8_t arr_pitch_velocity[2] = {0};
+ } raw_pitch_velocity; // pitch轴加速度（未解码）
+
+ Receive_Data()
+ {
+ my_color = ALL;
+ now_run_mode = AUTO_AIM;
+ my_robot_id = INFANTRY;
+ raw_bullet_velocity.veloctiy = 0;
+ raw_yaw_angle.yaw = 0;
+ raw_yaw_velocity.veloctiy = 0;
+ raw_pitch_angle.pitch = 0;
+ raw_pitch_velocity.veloctiy = 0;
+ yaw = 0.f; // yaw轴
+ pitch = 0.f; // pitch轴
+ yaw_velocity = 0.f; // yaw轴加速度
+ pitch_velocity = 0.f; // pitch轴加速度
+ bullet_velocity = 0.f; // 子弹速度
+ old_flag = false;
+ }
+ };
+
+ // 发送的数据结构
+ struct Write_Data
+ {
+ int data_type; // 01 识别信息
+ int is_shooting; // 02 射击信息 1发射 0不发射
+
+ float yaw; // 0304 yaw轴数据
+
+ float pitch; // 0506 pitch轴数据
+
+ struct node
+ {
+ int x; // 0708 预测坐标x轴
+ int y; // 0910 预测坐标y轴
+ } cord;
+
+ int depth; // 1112 深度
+
+ Write_Data()
+ {
+ data_type = 0; // 01
+ is_shooting = 0; // 02
+ yaw = 0.f; // 0304
+ pitch = 0.f; // 0506
+ cord.x = 0; // 0708
+ cord.y = 0; // 0910
+ depth = 0; // 1112
+ }
+ };
+
+ class SerialPort
+ {
+ public:
+ SerialPort() = default;
+ explicit SerialPort(std::string _serial_config);
+
+ ~SerialPort();
+ /**
+ * @brief 返回接受数据的结构体
+ *
+ * @return Receive_Data
+ */
+ inline Receive_Data returnReceive() { return receive_data_; }
+ /**
+ * @brief 返回子弹速度
+ *
+ * @return float
+ */
+ inline float returnReceiveBulletVelocity() { return receive_data_.bullet_velocity; }
+ /**
+ * @brief 返回机器人 ID
+ *
+ * @return int
+ */
+ inline int returnReceiveRobotId() { return receive_data_.my_robot_id; }
+ /**
+ * @brief 返回自身颜色
+ *
+ * @return int
+ */
+ inline int returnReceiceColor() { return receive_data_.my_color; }
+ /**
+ * @brief 返回模式选择
+ *
+ * @return int
+ */
+ inline int returnReceiveMode() { return /*receive_data_.now_run_mode;*/ 1; }
+ /**
+ * @brief 返回陀螺仪 Pitch 轴数据
+ *
+ * @return float
+ */
+ inline float returnReceivePitch() { return receive_data_.pitch; }
+ /**
+ * @brief 返回陀螺仪 Yaw 轴数据
+ *
+ * @return float
+ */
+ inline float returnReceiveYaw() { return receive_data_.yaw; }
+ /**
+ * @brief 返回陀螺仪Yaw轴速度数据
+ *
+ * @return float
+ */
+ inline float returnReceiveYawVelocity() { return receive_data_.yaw_velocity; }
+ /**
+ * @brief 返回陀螺仪Pitch轴速度数据
+ *
+ * @return float
+ */
+ inline float returnReceivePitchVelocity() { return receive_data_.pitch_velocity; }
+
+ inline bool returnOldFlag()
+ {
+ return receive_data_.old_flag;
+ }
+
+ /**
+ * @brief 返回高八位数据
+ * @brief 发送数据
+ * @param _yaw yaw 符号
+ * @param yaw yaw 绝对值
+ * @param _pitch pitch 符号
+ * @param pitch pitch 绝对值
+ * @param depth 深度
+ * @param data_type 是否发现目标
+ * @param is_shooting 开火命令
+ *
+ * @param Byte
+ * @return unsigned char
+ */
+ inline unsigned char returnHighBit(const int &Byte)
+ {
+ exchangebyte_ = (Byte >> 8) & 0xff;
+
+ return exchangebyte_;
+ }
+ /**
+ * @brief 返回低八位数据
+ *
+ * @param Byte
+ * @return unsigned char
+ */
+ inline unsigned char returnLowBit(const int &Byte)
+ {
+ exchangebyte_ = Byte & 0xff;
+
+ return exchangebyte_;
+ }
+ /**
+ * @brief 合并数据
+ *
+ * @param highbit 高八位数据
+ * @param lowbit 低八位数据
+ * @return int16_t 合并后数据
+ */
+ inline int16_t mergeIntoBytes(const unsigned char &highbit,
+ const unsigned char &lowbit)
+ {
+ exchangebit_ = (highbit << 8) | lowbit;
+
+ return exchangebit_;
+ }
+
+ /**
+ * @brief 写入发送数据
+ * @param data_type 是否发现目标
+ * @param is_shooting 开火命令
+ * @param yaw yaw轴
+ * @param pitch pitch轴
+ * @param cord 预测坐标
+ * @param depth 深度信息
+ */
+ void writeData(const int &data_type,
+ const int &is_shooting,
+ const int16_t &yaw,
+ const int16_t &pitch,
+ const Write_Data::node &cord,
+ const int16_t &depth);
+
+ void writeData();
+ /**
+ * @brief 发送数据
+ *
+ * @param _write_data 需要发送的 Write_Data 结构体
+ */
+ void writeData(const Write_Data &_write_data);
+
+ /**
+ * @brief 发送数据
+ *
+ * @param _data_type 是否发现目标（装甲板数量）
+ * @param _is_shooting 开火命令
+ * @param _yaw yaw 数据
+ * @param _pitch pitch 数据
+ * @param _cord 预测坐标
+ * @param _depth 深度
+ */
+ void updataWriteData(const int _data_type, const int _is_shooting,
+ const float _yaw, const float _pitch,
+ const Write_Data::node _cord, const int _depth);
+
+ /**
+ * @brief 数据转换为结构体
+ *
+ * @param _yaw yaw 数据
+ * @param _pitch pitch 数据
+ * @param _depth 深度
+ * @param _data_type 是否发现目标
+ * @param _is_shooting 开火命令
+ * @return Write_Data 返回写入数据结构体
+ */
+ Write_Data gainWriteData(const int _data_type, const int _is_shooting,
+ const float _yaw, const float _pitch,
+ const Write_Data::node _cord, const int _depth);
+
+ /**
+ * @brief 接收数据
+ */
+ void receiveData();
+
+ /**
+ * @brief 接收数据是否正常
+ * @return true 不正常
+ * @return false 正常
+ */
+ bool isEmpty();
+
+ /**
+ * @brief 更新数据信息
+ */
+ void updateReceiveInformation();
+
+ private:
+ Serial_Config serial_config_;
+ Receive_Data receive_data_;
+ Receive_Data last_receive_data_;
+ Write_Data write_data_;
+
+ int fd;
+ int transform_arr_[4];
+ unsigned char write_buff_[WRITE_BUFF_LENGTH];
+ unsigned char crc_buff_[CRC_BUFF_LENGTH];
+ unsigned char receive_buff_[REC_INFO_LENGTH];
+ unsigned char receive_buff_temp_[REC_INFO_LENGTH * 2];
+ unsigned char exchangebyte_;
+
+ int16_t yaw_reduction_;
+ int16_t pitch_reduction_;
+ int16_t depth_reduction_;
+ int16_t cord_reduction_x;
+ int16_t cord_reduction_y;
+
+ int16_t angle_reduction_;
+ int16_t acceleration_reduction_;
+
+ int16_t exchangebit_;
+
+ ssize_t read_message_;
+ ssize_t write_message_;
+
+ bool get_flag;
+
+ inline uint8_t checksumCRC(unsigned char *buf, uint16_t len);
+
+ int pre_mode = 0;
+
+ /**
+ * @brief Get the Data For CRC object
+ * @param data_type 是否发现目标
+ * @param is_shooting 开火命令
+ * @param _yaw yaw 符号
+ * @param yaw yaw 绝对值
+ * @param _pitch pitch 符号
+ * @param pitch pitch 绝对值
+ * @param depth 深度
+ */
+ void getDataForCRC(const int &data_type, const int &is_shooting,
+ const int16_t &yaw, const int16_t &pitch,
+ const Write_Data::node &cord, const int16_t &depth);
+
+ /**
+ * @brief 获取发送信息
+ * @param data_type 是否发现目标
+ * @param is_shooting 开火命令
+ * @param yaw yaw轴
+ * @param pitch pitch轴
+ * @param cord 预测坐标
+ * @param depth 深度
+ * @param CRC CRC 校验码
+ */
+ void getDataForSend(const int &data_type, const int &is_shooting,
+ const int16_t &yaw, const int16_t &pitch,
+ const Write_Data::node &cord, const int16_t &depth,
+ const uint8_t &CRC);
+ };
+
+ static constexpr unsigned char CRC8_Table[] = {
+ 0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31,
+ 65, 157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96,
+ 130, 220, 35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128,
+ 222, 60, 98, 190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158,
+ 29, 67, 161, 255, 70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56,
+ 102, 229, 187, 89, 7, 219, 133, 103, 57, 186, 228, 6, 88, 25, 71,
+ 165, 251, 120, 38, 196, 154, 101, 59, 217, 135, 4, 90, 184, 230, 167,
+ 249, 27, 69, 198, 152, 122, 36, 248, 166, 68, 26, 153, 199, 37, 123,
+ 58, 100, 134, 216, 91, 5, 231, 185, 140, 210, 48, 110, 237, 179, 81,
+ 15, 78, 16, 242, 172, 47, 113, 147, 205, 17, 79, 173, 243, 112, 46,
+ 204, 146, 211, 141, 111, 49, 178, 236, 14, 80, 175, 241, 19, 77, 206,
+ 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238, 50, 108, 142, 208,
+ 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115, 202, 148, 118,
+ 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139, 87, 9,
+ 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22, 233,
+ 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
+ 116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107,
+ 53};
+
+} // namespace uart
diff --git a/module/angle_solve/abstract_pnp.hpp b/module/angle_solve/abstract_pnp.hpp
index a5affd1..a1269fc 100644
--- a/module/angle_solve/abstract_pnp.hpp
+++ b/module/angle_solve/abstract_pnp.hpp
@@ -18,97 +18,103 @@
#include
#include
-namespace abstract_pnp {
-
-struct PnP_Config {
- int company = 1;
-
- double ptz_camera_x = 0.0;
- double ptz_camera_y = 0.0;
- double ptz_camera_z = 0.0;
-
- float barrel_ptz_offset_x = 0.0;
- float barrel_ptz_offset_y = 0.0;
- float offset_armor_pitch = 0.0;
- float offset_armor_yaw = 0.0;
- // 各种装甲板模型
- int small_armor_height = 60, small_armor_width = 140;
- int big_armor_width = 245, big_armor_height = 60;
- int light_size_width = 10, light_size_height = 55;
- int buff_armor_width = 250, buff_armor_height = 65;
-};
-
-class PnP {
- public:
- PnP_Config pnp_config_;
-
- PnP() {
- reference_Obj_.emplace_back(cv::Point3f(0.0, 0.0, 0.0));
- reference_Obj_.emplace_back(cv::Point3f(100, 0.0, 0.0));
- reference_Obj_.emplace_back(cv::Point3f(0.0, 100, 0.0));
- reference_Obj_.emplace_back(cv::Point3f(0.0, 0.0, 100));
-
- static double theta = 0;
- static double r_data[] = {1, 0, 0,
- 0, cos(theta), sin(theta),
- 0, -sin(theta), cos(theta)};
- static double t_data[] = {static_cast(pnp_config_.ptz_camera_x),
- static_cast(pnp_config_.ptz_camera_y),
- static_cast(pnp_config_.ptz_camera_z)};
-
- r_camera_ptz = cv::Mat(3, 3, CV_64FC1, r_data);
- t_camera_ptz = cv::Mat(3, 1, CV_64FC1, t_data);
-
- big_object_3d_.emplace_back(
- cv::Point3f(-pnp_config_.big_armor_width * 0.5,
- -pnp_config_.big_armor_height * 0.5, 0));
- big_object_3d_.emplace_back(
- cv::Point3f(pnp_config_.big_armor_width * 0.5,
- -pnp_config_.big_armor_height * 0.5, 0));
- big_object_3d_.emplace_back(
- cv::Point3f(pnp_config_.big_armor_width * 0.5,
- pnp_config_.big_armor_height * 0.5, 0));
- big_object_3d_.emplace_back(
- cv::Point3f(-pnp_config_.big_armor_width * 0.5,
- pnp_config_.big_armor_height * 0.5, 0));
-
- small_object_3d_.emplace_back(
- cv::Point3f(-pnp_config_.small_armor_width * 0.5,
- -pnp_config_.small_armor_height * 0.5, 0));
- small_object_3d_.emplace_back(
- cv::Point3f(pnp_config_.small_armor_width * 0.5,
- -pnp_config_.small_armor_height * 0.5, 0));
- small_object_3d_.emplace_back(
- cv::Point3f(pnp_config_.small_armor_width * 0.5,
- pnp_config_.small_armor_height * 0.5, 0));
- small_object_3d_.emplace_back(
- cv::Point3f(-pnp_config_.small_armor_width * 0.5,
- pnp_config_.small_armor_height * 0.5, 0));
-
- buff_object_3d_.emplace_back(
- cv::Point3f(-pnp_config_.buff_armor_width * 0.5,
- -pnp_config_.buff_armor_height * 0.5, 0));
- buff_object_3d_.emplace_back(
- cv::Point3f(pnp_config_.buff_armor_width * 0.5,
- -pnp_config_.buff_armor_height * 0.5, 0));
- buff_object_3d_.emplace_back(
- cv::Point3f(pnp_config_.buff_armor_width * 0.5,
- pnp_config_.buff_armor_height * 0.5, 0));
- buff_object_3d_.emplace_back(
- cv::Point3f(-pnp_config_.buff_armor_width * 0.5,
- pnp_config_.buff_armor_height * 0.5, 0));
- }
-
- ~PnP() = default;
- /**
- * @brief 初始化目标 3d 点
- *
- * @param _armor_type 装甲板类型
- * @return std::vector 返回目标 3d 点
- */
- std::vector initialize3DPoints(int _armor_type) {
- // 选择装甲板类型
- switch (_armor_type) {
+namespace abstract_pnp
+{
+
+ struct PnP_Config
+ {
+ int company = 1;
+
+ double ptz_camera_x = 0.0;
+ double ptz_camera_y = 0.0;
+ double ptz_camera_z = 0.0;
+
+ float barrel_ptz_offset_x = 0.0;
+ float barrel_ptz_offset_y = 0.0;
+ float offset_armor_pitch = 0.0;
+ float offset_armor_yaw = 0.0;
+ // 各种装甲板模型
+ int small_armor_height = 60, small_armor_width = 140;
+ int big_armor_width = 245, big_armor_height = 60;
+ int light_size_width = 10, light_size_height = 55;
+ int buff_armor_width = 250, buff_armor_height = 65;
+ };
+
+ class PnP
+ {
+ public:
+ PnP_Config pnp_config_;
+
+ PnP()
+ {
+ reference_Obj_.emplace_back(cv::Point3f(0.0, 0.0, 0.0));
+ reference_Obj_.emplace_back(cv::Point3f(100, 0.0, 0.0));
+ reference_Obj_.emplace_back(cv::Point3f(0.0, 100, 0.0));
+ reference_Obj_.emplace_back(cv::Point3f(0.0, 0.0, 100));
+
+ static double theta = 0;
+ static double r_data[] = {1, 0, 0,
+ 0, cos(theta), sin(theta),
+ 0, -sin(theta), cos(theta)};
+ static double t_data[] = {static_cast(pnp_config_.ptz_camera_x),
+ static_cast(pnp_config_.ptz_camera_y),
+ static_cast(pnp_config_.ptz_camera_z)};
+
+ r_camera_ptz = cv::Mat(3, 3, CV_64FC1, r_data);
+ t_camera_ptz = cv::Mat(3, 1, CV_64FC1, t_data);
+
+ big_object_3d_.emplace_back(
+ cv::Point3f(-pnp_config_.big_armor_width * 0.5,
+ -pnp_config_.big_armor_height * 0.5, 0));
+ big_object_3d_.emplace_back(
+ cv::Point3f(pnp_config_.big_armor_width * 0.5,
+ -pnp_config_.big_armor_height * 0.5, 0));
+ big_object_3d_.emplace_back(
+ cv::Point3f(pnp_config_.big_armor_width * 0.5,
+ pnp_config_.big_armor_height * 0.5, 0));
+ big_object_3d_.emplace_back(
+ cv::Point3f(-pnp_config_.big_armor_width * 0.5,
+ pnp_config_.big_armor_height * 0.5, 0));
+
+ small_object_3d_.emplace_back(
+ cv::Point3f(-pnp_config_.small_armor_width * 0.5,
+ -pnp_config_.small_armor_height * 0.5, 0));
+ small_object_3d_.emplace_back(
+ cv::Point3f(pnp_config_.small_armor_width * 0.5,
+ -pnp_config_.small_armor_height * 0.5, 0));
+ small_object_3d_.emplace_back(
+ cv::Point3f(pnp_config_.small_armor_width * 0.5,
+ pnp_config_.small_armor_height * 0.5, 0));
+ small_object_3d_.emplace_back(
+ cv::Point3f(-pnp_config_.small_armor_width * 0.5,
+ pnp_config_.small_armor_height * 0.5, 0));
+
+ buff_object_3d_.emplace_back(
+ cv::Point3f(-pnp_config_.buff_armor_width * 0.5,
+ -pnp_config_.buff_armor_height * 0.5, 0));
+ buff_object_3d_.emplace_back(
+ cv::Point3f(pnp_config_.buff_armor_width * 0.5,
+ -pnp_config_.buff_armor_height * 0.5, 0));
+ buff_object_3d_.emplace_back(
+ cv::Point3f(pnp_config_.buff_armor_width * 0.5,
+ pnp_config_.buff_armor_height * 0.5, 0));
+ buff_object_3d_.emplace_back(
+ cv::Point3f(-pnp_config_.buff_armor_width * 0.5,
+ pnp_config_.buff_armor_height * 0.5, 0));
+ }
+
+ ~PnP() = default;
+ /**
+ * @brief 初始化目标 3d 点
+ *
+ * @param _armor_type 装甲板类型
+ * @return std::vector 返回目标 3d 点
+ */
+ std::vector initialize3DPoints(int _armor_type)
+ {
+ // 选择装甲板类型
+ switch (_armor_type)
+ {
case 0:
return small_object_3d_;
break;
@@ -121,329 +127,382 @@ class PnP {
default:
return small_object_3d_;
break;
+ }
}
- }
- /**
- * @brief 初始化目标 3d 点
- *
- * @param _width 目标实际宽度
- * @param _heigth 目标实际高度
- * @return std::vector 返回目标 3d 点
- */
- std::vector initialize3DPoints(int _width, int _heigth) {
- float half_x = _width * 0.5;
- float half_y = _heigth * 0.5;
-
- // 3d点模型赋值
- std::vector object_3d = {
- cv::Point3f(-half_x, -half_y, 0),
- cv::Point3f(half_x, -half_y, 0),
- cv::Point3f(half_x, half_y, 0),
- cv::Point3f(-half_x, half_y, 0)
- };
-
- return object_3d;
- }
- /**
- * @brief 初始化目标 2d 点
- *
- * @param _rect 目标旋转矩形
- * @return std::vector 返回目标 2d 点
- */
- std::vector initialize2DPoints(cv::RotatedRect _rect) {
- std::vector target2d;
-
- static cv::Point2f vertex[4];
- static cv::Point2f lu, ld, ru, rd;
-
- _rect.points(vertex);
- // 排序 左上->右上->右下->左下
- std::sort(vertex, vertex + 4,
- [](const cv::Point2f& p1, const cv::Point2f& p2) {
- return p1.x < p2.x;
- });
-
- if (vertex[0].y < vertex[1].y) {
- lu = vertex[0];
- ld = vertex[1];
- } else {
- lu = vertex[1];
- ld = vertex[0];
- }
- if (vertex[2].y < vertex[3].y) {
- ru = vertex[2];
- rd = vertex[3];
- } else {
- ru = vertex[3];
- rd = vertex[2];
+ /**
+ * @brief 初始化目标 3d 点
+ *
+ * @param _width 目标实际宽度
+ * @param _heigth 目标实际高度
+ * @return std::vector 返回目标 3d 点
+ */
+ std::vector initialize3DPoints(int _width, int _heigth)
+ {
+ float half_x = _width * 0.5;
+ float half_y = _heigth * 0.5;
+
+ // 3d点模型赋值
+ std::vector object_3d = {
+ cv::Point3f(-half_x, -half_y, 0),
+ cv::Point3f(half_x, -half_y, 0),
+ cv::Point3f(half_x, half_y, 0),
+ cv::Point3f(-half_x, half_y, 0)};
+
+ return object_3d;
}
+ /**
+ * @brief 初始化目标 2d 点
+ *
+ * @param _rect 目标旋转矩形
+ * @return std::vector 返回目标 2d 点
+ */
+ std::vector initialize2DPoints(cv::RotatedRect _rect)
+ {
+ std::vector target2d;
+
+ static cv::Point2f vertex[4];
+ static cv::Point2f lu, ld, ru, rd;
+
+ _rect.points(vertex);
+ // 排序 左上->右上->右下->左下
+ std::sort(vertex, vertex + 4,
+ [](const cv::Point2f &p1, const cv::Point2f &p2)
+ {
+ return p1.x < p2.x;
+ });
+
+ if (vertex[0].y < vertex[1].y)
+ {
+ lu = vertex[0];
+ ld = vertex[1];
+ }
+ else
+ {
+ lu = vertex[1];
+ ld = vertex[0];
+ }
+ if (vertex[2].y < vertex[3].y)
+ {
+ ru = vertex[2];
+ rd = vertex[3];
+ }
+ else
+ {
+ ru = vertex[3];
+ rd = vertex[2];
+ }
- target2d.emplace_back(ld);
- target2d.emplace_back(rd);
- target2d.emplace_back(ru);
- target2d.emplace_back(lu);
- return target2d;
- }
- /**
- * @brief 初始化目标 2d 点
- *
- * @param _rect 目标外接矩形
- * @return std::vector 返回目标 2d 点
- */
- std::vector initialize2DPoints(cv::Rect _rect) {
- cv::RotatedRect box = rectChangeRotatedrect(_rect);
-
- return initialize2DPoints(box);
- }
- /**
- * @brief 外接矩形转旋转矩形
- *
- * @param _rect 目标外接矩形
- * @return cv::RotatedRect 返回旋转矩形
- */
- cv::RotatedRect rectChangeRotatedrect(cv::Rect _rect) {
- cv::RotatedRect box =
- cv::RotatedRect((_rect.tl() + _rect.br()) / 2,
- cv::Size(_rect.width / 2, _rect.height / 2),
- 0);
-
- return box;
- }
- /**
- * @brief 转换坐标系
- *
- * @param _t 旋转向量
- * @return cv::Mat 返回转化后的旋转向量
- */
- cv::Mat cameraPtz(cv::Mat& _t) {
- return r_camera_ptz * _t - t_camera_ptz;
- }
- /**
- * @brief 绘制坐标系
- *
- * @param _draw_img 画板
- * @param _rvec 旋转矩阵
- * @param _tvec 旋转向量
- * @param _cameraMatrix 相机内参
- * @param _distcoeffs 相机外参
- */
- void drawCoordinate(cv::Mat& _draw_img,
- cv::Mat& _rvec, cv::Mat& _tvec,
- cv::Mat& _cameraMatrix, cv::Mat& _distcoeffs) {
- std::vector reference_Img;
-
- cv::projectPoints(reference_Obj_,
- _rvec, _tvec,
- _cameraMatrix, _distcoeffs,
- reference_Img);
- // 绘制坐标系
- cv::line(_draw_img, reference_Img[0], reference_Img[1], cv::Scalar(0, 0, 255), 2);
- cv::line(_draw_img, reference_Img[0], reference_Img[2], cv::Scalar(0, 255, 0), 2);
- cv::line(_draw_img, reference_Img[0], reference_Img[3], cv::Scalar(255, 0, 0), 2);
+ target2d.emplace_back(ld);
+ target2d.emplace_back(rd);
+ target2d.emplace_back(ru);
+ target2d.emplace_back(lu);
+ return target2d;
+ }
+ /**
+ * @brief 初始化目标 2d 点
+ *
+ * @param _rect 目标外接矩形
+ * @return std::vector 返回目标 2d 点
+ */
+ std::vector initialize2DPoints(cv::Rect _rect)
+ {
+ cv::RotatedRect box = rectChangeRotatedrect(_rect);
+
+ return initialize2DPoints(box);
+ }
+ /**
+ * @brief 外接矩形转旋转矩形
+ *
+ * @param _rect 目标外接矩形
+ * @return cv::RotatedRect 返回旋转矩形
+ */
+ cv::RotatedRect rectChangeRotatedrect(cv::Rect _rect)
+ {
+ cv::RotatedRect box =
+ cv::RotatedRect((_rect.tl() + _rect.br()) / 2,
+ cv::Size(_rect.width / 2, _rect.height / 2),
+ 0);
+
+ return box;
+ }
+ /**
+ * @brief 转换坐标系
+ *
+ * @param _t 旋转向量
+ * @return cv::Mat 返回转化后的旋转向量
+ */
+ cv::Mat cameraPtz(cv::Mat &_t)
+ {
+ return r_camera_ptz * _t - t_camera_ptz;
+ }
+ /**
+ * @brief 绘制坐标系
+ *
+ * @param _draw_img 画板
+ * @param _rvec 旋转矩阵
+ * @param _tvec 旋转向量
+ * @param _cameraMatrix 相机内参
+ * @param _distcoeffs 相机外参
+ */
+ void drawCoordinate(cv::Mat &_draw_img,
+ cv::Mat &_rvec, cv::Mat &_tvec,
+ cv::Mat &_cameraMatrix, cv::Mat &_distcoeffs)
+ {
+ std::vector reference_Img;
+
+ cv::projectPoints(reference_Obj_,
+ _rvec, _tvec,
+ _cameraMatrix, _distcoeffs,
+ reference_Img);
+ // 绘制坐标系
+ cv::line(_draw_img, reference_Img[0], reference_Img[1], cv::Scalar(0, 0, 255), 2);
+ cv::line(_draw_img, reference_Img[0], reference_Img[2], cv::Scalar(0, 255, 0), 2);
+ cv::line(_draw_img, reference_Img[0], reference_Img[3], cv::Scalar(255, 0, 0), 2);
#ifndef RELEASE
- cv::imshow("[abstract_pnp] drawCoordinate() -> _draw_img", _draw_img);
+ cv::imshow("[abstract_pnp] drawCoordinate() -> _draw_img", _draw_img);
#endif
- }
- /**
- * @brief 计算子弹下坠
- *
- * @param _dist 目标深度
- * @param _tvec_y 目标高度
- * @param _ballet_speed 子弹速度
- * @param _company 计算单位
- * @return float 返回补偿角度
- */
- float getPitch(float _dist,
- float _tvec_y,
- float _ballet_speed,
- const int _company = 1) {
- // 选择计算单位
- _dist /= _company;
- _tvec_y /= _company;
- _ballet_speed /= _company;
-
- float y_temp = _tvec_y;
- float y_actual = 0.f;
- float dy = 0.f;
- float a = 0.f;
- const float gravity = 10000.f / _company;
-
- for (size_t i = 0; i != 20; ++i) {
- a = static_cast(atan2(y_temp, _dist));
- // 子弹飞行时间
- float t = (float((exp( 0.001 * _dist)-1) / ( 0.001 * _dist * 0.001 * _ballet_speed * cos(a))));
- // float t = _dist / _ballet_speed * cos(a);
-
- y_actual = _ballet_speed * sin(a) * t - gravity * t * t / 2;
- dy = _tvec_y - y_actual;
- y_temp += dy;
-
- if (fabsf(dy) < 1e-2) { break; }
}
+ /**
+ * @brief 计算子弹下坠
+ *
+ * @param _dist 目标深度
+ * @param _tvec_y 目标高度
+ * @param _ballet_speed 子弹速度
+ * @param _company 计算单位
+ * @return float 返回补偿角度
+ */
+ float getPitch(float _dist,
+ float _tvec_y,
+ float _ballet_speed,
+ const int _company = 1)
+ {
+ // 选择计算单位
+ _dist /= _company;
+ _tvec_y /= _company;
+ _ballet_speed /= _company;
+
+ float y_temp = _tvec_y;
+ float y_actual = 0.f;
+ float dy = 0.f;
+ float a = 0.f;
+ const float gravity = 10000.f / _company;
+
+ for (size_t i = 0; i != 20; ++i)
+ {
+ a = static_cast(atan2(y_temp, _dist));
+ // 子弹飞行时间
+ float t = (float((exp(0.001 * _dist) - 1) / (0.001 * _dist * 0.001 * _ballet_speed * cos(a))));
+ // float t = _dist / _ballet_speed * cos(a);
+
+ y_actual = _ballet_speed * sin(a) * t - gravity * t * t / 2;
+ dy = _tvec_y - y_actual;
+ y_temp += dy;
+
+ if (fabsf(dy) < 1e-2)
+ {
+ break;
+ }
+ }
- return a;
- }
- /**
- * @brief 计算云台偏差角度
- *
- * @param _pos_in_ptz 旋转向量
- * @param _bullet_speed 子弹速度
- * @param _company 计算子弹下坠单位
- * @return cv::Point3f 返回 Yaw Pitch 轴的偏移量和深度（mm）
- */
- cv::Point3f getAngle(const cv::Mat& _pos_in_ptz,
- const int _bullet_speed,
- const int _company) {
- cv::Point3f angle;
-
- const double *_xyz = reinterpret_cast(_pos_in_ptz.data);
- double down_t = 0.f;
-
- if (_bullet_speed > 1e-2) {
- down_t = _xyz[2] / (_bullet_speed * 1000);
+ return a;
}
+ /**
+ * @brief 计算云台偏差角度
+ *
+ * @param _pos_in_ptz 旋转向量
+ * @param _bullet_speed 子弹速度
+ * @param _company 计算子弹下坠单位
+ * @return cv::Point3f 返回 Yaw Pitch 轴的偏移量和深度（mm）
+ */
+ cv::Point3f getAngle(const cv::Mat &_pos_in_ptz,
+ const int _bullet_speed,
+ const int _company)
+ {
+ cv::Point3f angle;
+
+ const double *_xyz = reinterpret_cast(_pos_in_ptz.data);
+ double down_t = 0.f;
+
+ if (_bullet_speed > 1e-2)
+ {
+ down_t = _xyz[2] / (_bullet_speed * 1000);
+ }
- double offset_gravity = 0.5 * 9.8 * down_t * down_t * 1000;
- double xyz[3] = {_xyz[0], _xyz[1] - offset_gravity, _xyz[2]};
-
- if (pnp_config_.barrel_ptz_offset_y != 0.f) {
- double alpha =
- asin(static_cast(pnp_config_.barrel_ptz_offset_y) /
- sqrt(xyz[1] * xyz[1] + xyz[2] * xyz[2]));
- double beta = 0.f;
-
- if (xyz[1] < 0) {
- beta = atan(-xyz[1] / xyz[2]);
- angle.y = static_cast(-(alpha + beta)); // camera coordinate
- } else if (xyz[1] < static_cast(pnp_config_.barrel_ptz_offset_y)) {
- beta = atan(xyz[1] / xyz[2]);
- angle.y = static_cast(-(alpha - beta));
- } else {
- beta = atan(xyz[1] / xyz[2]);
- angle.y = static_cast((beta - alpha)); // camera coordinate
+ double offset_gravity = 0.5 * 9.8 * down_t * down_t * 1000;
+ double xyz[3] = {_xyz[0], _xyz[1] - offset_gravity, _xyz[2]};
+
+ if (pnp_config_.barrel_ptz_offset_y != 0.f)
+ {
+ double alpha =
+ asin(static_cast(pnp_config_.barrel_ptz_offset_y) /
+ sqrt(xyz[1] * xyz[1] + xyz[2] * xyz[2]));
+ double beta = 0.f;
+
+ if (xyz[1] < 0)
+ {
+ beta = atan(-xyz[1] / xyz[2]);
+ angle.y = static_cast(-(alpha + beta)); // camera coordinate
+ }
+ else if (xyz[1] < static_cast(pnp_config_.barrel_ptz_offset_y))
+ {
+ beta = atan(xyz[1] / xyz[2]);
+ angle.y = static_cast(-(alpha - beta));
+ }
+ else
+ {
+ beta = atan(xyz[1] / xyz[2]);
+ angle.y = static_cast((beta - alpha)); // camera coordinate
+ }
+ }
+ else
+ {
+ angle.y = static_cast(atan2(xyz[1], xyz[2]));
}
- } else {
- angle.y = static_cast(atan2(xyz[1], xyz[2]));
- }
- if (pnp_config_.barrel_ptz_offset_x != 0.f) {
- double alpha =
- asin(static_cast(pnp_config_.barrel_ptz_offset_x) /
- sqrt(xyz[0] * xyz[0] + xyz[2] * xyz[2]));
- double beta = 0.f;
-
- if (xyz[0] > 0) {
- beta = atan(-xyz[0] / xyz[2]);
- angle.x = static_cast(-(alpha + beta)); // camera coordinate
- } else if (xyz[0] < static_cast(pnp_config_.barrel_ptz_offset_x)) {
- beta = atan(xyz[0] / xyz[2]);
- angle.x = static_cast(-(alpha - beta));
- } else {
- beta = atan(xyz[0] / xyz[2]);
- angle.x = static_cast(beta - alpha); // camera coordinate
+ if (pnp_config_.barrel_ptz_offset_x != 0.f)
+ {
+ double alpha =
+ asin(static_cast(pnp_config_.barrel_ptz_offset_x) /
+ sqrt(xyz[0] * xyz[0] + xyz[2] * xyz[2]));
+ double beta = 0.f;
+
+ if (xyz[0] > 0)
+ {
+ beta = atan(-xyz[0] / xyz[2]);
+ angle.x = static_cast(-(alpha + beta)); // camera coordinate
+ }
+ else if (xyz[0] < static_cast(pnp_config_.barrel_ptz_offset_x))
+ {
+ beta = atan(xyz[0] / xyz[2]);
+ angle.x = static_cast(-(alpha - beta));
+ }
+ else
+ {
+ beta = atan(xyz[0] / xyz[2]);
+ angle.x = static_cast(beta - alpha); // camera coordinate
+ }
}
- } else {
- angle.x = static_cast(atan2(xyz[0], xyz[2]));
- }
+ else
+ {
+ angle.x = static_cast(atan2(xyz[0], xyz[2]));
+ }
+
+ // 深度
+ angle.z = static_cast(xyz[2]);
+ // Yaw
+ angle.x = static_cast(angle.x) * 180 / CV_PI;
+ // Pitch
+ angle.y = static_cast(angle.y) * 180 / CV_PI;
+ angle.y -= getPitch(xyz[2], xyz[1], _bullet_speed * 1000, _company);
- // 深度
- angle.z = static_cast(xyz[2]);
- // Yaw
- angle.x = static_cast(angle.x) * 180 / CV_PI;
- // Pitch
- angle.y = static_cast(angle.y) * 180 / CV_PI;
- angle.y -= getPitch(xyz[2], xyz[1], _bullet_speed * 1000, _company);
-
- return angle;
- }
- /**
- * @brief 计算云台偏差角度
- *
- * @param _pos_in_ptz 旋转向量
- * @param _bullet_speed 子弹速度
- * @param _company 子弹下坠单位
- * @param _depth 距目标的深度
- * @return cv::Point3f 返回 Yaw Pitch 轴的偏移量和深度（mm）
- */
- cv::Point3f getAngle(const cv::Mat& _pos_in_ptz,
- const int _bullet_speed,
- const int _company,
- const int _depth) {
- cv::Point3f angle;
-
- const double *_xyz = reinterpret_cast(_pos_in_ptz.data);
- double down_t = 0.f;
-
- if (_bullet_speed > 1e-2) {
- down_t = _xyz[2] / (_bullet_speed * 1000);
+ return angle;
}
+ /**
+ * @brief 计算云台偏差角度
+ *
+ * @param _pos_in_ptz 旋转向量
+ * @param _bullet_speed 子弹速度
+ * @param _company 子弹下坠单位
+ * @param _depth 距目标的深度
+ * @return cv::Point3f 返回 Yaw Pitch 轴的偏移量和深度（mm）
+ */
+ cv::Point3f getAngle(const cv::Mat &_pos_in_ptz,
+ const int _bullet_speed,
+ const int _company,
+ const int _depth)
+ {
+ cv::Point3f angle;
+
+ const double *_xyz = reinterpret_cast(_pos_in_ptz.data);
+ double down_t = 0.f;
+
+ if (_bullet_speed > 1e-2)
+ {
+ down_t = _xyz[2] / (_bullet_speed * 1000);
+ }
- double offset_gravity = 0.5 * 9.8 * down_t * down_t * 1000;
- double xyz[3] = {_xyz[0], _xyz[1] - offset_gravity, _xyz[2]};
-
- if (pnp_config_.barrel_ptz_offset_y != 0.f) {
- double alpha =
- asin(static_cast(pnp_config_.barrel_ptz_offset_y) /
- sqrt(xyz[1] * xyz[1] + xyz[2] * xyz[2]));
- double beta = 0.f;
-
- if (xyz[1] < 0) {
- beta = atan(-xyz[1] / xyz[2]);
- angle.y = static_cast(-(alpha + beta)); // camera coordinate
- } else if (xyz[1] < static_cast(pnp_config_.barrel_ptz_offset_y)) {
- beta = atan(xyz[1] / xyz[2]);
- angle.y = static_cast(-(alpha - beta));
- } else {
- beta = atan(xyz[1] / xyz[2]);
- angle.y = static_cast((beta - alpha)); // camera coordinate
+ double offset_gravity = 0.5 * 9.8 * down_t * down_t * 1000;
+ double xyz[3] = {_xyz[0], _xyz[1] - offset_gravity, _xyz[2]};
+
+ if (pnp_config_.barrel_ptz_offset_y != 0.f)
+ {
+ double alpha =
+ asin(static_cast(pnp_config_.barrel_ptz_offset_y) /
+ sqrt(xyz[1] * xyz[1] + xyz[2] * xyz[2]));
+ double beta = 0.f;
+
+ if (xyz[1] < 0)
+ {
+ beta = atan(-xyz[1] / xyz[2]);
+ angle.y = static_cast(-(alpha + beta)); // camera coordinate
+ }
+ else if (xyz[1] < static_cast(pnp_config_.barrel_ptz_offset_y))
+ {
+ beta = atan(xyz[1] / xyz[2]);
+ angle.y = static_cast(-(alpha - beta));
+ }
+ else
+ {
+ beta = atan(xyz[1] / xyz[2]);
+ angle.y = static_cast((beta - alpha)); // camera coordinate
+ }
+ }
+ else
+ {
+ angle.y = static_cast(atan2(xyz[1], xyz[2]));
}
- } else {
- angle.y = static_cast(atan2(xyz[1], xyz[2]));
- }
- if (pnp_config_.barrel_ptz_offset_x != 0.f) {
- double alpha =
- asin(static_cast(pnp_config_.barrel_ptz_offset_x) /
- sqrt(xyz[0] * xyz[0] + xyz[2] * xyz[2]));
- double beta = 0.f;
-
- if (xyz[0] > 0) {
- beta = atan(-xyz[0] / xyz[2]);
- angle.x = static_cast(-(alpha + beta)); // camera coordinate
- } else if (xyz[0] <
- static_cast(pnp_config_.barrel_ptz_offset_x)) {
- beta = atan(xyz[0] / xyz[2]);
- angle.x = static_cast(-(alpha - beta));
- } else {
- beta = atan(xyz[0] / xyz[2]);
- angle.x = static_cast(beta - alpha); // camera coordinate
+ if (pnp_config_.barrel_ptz_offset_x != 0.f)
+ {
+ double alpha =
+ asin(static_cast(pnp_config_.barrel_ptz_offset_x) /
+ sqrt(xyz[0] * xyz[0] + xyz[2] * xyz[2]));
+ double beta = 0.f;
+
+ if (xyz[0] > 0)
+ {
+ beta = atan(-xyz[0] / xyz[2]);
+ angle.x = static_cast(-(alpha + beta)); // camera coordinate
+ }
+ else if (xyz[0] <
+ static_cast(pnp_config_.barrel_ptz_offset_x))
+ {
+ beta = atan(xyz[0] / xyz[2]);
+ angle.x = static_cast(-(alpha - beta));
+ }
+ else
+ {
+ beta = atan(xyz[0] / xyz[2]);
+ angle.x = static_cast(beta - alpha); // camera coordinate
+ }
+ }
+ else
+ {
+ angle.x = static_cast(atan2(xyz[0], xyz[2]));
}
- } else {
- angle.x = static_cast(atan2(xyz[0], xyz[2]));
- }
- angle.z = _depth;
- angle.x = static_cast(angle.x) * 180 / CV_PI;
- angle.y = static_cast(angle.y) * 180 / CV_PI;
- angle.y -= getPitch(_depth, xyz[1], _bullet_speed * 1000, _company);
+ angle.z = _depth;
+ angle.x = static_cast(angle.x) * 180 / CV_PI;
+ angle.y = static_cast(angle.y) * 180 / CV_PI;
+ angle.y -= getPitch(_depth, xyz[1], _bullet_speed * 1000, _company);
- return angle;
- }
+ return angle;
+ }
- private:
- cv::Mat pnp_config_trackbar_ = cv::Mat::zeros(1, 300, CV_8UC1);
+ private:
+ cv::Mat pnp_config_trackbar_ = cv::Mat::zeros(1, 300, CV_8UC1);
- std::vector reference_Obj_;
- std::vector big_object_3d_;
- std::vector small_object_3d_;
- std::vector buff_object_3d_;
+ std::vector reference_Obj_;
+ std::vector big_object_3d_;
+ std::vector small_object_3d_;
+ std::vector buff_object_3d_;
- double theta = 0.0;
- double r_data[9];
- double t_data[3];
+ double theta = 0.0;
+ double r_data[9];
+ double t_data[3];
- cv::Mat r_camera_ptz;
- cv::Mat t_camera_ptz;
-};
+ cv::Mat r_camera_ptz;
+ cv::Mat t_camera_ptz;
+ };
-} // namespace abstract_pnp
+} // namespace abstract_pnp
diff --git a/module/angle_solve/angle_solve.cpp b/module/angle_solve/angle_solve.cpp
index 07d6e02..6ca1bd0 100644
--- a/module/angle_solve/angle_solve.cpp
+++ b/module/angle_solve/angle_solve.cpp
@@ -9,8 +9,10 @@
#include "angle_solve.hpp"
-namespace angle_solve {
- void solve::set_config(std::string config_path) {
+namespace angle_solve
+{
+ void solve::set_config(std::string config_path)
+ {
cv::FileStorage conf(config_path, cv::FileStorage::READ);
conf["ARMOR_HEIGHT"] >> config.armor_height;
conf["ARMOR_LENGHT"] >> config.armor_lenght;
@@ -19,53 +21,58 @@ namespace angle_solve {
conf["PIC_DISTANCE"] >> config.pic_distance;
conf["ARMOR_DISTANCE"] >> config.armor_distance;
conf["SPEED_ARG"] >> config.speed_arg;
- fmt::print("[angle info] config st {}\n",config.pic_distance);
+ fmt::print("[angle info] config st {}\n", config.pic_distance);
conf.release();
}
- void solve::angleSolve(cv::RotatedRect object, int row, int col, uart::SerialPort& info) {
+ void solve::angleSolve(cv::RotatedRect object, int row, int col, uart::SerialPort &info)
+ {
//
// 左加右减 上减下加
- fmt::print("[angle info] config {}\n",config.pic_distance);
+ fmt::print("[angle info] config {}\n", config.pic_distance);
target.predict.y = config.pic_distance * config.armor_height / object.size.height;
target.predict.x = (object.center.x - col / 2.0) * target.predict.y / config.pic_distance;
target.predict.z = ((row - object.center.y) - row / 2.0) * target.predict.y / config.pic_distance;
- fmt::print("[{}] L: {} H: {} row: {} object_y: {}\n", angle_info,target.predict.y, target.predict.z, row, object.center.y);
- //fmt::print("[{}] velo: {}\n",angle_info,target.predict.y, info.returnReceiveBulletVelocity());
- target.time = target.predict.y / ((info.returnReceiveBulletVelocity()*config.speed_arg) * cos(-info.returnReceivePitch()/180*PI) * 100);
+ fmt::print("[{}] L: {} H: {} row: {} object_y: {}\n", angle_info, target.predict.y, target.predict.z, row, object.center.y);
+ // fmt::print("[{}] velo: {}\n",angle_info,target.predict.y, info.returnReceiveBulletVelocity());
+ target.time = target.predict.y / ((info.returnReceiveBulletVelocity() * config.speed_arg) * cos(-info.returnReceivePitch() / 180 * PI) * 100);
target.predict.z = target.predict.z + 0.5 * 9.8 * 100 * target.time * target.time;
- fmt::print("[angle info] p {} {} {}\n", 0.5 * 9.8 * 100 * target.time * target.time,target.time, info.returnReceiveBulletVelocity());
- //compensation.pitch =
+ fmt::print("[angle info] p {} {} {}\n", 0.5 * 9.8 * 100 * target.time * target.time, target.time, info.returnReceiveBulletVelocity());
+ // compensation.pitch =
target.yaw = atan(-target.predict.x / target.predict.y) / PI * 180;
target.pitch = atan(-target.predict.z / sqrt(pow(target.predict.y, 2) + pow(target.predict.x, 2))) / PI * 180;
fmt::print("[angle info] x {} y {} z {} yaw {} pitch {}\n", target.predict.x, target.predict.y, target.predict.z, target.yaw, target.pitch);
fmt::print("[angle info] y {}\n", target.predict.y);
-
+
fmt::print("[angle info] center x {} center y {}\n", object.center.x, object.center.y);
}
-
- float solve::returnYawAngle() {
- if(fabs(target.yaw) < 0.02) {
+ float solve::returnYawAngle()
+ {
+ if (fabs(target.yaw) < 0.02)
+ {
return 0;
- //return target.yaw;
- } else {
+ // return target.yaw;
+ }
+ else
+ {
return target.yaw;
}
-
}
- float solve::returnPitchAngle() {
- if(fabs(target.pitch) < 0.02) {
+ float solve::returnPitchAngle()
+ {
+ if (fabs(target.pitch) < 0.02)
+ {
return 0;
- //return target.pitch;
- } else {
+ // return target.pitch;
+ }
+ else
+ {
return target.pitch;
}
-
}
-
} // namespace angle_solve
diff --git a/module/angle_solve/angle_solve.hpp b/module/angle_solve/angle_solve.hpp
index b2d4e69..065d8d2 100644
--- a/module/angle_solve/angle_solve.hpp
+++ b/module/angle_solve/angle_solve.hpp
@@ -19,51 +19,49 @@
#include
-
#define PI 3.14159265
auto angle_info = fmt::format(fg(fmt::color::pink) | fmt::emphasis::bold, "angle_solve_info");
-namespace angle_solve {
- class solve {
- public:
-
- void set_config(std::string config_path);
-
- void angleSolve(cv::RotatedRect object, int row, int col, uart::SerialPort& info);
- //
-
- float returnYawAngle();
-
- float returnPitchAngle();
-
-
- private:
-
- struct info {
- float armor_height;
- float armor_lenght;
- float pic_armor_height;
- float pic_armor_lenght;
- float pic_distance;
- float armor_distance;
- float speed_arg;
- } config;
-
- struct cord {
- cv::Point3d predict;
- float yaw;
- float pitch;
- float time;
- } target;
-
- struct gyro {
- float yaw;
- float pitch;
- } compensation;
-
+namespace angle_solve
+{
+ class solve
+ {
+ public:
+ void set_config(std::string config_path);
+
+ void angleSolve(cv::RotatedRect object, int row, int col, uart::SerialPort &info);
+ //
+
+ float returnYawAngle();
+
+ float returnPitchAngle();
+
+ private:
+ struct info
+ {
+ float armor_height;
+ float armor_lenght;
+ float pic_armor_height;
+ float pic_armor_lenght;
+ float pic_distance;
+ float armor_distance;
+ float speed_arg;
+ } config;
+
+ struct cord
+ {
+ cv::Point3d predict;
+ float yaw;
+ float pitch;
+ float time;
+ } target;
+
+ struct gyro
+ {
+ float yaw;
+ float pitch;
+ } compensation;
};
-
-
};
\ No newline at end of file
diff --git a/module/angle_solve/basic_pnp.cpp b/module/angle_solve/basic_pnp.cpp
index 35345f2..d765260 100644
--- a/module/angle_solve/basic_pnp.cpp
+++ b/module/angle_solve/basic_pnp.cpp
@@ -9,51 +9,54 @@
*/
#include "basic_pnp.hpp"
-namespace basic_pnp {
+namespace basic_pnp
+{
-PnP::PnP(std::string _camera_path,
- std::string _pnp_config_path) {
- cv::FileStorage fs_camera(_camera_path, cv::FileStorage::READ);
+ PnP::PnP(std::string _camera_path,
+ std::string _pnp_config_path)
+ {
+ cv::FileStorage fs_camera(_camera_path, cv::FileStorage::READ);
- fs_camera["camera-matrix"] >> cameraMatrix_;
- fs_camera["distortion"] >> distCoeffs_;
+ fs_camera["camera-matrix"] >> cameraMatrix_;
+ fs_camera["distortion"] >> distCoeffs_;
- cv::FileStorage fs_config(_pnp_config_path, cv::FileStorage::READ);
+ cv::FileStorage fs_config(_pnp_config_path, cv::FileStorage::READ);
- fs_config["PTZ_CAMERA_X"] >> pnp_config_.ptz_camera_x;
- fs_config["PTZ_CAMERA_Y"] >> pnp_config_.ptz_camera_y;
- fs_config["PTZ_CAMERA_Z"] >> pnp_config_.ptz_camera_z;
+ fs_config["PTZ_CAMERA_X"] >> pnp_config_.ptz_camera_x;
+ fs_config["PTZ_CAMERA_Y"] >> pnp_config_.ptz_camera_y;
+ fs_config["PTZ_CAMERA_Z"] >> pnp_config_.ptz_camera_z;
- fs_config["PTZ_BARREL_X"] >> pnp_config_.barrel_ptz_offset_x;
- fs_config["PTZ_BARREL_Y"] >> pnp_config_.barrel_ptz_offset_y;
+ fs_config["PTZ_BARREL_X"] >> pnp_config_.barrel_ptz_offset_x;
+ fs_config["PTZ_BARREL_Y"] >> pnp_config_.barrel_ptz_offset_y;
- fs_config["OFFSET_ARMOR_YAW"] >> pnp_config_.offset_armor_yaw;
- fs_config["OFFSET_ARMOR_PITCH"] >> pnp_config_.offset_armor_pitch;
+ fs_config["OFFSET_ARMOR_YAW"] >> pnp_config_.offset_armor_yaw;
+ fs_config["OFFSET_ARMOR_PITCH"] >> pnp_config_.offset_armor_pitch;
- fs_config["COMPANY"] >> pnp_config_.company;
- fs_config["BIG_ARMOR_WIDTH"] >> pnp_config_.big_armor_width;
- fs_config["BIG_ARMOR_HEIGHT"] >> pnp_config_.big_armor_height;
+ fs_config["COMPANY"] >> pnp_config_.company;
+ fs_config["BIG_ARMOR_WIDTH"] >> pnp_config_.big_armor_width;
+ fs_config["BIG_ARMOR_HEIGHT"] >> pnp_config_.big_armor_height;
- fs_config["SMALL_ARMOR_WIDTH"] >> pnp_config_.small_armor_width;
- fs_config["SMALL_ARMOR_HEIGHT"] >> pnp_config_.small_armor_height;
+ fs_config["SMALL_ARMOR_WIDTH"] >> pnp_config_.small_armor_width;
+ fs_config["SMALL_ARMOR_HEIGHT"] >> pnp_config_.small_armor_height;
- fs_config["BUFF_ARMOR_WIDTH"] >> pnp_config_.buff_armor_width;
- fs_config["BUFF_ARMOR_HEIGHT"] >> pnp_config_.buff_armor_height;
+ fs_config["BUFF_ARMOR_WIDTH"] >> pnp_config_.buff_armor_width;
+ fs_config["BUFF_ARMOR_HEIGHT"] >> pnp_config_.buff_armor_height;
- fmt::print("[{}] Info, ptz_camera_x,y,z: {}, {}, {}\n", idntifier_green,
- pnp_config_.ptz_camera_x,
- pnp_config_.ptz_camera_y,
- pnp_config_.ptz_camera_z);
+ fmt::print("[{}] Info, ptz_camera_x,y,z: {}, {}, {}\n", idntifier_green,
+ pnp_config_.ptz_camera_x,
+ pnp_config_.ptz_camera_y,
+ pnp_config_.ptz_camera_z);
- fmt::print("[{}] Info, ptz_barrel_x,y: {}, {}\n", idntifier_green,
- pnp_config_.barrel_ptz_offset_x,
- pnp_config_.barrel_ptz_offset_y);
+ fmt::print("[{}] Info, ptz_barrel_x,y: {}, {}\n", idntifier_green,
+ pnp_config_.barrel_ptz_offset_x,
+ pnp_config_.barrel_ptz_offset_y);
- fmt::print("[{}] Info, offset_armor_yaw,pitch: {}, {}\n", idntifier_green,
- pnp_config_.offset_armor_yaw,
- pnp_config_.offset_armor_pitch);
+ fmt::print("[{}] Info, offset_armor_yaw,pitch: {}, {}\n", idntifier_green,
+ pnp_config_.offset_armor_yaw,
+ pnp_config_.offset_armor_pitch);
- switch (pnp_config_.company) {
+ switch (pnp_config_.company)
+ {
case 1:
fmt::print("[{}] Info, Gravity compensation algorithm unit: millimeter\n", idntifier_green);
break;
@@ -69,366 +72,378 @@ PnP::PnP(std::string _camera_path,
default:
fmt::print("[{}] Error, Gravity compensation algorithm unit: unknown\n", idntifier_red);
break;
+ }
+
+ fmt::print("[{}] Init PnP configuration finished\n", idntifier_green);
+ }
+
+ void PnP::solvePnP(int _ballet_speed,
+ int _armor_type,
+ cv::Mat &_src_img,
+ cv::RotatedRect _rect)
+ {
+ object_3d_ = initialize3DPoints(_armor_type);
+ target_2d_ = initialize2DPoints(_rect);
+
+ cv::solvePnP(object_3d_,
+ target_2d_,
+ cameraMatrix_,
+ distCoeffs_,
+ rvec_,
+ tvec_,
+ false,
+ cv::SOLVEPNP_ITERATIVE);
+
+ drawCoordinate(_src_img, rvec_, tvec_, cameraMatrix_, distCoeffs_);
+
+ cv::Mat ptz = cameraPtz(tvec_);
+ cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
+ // 固定偏移量
+ pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
+ pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
+ pnp_info_.depth = angle.z;
+
+ object_3d_.clear();
+ object_3d_.shrink_to_fit();
+ target_2d_.clear();
+ target_2d_.shrink_to_fit();
+ }
+
+ void PnP::solvePnP(int _ballet_speed,
+ int _armor_type,
+ cv::Mat &_src_img,
+ cv::Rect _rect)
+ {
+ object_3d_ = initialize3DPoints(_armor_type);
+ target_2d_ = initialize2DPoints(_rect);
+
+ cv::solvePnP(object_3d_,
+ target_2d_,
+ cameraMatrix_,
+ distCoeffs_,
+ rvec_,
+ tvec_,
+ false,
+ cv::SOLVEPNP_ITERATIVE);
+
+ drawCoordinate(_src_img, rvec_, tvec_, cameraMatrix_, distCoeffs_);
+
+ cv::Mat ptz = cameraPtz(tvec_);
+ cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
+
+ pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
+ pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
+ pnp_info_.depth = angle.z;
+
+ object_3d_.clear();
+ object_3d_.shrink_to_fit();
+ target_2d_.clear();
+ target_2d_.shrink_to_fit();
+ }
+
+ void PnP::solvePnP(int _ballet_speed,
+ int _width,
+ int _height,
+ cv::Mat &_src_img,
+ cv::RotatedRect _rect)
+ {
+ object_3d_ = initialize3DPoints(_width, _height);
+ target_2d_ = initialize2DPoints(_rect);
+
+ cv::solvePnP(object_3d_,
+ target_2d_,
+ cameraMatrix_,
+ distCoeffs_,
+ rvec_,
+ tvec_,
+ false,
+ cv::SOLVEPNP_ITERATIVE);
+
+ drawCoordinate(_src_img, rvec_, tvec_, cameraMatrix_, distCoeffs_);
+
+ cv::Mat ptz = cameraPtz(tvec_);
+ cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
+
+ pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
+ pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
+ pnp_info_.depth = angle.z;
+
+ object_3d_.clear();
+ object_3d_.shrink_to_fit();
+ target_2d_.clear();
+ target_2d_.shrink_to_fit();
+ }
+
+ void PnP::solvePnP(int _ballet_speed,
+ int _width,
+ int _height,
+ cv::Mat &_src_img,
+ cv::Rect _rect)
+ {
+ object_3d_ = initialize3DPoints(_width, _height);
+ target_2d_ = initialize2DPoints(_rect);
+
+ cv::solvePnP(object_3d_,
+ target_2d_,
+ cameraMatrix_,
+ distCoeffs_,
+ rvec_,
+ tvec_,
+ false,
+ cv::SOLVEPNP_ITERATIVE);
+
+ drawCoordinate(_src_img, rvec_, tvec_, cameraMatrix_, distCoeffs_);
+
+ cv::Mat ptz = cameraPtz(tvec_);
+ cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
+
+ pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
+ pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
+ pnp_info_.depth = angle.z;
+
+ object_3d_.clear();
+ object_3d_.shrink_to_fit();
+ target_2d_.clear();
+ target_2d_.shrink_to_fit();
+ }
+
+ void PnP::solvePnP(int _ballet_speed,
+ int _armor_type,
+ cv::RotatedRect _rect)
+ {
+ object_3d_ = initialize3DPoints(_armor_type);
+ target_2d_ = initialize2DPoints(_rect);
+
+ cv::solvePnP(object_3d_,
+ target_2d_,
+ cameraMatrix_,
+ distCoeffs_,
+ rvec_,
+ tvec_,
+ false,
+ cv::SOLVEPNP_ITERATIVE);
+
+ cv::Mat ptz = cameraPtz(tvec_);
+ cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
+
+ pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
+ pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
+ pnp_info_.depth = angle.z;
+
+ object_3d_.clear();
+ object_3d_.shrink_to_fit();
+ target_2d_.clear();
+ target_2d_.shrink_to_fit();
+ }
+
+ void PnP::solvePnP(int _ballet_speed,
+ int _armor_type,
+ cv::Rect _rect)
+ {
+ object_3d_ = initialize3DPoints(_armor_type);
+ target_2d_ = initialize2DPoints(_rect);
+
+ cv::solvePnP(object_3d_,
+ target_2d_,
+ cameraMatrix_,
+ distCoeffs_,
+ rvec_,
+ tvec_,
+ false,
+ cv::SOLVEPNP_ITERATIVE);
+
+ cv::Mat ptz = cameraPtz(tvec_);
+ cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
+
+ pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
+ pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
+ pnp_info_.depth = angle.z;
+
+ object_3d_.clear();
+ object_3d_.shrink_to_fit();
+ target_2d_.clear();
+ target_2d_.shrink_to_fit();
+ }
+
+ void PnP::solvePnP(int _ballet_speed,
+ int _width,
+ int _height,
+ cv::RotatedRect _rect)
+ {
+ object_3d_ = initialize3DPoints(_width, _height);
+ target_2d_ = initialize2DPoints(_rect);
+
+ cv::solvePnP(object_3d_,
+ target_2d_,
+ cameraMatrix_,
+ distCoeffs_,
+ rvec_,
+ tvec_,
+ false,
+ cv::SOLVEPNP_ITERATIVE);
+
+ cv::Mat ptz = cameraPtz(tvec_);
+ cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
+
+ pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
+ pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
+ pnp_info_.depth = angle.z;
+
+ object_3d_.clear();
+ object_3d_.shrink_to_fit();
+ target_2d_.clear();
+ target_2d_.shrink_to_fit();
+ }
+
+ void PnP::solvePnP(int _ballet_speed,
+ int _width,
+ int _height,
+ cv::Rect _rect)
+ {
+ object_3d_ = initialize3DPoints(_width, _height);
+ target_2d_ = initialize2DPoints(_rect);
+
+ cv::solvePnP(object_3d_,
+ target_2d_,
+ cameraMatrix_,
+ distCoeffs_,
+ rvec_,
+ tvec_,
+ false,
+ cv::SOLVEPNP_ITERATIVE);
+
+ cv::Mat ptz = cameraPtz(tvec_);
+ cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
+
+ pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
+ pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
+ pnp_info_.depth = angle.z;
+
+ object_3d_.clear();
+ object_3d_.shrink_to_fit();
+ target_2d_.clear();
+ target_2d_.shrink_to_fit();
+ }
+
+ void PnP::solvePnP(int _ballet_speed,
+ int _armor_type,
+ std::vector _target_2d)
+ {
+ object_3d_ = initialize3DPoints(_armor_type);
+ target_2d_ = _target_2d;
+
+ cv::solvePnP(object_3d_,
+ target_2d_,
+ cameraMatrix_,
+ distCoeffs_,
+ rvec_,
+ tvec_,
+ false,
+ cv::SOLVEPNP_ITERATIVE);
+
+ cv::Mat ptz = cameraPtz(tvec_);
+ cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
+
+ pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
+ pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
+ pnp_info_.depth = angle.z;
+
+ object_3d_.clear();
+ object_3d_.shrink_to_fit();
+ target_2d_.clear();
+ target_2d_.shrink_to_fit();
+ }
+
+ void PnP::solvePnP(int _ballet_speed,
+ int _armor_type,
+ cv::Mat &_src_img,
+ std::vector _target_2d)
+ {
+ object_3d_ = initialize3DPoints(_armor_type);
+ target_2d_ = _target_2d;
+
+ cv::solvePnP(object_3d_,
+ target_2d_,
+ cameraMatrix_,
+ distCoeffs_,
+ rvec_,
+ tvec_,
+ false,
+ cv::SOLVEPNP_ITERATIVE);
+
+ drawCoordinate(_src_img, rvec_, tvec_, cameraMatrix_, distCoeffs_);
+
+ cv::Mat ptz = cameraPtz(tvec_);
+ cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
+
+ pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
+ pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
+ pnp_info_.depth = angle.z;
+
+ object_3d_.clear();
+ object_3d_.shrink_to_fit();
+ target_2d_.clear();
+ target_2d_.shrink_to_fit();
+ }
+
+ void PnP::solvePnP(int _ballet_speed,
+ int _armor_type,
+ cv::RotatedRect _rect,
+ int _depth)
+ {
+ object_3d_ = initialize3DPoints(_armor_type);
+ target_2d_ = initialize2DPoints(_rect);
+
+ cv::solvePnP(object_3d_,
+ target_2d_,
+ cameraMatrix_,
+ distCoeffs_,
+ rvec_,
+ tvec_,
+ false,
+ cv::SOLVEPNP_ITERATIVE);
+
+ cv::Mat ptz = cameraPtz(tvec_);
+ cv::Point3f angle = getAngle(ptz, _ballet_speed, 1, _depth);
+
+ pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
+ pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
+ pnp_info_.depth = angle.z;
+
+ object_3d_.clear();
+ object_3d_.shrink_to_fit();
+ target_2d_.clear();
+ target_2d_.shrink_to_fit();
+ }
+
+ void PnP::solvePnP(int _ballet_speed,
+ int _armor_type,
+ std::vector _target_2d,
+ int _depth)
+ {
+ object_3d_ = initialize3DPoints(_armor_type);
+ target_2d_ = _target_2d;
+
+ cv::solvePnP(object_3d_,
+ target_2d_,
+ cameraMatrix_,
+ distCoeffs_,
+ rvec_,
+ tvec_,
+ false,
+ cv::SOLVEPNP_ITERATIVE);
+
+ cv::Mat ptz = cameraPtz(tvec_);
+ cv::Point3f angle = getAngle(ptz, _ballet_speed, 1, _depth);
+
+ pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
+ pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
+ pnp_info_.depth = angle.z;
+
+ object_3d_.clear();
+ object_3d_.shrink_to_fit();
+ target_2d_.clear();
+ target_2d_.shrink_to_fit();
}
- fmt::print("[{}] Init PnP configuration finished\n", idntifier_green);
-}
-
-void PnP::solvePnP(int _ballet_speed,
- int _armor_type,
- cv::Mat& _src_img,
- cv::RotatedRect _rect) {
- object_3d_ = initialize3DPoints(_armor_type);
- target_2d_ = initialize2DPoints(_rect);
-
- cv::solvePnP(object_3d_,
- target_2d_,
- cameraMatrix_,
- distCoeffs_,
- rvec_,
- tvec_,
- false,
- cv::SOLVEPNP_ITERATIVE);
-
- drawCoordinate(_src_img, rvec_, tvec_, cameraMatrix_, distCoeffs_);
-
- cv::Mat ptz = cameraPtz(tvec_);
- cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
- // 固定偏移量
- pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
- pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
- pnp_info_.depth = angle.z;
-
- object_3d_.clear();
- object_3d_.shrink_to_fit();
- target_2d_.clear();
- target_2d_.shrink_to_fit();
-}
-
-void PnP::solvePnP(int _ballet_speed,
- int _armor_type,
- cv::Mat& _src_img,
- cv::Rect _rect) {
- object_3d_ = initialize3DPoints(_armor_type);
- target_2d_ = initialize2DPoints(_rect);
-
- cv::solvePnP(object_3d_,
- target_2d_,
- cameraMatrix_,
- distCoeffs_,
- rvec_,
- tvec_,
- false,
- cv::SOLVEPNP_ITERATIVE);
-
- drawCoordinate(_src_img, rvec_, tvec_, cameraMatrix_, distCoeffs_);
-
- cv::Mat ptz = cameraPtz(tvec_);
- cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
-
- pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
- pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
- pnp_info_.depth = angle.z;
-
- object_3d_.clear();
- object_3d_.shrink_to_fit();
- target_2d_.clear();
- target_2d_.shrink_to_fit();
-}
-
-void PnP::solvePnP(int _ballet_speed,
- int _width,
- int _height,
- cv::Mat& _src_img,
- cv::RotatedRect _rect) {
- object_3d_ = initialize3DPoints(_width, _height);
- target_2d_ = initialize2DPoints(_rect);
-
- cv::solvePnP(object_3d_,
- target_2d_,
- cameraMatrix_,
- distCoeffs_,
- rvec_,
- tvec_,
- false,
- cv::SOLVEPNP_ITERATIVE);
-
- drawCoordinate(_src_img, rvec_, tvec_, cameraMatrix_, distCoeffs_);
-
- cv::Mat ptz = cameraPtz(tvec_);
- cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
-
- pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
- pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
- pnp_info_.depth = angle.z;
-
- object_3d_.clear();
- object_3d_.shrink_to_fit();
- target_2d_.clear();
- target_2d_.shrink_to_fit();
-}
-
-void PnP::solvePnP(int _ballet_speed,
- int _width,
- int _height,
- cv::Mat& _src_img,
- cv::Rect _rect) {
- object_3d_ = initialize3DPoints(_width, _height);
- target_2d_ = initialize2DPoints(_rect);
-
- cv::solvePnP(object_3d_,
- target_2d_,
- cameraMatrix_,
- distCoeffs_,
- rvec_,
- tvec_,
- false,
- cv::SOLVEPNP_ITERATIVE);
-
- drawCoordinate(_src_img, rvec_, tvec_, cameraMatrix_, distCoeffs_);
-
- cv::Mat ptz = cameraPtz(tvec_);
- cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
-
- pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
- pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
- pnp_info_.depth = angle.z;
-
- object_3d_.clear();
- object_3d_.shrink_to_fit();
- target_2d_.clear();
- target_2d_.shrink_to_fit();
-}
-
-void PnP::solvePnP(int _ballet_speed,
- int _armor_type,
- cv::RotatedRect _rect) {
- object_3d_ = initialize3DPoints(_armor_type);
- target_2d_ = initialize2DPoints(_rect);
-
- cv::solvePnP(object_3d_,
- target_2d_,
- cameraMatrix_,
- distCoeffs_,
- rvec_,
- tvec_,
- false,
- cv::SOLVEPNP_ITERATIVE);
-
- cv::Mat ptz = cameraPtz(tvec_);
- cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
-
- pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
- pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
- pnp_info_.depth = angle.z;
-
- object_3d_.clear();
- object_3d_.shrink_to_fit();
- target_2d_.clear();
- target_2d_.shrink_to_fit();
-}
-
-void PnP::solvePnP(int _ballet_speed,
- int _armor_type,
- cv::Rect _rect) {
- object_3d_ = initialize3DPoints(_armor_type);
- target_2d_ = initialize2DPoints(_rect);
-
- cv::solvePnP(object_3d_,
- target_2d_,
- cameraMatrix_,
- distCoeffs_,
- rvec_,
- tvec_,
- false,
- cv::SOLVEPNP_ITERATIVE);
-
- cv::Mat ptz = cameraPtz(tvec_);
- cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
-
- pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
- pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
- pnp_info_.depth = angle.z;
-
- object_3d_.clear();
- object_3d_.shrink_to_fit();
- target_2d_.clear();
- target_2d_.shrink_to_fit();
-}
-
-void PnP::solvePnP(int _ballet_speed,
- int _width,
- int _height,
- cv::RotatedRect _rect) {
- object_3d_ = initialize3DPoints(_width, _height);
- target_2d_ = initialize2DPoints(_rect);
-
- cv::solvePnP(object_3d_,
- target_2d_,
- cameraMatrix_,
- distCoeffs_,
- rvec_,
- tvec_,
- false,
- cv::SOLVEPNP_ITERATIVE);
-
- cv::Mat ptz = cameraPtz(tvec_);
- cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
-
- pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
- pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
- pnp_info_.depth = angle.z;
-
- object_3d_.clear();
- object_3d_.shrink_to_fit();
- target_2d_.clear();
- target_2d_.shrink_to_fit();
-}
-
-void PnP::solvePnP(int _ballet_speed,
- int _width,
- int _height,
- cv::Rect _rect) {
- object_3d_ = initialize3DPoints(_width, _height);
- target_2d_ = initialize2DPoints(_rect);
-
- cv::solvePnP(object_3d_,
- target_2d_,
- cameraMatrix_,
- distCoeffs_,
- rvec_,
- tvec_,
- false,
- cv::SOLVEPNP_ITERATIVE);
-
- cv::Mat ptz = cameraPtz(tvec_);
- cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
-
- pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
- pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
- pnp_info_.depth = angle.z;
-
- object_3d_.clear();
- object_3d_.shrink_to_fit();
- target_2d_.clear();
- target_2d_.shrink_to_fit();
-}
-
-void PnP::solvePnP(int _ballet_speed,
- int _armor_type,
- std::vector _target_2d) {
- object_3d_ = initialize3DPoints(_armor_type);
- target_2d_ = _target_2d;
-
- cv::solvePnP(object_3d_,
- target_2d_,
- cameraMatrix_,
- distCoeffs_,
- rvec_,
- tvec_,
- false,
- cv::SOLVEPNP_ITERATIVE);
-
- cv::Mat ptz = cameraPtz(tvec_);
- cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
-
- pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
- pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
- pnp_info_.depth = angle.z;
-
- object_3d_.clear();
- object_3d_.shrink_to_fit();
- target_2d_.clear();
- target_2d_.shrink_to_fit();
-}
-
-void PnP::solvePnP(int _ballet_speed,
- int _armor_type,
- cv::Mat& _src_img,
- std::vector _target_2d) {
- object_3d_ = initialize3DPoints(_armor_type);
- target_2d_ = _target_2d;
-
- cv::solvePnP(object_3d_,
- target_2d_,
- cameraMatrix_,
- distCoeffs_,
- rvec_,
- tvec_,
- false,
- cv::SOLVEPNP_ITERATIVE);
-
- drawCoordinate(_src_img, rvec_, tvec_, cameraMatrix_, distCoeffs_);
-
- cv::Mat ptz = cameraPtz(tvec_);
- cv::Point3f angle = getAngle(ptz, _ballet_speed, 1);
-
- pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
- pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
- pnp_info_.depth = angle.z;
-
- object_3d_.clear();
- object_3d_.shrink_to_fit();
- target_2d_.clear();
- target_2d_.shrink_to_fit();
-}
-
-void PnP::solvePnP(int _ballet_speed,
- int _armor_type,
- cv::RotatedRect _rect,
- int _depth) {
- object_3d_ = initialize3DPoints(_armor_type);
- target_2d_ = initialize2DPoints(_rect);
-
- cv::solvePnP(object_3d_,
- target_2d_,
- cameraMatrix_,
- distCoeffs_,
- rvec_,
- tvec_,
- false,
- cv::SOLVEPNP_ITERATIVE);
-
- cv::Mat ptz = cameraPtz(tvec_);
- cv::Point3f angle = getAngle(ptz, _ballet_speed, 1, _depth);
-
- pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
- pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
- pnp_info_.depth = angle.z;
-
- object_3d_.clear();
- object_3d_.shrink_to_fit();
- target_2d_.clear();
- target_2d_.shrink_to_fit();
-}
-
-void PnP::solvePnP(int _ballet_speed,
- int _armor_type,
- std::vector _target_2d,
- int _depth) {
- object_3d_ = initialize3DPoints(_armor_type);
- target_2d_ = _target_2d;
-
- cv::solvePnP(object_3d_,
- target_2d_,
- cameraMatrix_,
- distCoeffs_,
- rvec_,
- tvec_,
- false,
- cv::SOLVEPNP_ITERATIVE);
-
- cv::Mat ptz = cameraPtz(tvec_);
- cv::Point3f angle = getAngle(ptz, _ballet_speed, 1, _depth);
-
- pnp_info_.pitch_angle = angle.y + pnp_config_.offset_armor_pitch;
- pnp_info_.yaw_angle = angle.x + pnp_config_.offset_armor_yaw;
- pnp_info_.depth = angle.z;
-
- object_3d_.clear();
- object_3d_.shrink_to_fit();
- target_2d_.clear();
- target_2d_.shrink_to_fit();
-}
-
-} // namespace basic_pnp
+} // namespace basic_pnp
diff --git a/module/angle_solve/basic_pnp.hpp b/module/angle_solve/basic_pnp.hpp
index 9d4d698..4072b6e 100644
--- a/module/angle_solve/basic_pnp.hpp
+++ b/module/angle_solve/basic_pnp.hpp
@@ -16,207 +16,211 @@
#include "abstract_pnp.hpp"
-namespace basic_pnp {
+namespace basic_pnp
+{
-auto idntifier_green = fmt::format(fg(fmt::color::green) | fmt::emphasis::bold, "basic_pnp");
-auto idntifier_red = fmt::format(fg(fmt::color::red) | fmt::emphasis::bold, "basic_pnp");
+ auto idntifier_green = fmt::format(fg(fmt::color::green) | fmt::emphasis::bold, "basic_pnp");
+ auto idntifier_red = fmt::format(fg(fmt::color::red) | fmt::emphasis::bold, "basic_pnp");
-struct PnP_Information {
- float yaw_angle;
- float pitch_angle;
- int depth;
+ struct PnP_Information
+ {
+ float yaw_angle;
+ float pitch_angle;
+ int depth;
- PnP_Information() {
- yaw_angle = 0.f;
- pitch_angle = 0.f;
- depth = 0;
- }
-};
+ PnP_Information()
+ {
+ yaw_angle = 0.f;
+ pitch_angle = 0.f;
+ depth = 0;
+ }
+ };
-class PnP : public abstract_pnp::PnP {
- public:
- PnP() = default;
- explicit PnP(std::string _camera_path, std::string _pnp_config_path);
+ class PnP : public abstract_pnp::PnP
+ {
+ public:
+ PnP() = default;
+ explicit PnP(std::string _camera_path, std::string _pnp_config_path);
- ~PnP() = default;
- /**
- * @brief 返回 Yaw 轴角度
- *
- * @return float
- */
- inline float returnYawAngle() { return pnp_info_.yaw_angle; }
- /**
- * @brief 返回 Pitch 轴角度
- *
- * @return float
- */
- inline float returnPitchAngle() { return pnp_info_.pitch_angle; }
- /**
- * @brief 返回深度
- *
- * @return int
- */
- inline int returnDepth() { return pnp_info_.depth; }
- /**
- * @brief 返回目标旋转向量
- *
- * @return double
- */
- inline double returnTvecTx() { return tvec_.ptr(0)[0]; }
- inline double returnTvecTy() { return tvec_.ptr(0)[1]; }
- inline double returnTvecTz() { return tvec_.ptr(0)[2]; }
- /**
- * @brief 角度结算
- *
- * @param _ballet_speed 子弹速度
- * @param _armor_type 装甲板类型
- * @param _src_img 原图（ CV_8UC3 ）
- * @param _rect 目标旋转矩形
- */
- void solvePnP(int _ballet_speed,
- int _armor_type,
- cv::Mat& _src_img,
- cv::RotatedRect _rect);
- /**
- * @brief 角度结算
- *
- * @param _ballet_speed 子弹速度
- * @param _armor_type 装甲板类型
- * @param _src_img 原图（ CV_8UC3 ）
- * @param _rect 目标外接矩形
- */
- void solvePnP(int _ballet_speed,
- int _armor_type,
- cv::Mat& _src_img,
- cv::Rect _rect);
- /**
- * @brief 角度结算
- *
- * @param _ballet_speed 子弹速度
- * @param _width 目标实际宽度
- * @param _height 目标实际高度
- * @param _src_img 原图（ CV_8UC3 ）
- * @param _rect 目标旋转矩形
- */
- void solvePnP(int _ballet_speed,
- int _width,
- int _height,
- cv::Mat& _src_img,
- cv::RotatedRect _rect);
- /**
- * @brief 角度结算
- *
- * @param _ballet_speed 子弹速度
- * @param _width 目标实际宽度
- * @param _height 目标实际高度
- * @param _src_img 原图（ CV_8UC3 ）
- * @param _rect 目标外接矩形
- */
- void solvePnP(int _ballet_speed,
- int _width,
- int _height,
- cv::Mat& _src_img,
- cv::Rect _rect);
- /**
- * @brief 角度结算
- *
- * @param _ballet_speed 子弹速度
- * @param _armor_type 装甲板类型
- * @param _rect 目标旋转矩形
- */
- void solvePnP(int _ballet_speed,
- int _armor_type,
- cv::RotatedRect _rect);
- /**
- * @brief 角度结算
- *
- * @param _ballet_speed 子弹速度
- * @param _width 目标实际宽度
- * @param _height 目标实际高度
- * @param _rect 目标外接矩形
- */
- void solvePnP(int _ballet_speed,
- int _width,
- int _height,
- cv::RotatedRect _rect);
- /**
- * @brief 角度结算
- *
- * @param _ballet_speed 子弹速度
- * @param _armor_type 装甲板类型
- * @param _rect 目标外接矩形
- */
- void solvePnP(int _ballet_speed,
- int _armor_type,
- cv::Rect _rect);
- /**
- * @brief 角度结算
- *
- * @param _ballet_speed 子弹速度
- * @param _width 目标实际宽度
- * @param _height 目标实际高度
- * @param _rect 目标外接矩形
- */
- void solvePnP(int _ballet_speed,
- int _width,
- int _height,
- cv::Rect _rect);
- /**
- * @brief 角度结算
- *
- * @param _ballet_speed 子弹速度
- * @param _armor_type 装甲板类型
- * @param _rect 目标旋转矩形
- * @param _depth 目标深度
- */
- void solvePnP(int _ballet_speed,
- int _armor_type,
- cv::RotatedRect _rect,
- int _depth);
- /**
- * @brief 角度结算
- *
- * @param _ballet_speed 子弹速度
- * @param _armor_type 装甲板类型
- * @param _target_2d 目标 2d 点坐标
- * @param _depth 目标深度
- */
- void solvePnP(int _ballet_speed,
- int _armor_type,
- std::vector _target_2d,
- int _depth);
- /**
- * @brief 角度结算
- *
- * @param _ballet_speed 子弹速度
- * @param _armor_type 装甲板类型
- * @param _src_img 原图（ CV_8UC3 ）
- * @param _target_2d 目标 2d 点坐标
- */
- void solvePnP(int _ballet_speed,
- int _armor_type,
- cv::Mat& _src_img,
- std::vector _target_2d);
- /**
- * @brief 角度结算
- *
- * @param _ballet_speed 子弹速度
- * @param _armor_type 装甲板类型
- * @param _target_2d 目标 2d 点坐标
- */
- void solvePnP(int _ballet_speed,
- int _armor_type,
- std::vector _target_2d);
+ ~PnP() = default;
+ /**
+ * @brief 返回 Yaw 轴角度
+ *
+ * @return float
+ */
+ inline float returnYawAngle() { return pnp_info_.yaw_angle; }
+ /**
+ * @brief 返回 Pitch 轴角度
+ *
+ * @return float
+ */
+ inline float returnPitchAngle() { return pnp_info_.pitch_angle; }
+ /**
+ * @brief 返回深度
+ *
+ * @return int
+ */
+ inline int returnDepth() { return pnp_info_.depth; }
+ /**
+ * @brief 返回目标旋转向量
+ *
+ * @return double
+ */
+ inline double returnTvecTx() { return tvec_.ptr(0)[0]; }
+ inline double returnTvecTy() { return tvec_.ptr(0)[1]; }
+ inline double returnTvecTz() { return tvec_.ptr(0)[2]; }
+ /**
+ * @brief 角度结算
+ *
+ * @param _ballet_speed 子弹速度
+ * @param _armor_type 装甲板类型
+ * @param _src_img 原图（ CV_8UC3 ）
+ * @param _rect 目标旋转矩形
+ */
+ void solvePnP(int _ballet_speed,
+ int _armor_type,
+ cv::Mat &_src_img,
+ cv::RotatedRect _rect);
+ /**
+ * @brief 角度结算
+ *
+ * @param _ballet_speed 子弹速度
+ * @param _armor_type 装甲板类型
+ * @param _src_img 原图（ CV_8UC3 ）
+ * @param _rect 目标外接矩形
+ */
+ void solvePnP(int _ballet_speed,
+ int _armor_type,
+ cv::Mat &_src_img,
+ cv::Rect _rect);
+ /**
+ * @brief 角度结算
+ *
+ * @param _ballet_speed 子弹速度
+ * @param _width 目标实际宽度
+ * @param _height 目标实际高度
+ * @param _src_img 原图（ CV_8UC3 ）
+ * @param _rect 目标旋转矩形
+ */
+ void solvePnP(int _ballet_speed,
+ int _width,
+ int _height,
+ cv::Mat &_src_img,
+ cv::RotatedRect _rect);
+ /**
+ * @brief 角度结算
+ *
+ * @param _ballet_speed 子弹速度
+ * @param _width 目标实际宽度
+ * @param _height 目标实际高度
+ * @param _src_img 原图（ CV_8UC3 ）
+ * @param _rect 目标外接矩形
+ */
+ void solvePnP(int _ballet_speed,
+ int _width,
+ int _height,
+ cv::Mat &_src_img,
+ cv::Rect _rect);
+ /**
+ * @brief 角度结算
+ *
+ * @param _ballet_speed 子弹速度
+ * @param _armor_type 装甲板类型
+ * @param _rect 目标旋转矩形
+ */
+ void solvePnP(int _ballet_speed,
+ int _armor_type,
+ cv::RotatedRect _rect);
+ /**
+ * @brief 角度结算
+ *
+ * @param _ballet_speed 子弹速度
+ * @param _width 目标实际宽度
+ * @param _height 目标实际高度
+ * @param _rect 目标外接矩形
+ */
+ void solvePnP(int _ballet_speed,
+ int _width,
+ int _height,
+ cv::RotatedRect _rect);
+ /**
+ * @brief 角度结算
+ *
+ * @param _ballet_speed 子弹速度
+ * @param _armor_type 装甲板类型
+ * @param _rect 目标外接矩形
+ */
+ void solvePnP(int _ballet_speed,
+ int _armor_type,
+ cv::Rect _rect);
+ /**
+ * @brief 角度结算
+ *
+ * @param _ballet_speed 子弹速度
+ * @param _width 目标实际宽度
+ * @param _height 目标实际高度
+ * @param _rect 目标外接矩形
+ */
+ void solvePnP(int _ballet_speed,
+ int _width,
+ int _height,
+ cv::Rect _rect);
+ /**
+ * @brief 角度结算
+ *
+ * @param _ballet_speed 子弹速度
+ * @param _armor_type 装甲板类型
+ * @param _rect 目标旋转矩形
+ * @param _depth 目标深度
+ */
+ void solvePnP(int _ballet_speed,
+ int _armor_type,
+ cv::RotatedRect _rect,
+ int _depth);
+ /**
+ * @brief 角度结算
+ *
+ * @param _ballet_speed 子弹速度
+ * @param _armor_type 装甲板类型
+ * @param _target_2d 目标 2d 点坐标
+ * @param _depth 目标深度
+ */
+ void solvePnP(int _ballet_speed,
+ int _armor_type,
+ std::vector _target_2d,
+ int _depth);
+ /**
+ * @brief 角度结算
+ *
+ * @param _ballet_speed 子弹速度
+ * @param _armor_type 装甲板类型
+ * @param _src_img 原图（ CV_8UC3 ）
+ * @param _target_2d 目标 2d 点坐标
+ */
+ void solvePnP(int _ballet_speed,
+ int _armor_type,
+ cv::Mat &_src_img,
+ std::vector _target_2d);
+ /**
+ * @brief 角度结算
+ *
+ * @param _ballet_speed 子弹速度
+ * @param _armor_type 装甲板类型
+ * @param _target_2d 目标 2d 点坐标
+ */
+ void solvePnP(int _ballet_speed,
+ int _armor_type,
+ std::vector _target_2d);
- private:
- PnP_Information pnp_info_;
+ private:
+ PnP_Information pnp_info_;
- cv::Mat cameraMatrix_, distCoeffs_;
- cv::Mat rvec_ = cv::Mat::zeros(3, 3, CV_64FC1);
- cv::Mat tvec_ = cv::Mat::zeros(3, 1, CV_64FC1);
+ cv::Mat cameraMatrix_, distCoeffs_;
+ cv::Mat rvec_ = cv::Mat::zeros(3, 3, CV_64FC1);
+ cv::Mat tvec_ = cv::Mat::zeros(3, 1, CV_64FC1);
- std::vector target_2d_;
- std::vector object_3d_;
-};
+ std::vector target_2d_;
+ std::vector object_3d_;
+ };
-} // namespace basic_pnp
+} // namespace basic_pnp
diff --git a/module/armor/basic_armor.cpp b/module/armor/basic_armor.cpp
index 5d484c8..82a981f 100755
--- a/module/armor/basic_armor.cpp
+++ b/module/armor/basic_armor.cpp
@@ -9,23 +9,25 @@
*/
#include "basic_armor.hpp"
#define RELEASE
-namespace basic_armor {
-
-Detector::Detector(const std::string _armor_config) {
- cv::FileStorage fs_armor(_armor_config, cv::FileStorage::READ);
- // 初始化基本参数
- fs_armor["GRAY_EDIT"] >> image_config_.gray_edit;
- fs_armor["COLOR_EDIT"] >> image_config_.color_edit;
- fs_armor["METHOD"] >> image_config_.method;
- fs_armor["BLUE_ARMOR_GRAY_TH"] >> image_config_.blue_armor_gray_th;
- fs_armor["RED_ARMOR_GRAY_TH"] >> image_config_.red_armor_gray_th;
-
- //if (image_config_.method == 0) {
- fs_armor["RED_ARMOR_COLOR_TH"] >> image_config_.red_armor_color_th;
- fs_armor["BLUE_ARMOR_COLOR_TH"] >> image_config_.blue_armor_color_th;
+namespace basic_armor
+{
+
+ Detector::Detector(const std::string _armor_config)
+ {
+ cv::FileStorage fs_armor(_armor_config, cv::FileStorage::READ);
+ // 初始化基本参数
+ fs_armor["GRAY_EDIT"] >> image_config_.gray_edit;
+ fs_armor["COLOR_EDIT"] >> image_config_.color_edit;
+ fs_armor["METHOD"] >> image_config_.method;
+ fs_armor["BLUE_ARMOR_GRAY_TH"] >> image_config_.blue_armor_gray_th;
+ fs_armor["RED_ARMOR_GRAY_TH"] >> image_config_.red_armor_gray_th;
+
+ // if (image_config_.method == 0) {
+ fs_armor["RED_ARMOR_COLOR_TH"] >> image_config_.red_armor_color_th;
+ fs_armor["BLUE_ARMOR_COLOR_TH"] >> image_config_.blue_armor_color_th;
fs_armor["GREEN_ARMOR_COLOR_TH"] >> image_config_.green_armor_color_th;
fs_armor["WHILE_ARMOR_COLOR_TH"] >> image_config_.while_armor_color_th;
- //} else {
+ //} else {
fs_armor["H_RED_MIN"] >> image_config_.h_red_min;
fs_armor["H_RED_MAX"] >> image_config_.h_red_max;
fs_armor["S_RED_MIN"] >> image_config_.s_red_min;
@@ -39,67 +41,69 @@ Detector::Detector(const std::string _armor_config) {
fs_armor["S_BLUE_MAX"] >> image_config_.s_blue_max;
fs_armor["V_BLUE_MIN"] >> image_config_.v_blue_min;
fs_armor["V_BLUE_MAX"] >> image_config_.v_blue_max;
- //}
+ //}
- fs_armor["LIGHT_DRAW"] >> light_config_.light_draw;
- fs_armor["LIGHT_EDTI"] >> light_config_.light_edit;
+ fs_armor["LIGHT_DRAW"] >> light_config_.light_draw;
+ fs_armor["LIGHT_EDTI"] >> light_config_.light_edit;
- fs_armor["LIGHT_RATIO_W_H_MIN"] >> light_config_.ratio_w_h_min;
- fs_armor["LIGHT_RATIO_W_H_MAX"] >> light_config_.ratio_w_h_max;
+ fs_armor["LIGHT_RATIO_W_H_MIN"] >> light_config_.ratio_w_h_min;
+ fs_armor["LIGHT_RATIO_W_H_MAX"] >> light_config_.ratio_w_h_max;
- fs_armor["LIGHT_ANGLE_MIN"] >> light_config_.angle_min;
- fs_armor["LIGHT_ANGLE_MAX"] >> light_config_.angle_max;
+ fs_armor["LIGHT_ANGLE_MIN"] >> light_config_.angle_min;
+ fs_armor["LIGHT_ANGLE_MAX"] >> light_config_.angle_max;
- fs_armor["LIGHT_PERIMETER_MIN"] >> light_config_.perimeter_min;
- fs_armor["LIGHT_PERIMETER_MAX"] >> light_config_.perimeter_max;
+ fs_armor["LIGHT_PERIMETER_MIN"] >> light_config_.perimeter_min;
+ fs_armor["LIGHT_PERIMETER_MAX"] >> light_config_.perimeter_max;
- fs_armor["ARMOR_EDIT"] >> armor_config_.armor_edit;
- fs_armor["ARMOR_DRAW"] >> armor_config_.armor_draw;
- fs_armor["ARMOR_FORECAST"] >> armor_config_.armor_forecast;
- fs_armor["ARMOR_HEIGHT_RATIO_MIN"] >> armor_config_.light_height_ratio_min;
- fs_armor["ARMOR_HEIGHT_RATIO_MAX"] >> armor_config_.light_height_ratio_max;
+ fs_armor["ARMOR_EDIT"] >> armor_config_.armor_edit;
+ fs_armor["ARMOR_DRAW"] >> armor_config_.armor_draw;
+ fs_armor["ARMOR_FORECAST"] >> armor_config_.armor_forecast;
+ fs_armor["ARMOR_HEIGHT_RATIO_MIN"] >> armor_config_.light_height_ratio_min;
+ fs_armor["ARMOR_HEIGHT_RATIO_MAX"] >> armor_config_.light_height_ratio_max;
- fs_armor["ARMOR_WIDTH_RATIO_MIN"] >> armor_config_.light_width_ratio_min;
- fs_armor["ARMOR_WIDTH_RATIO_MAX"] >> armor_config_.light_width_ratio_max;
+ fs_armor["ARMOR_WIDTH_RATIO_MIN"] >> armor_config_.light_width_ratio_min;
+ fs_armor["ARMOR_WIDTH_RATIO_MAX"] >> armor_config_.light_width_ratio_max;
- fs_armor["ARMOR_Y_DIFFERENT"] >> armor_config_.light_y_different;
- fs_armor["ARMOR_HEIGHT_DIFFERENT"] >> armor_config_.light_height_different;
- fs_armor["ARMOR_ANGLE_DIFFERENT"] >> armor_config_.armor_angle_different;
+ fs_armor["ARMOR_Y_DIFFERENT"] >> armor_config_.light_y_different;
+ fs_armor["ARMOR_HEIGHT_DIFFERENT"] >> armor_config_.light_height_different;
+ fs_armor["ARMOR_ANGLE_DIFFERENT"] >> armor_config_.armor_angle_different;
- fs_armor["ARMOR_SMALL_ASPECT_MIN"] >> armor_config_.small_armor_aspect_min;
- fs_armor["ARMOR_TYPE_TH"] >> armor_config_.armor_type_th;
- fs_armor["ARMOR_BIG_ASPECT_MAX"] >> armor_config_.big_armor_aspect_max;
- fs_armor.release();
+ fs_armor["ARMOR_SMALL_ASPECT_MIN"] >> armor_config_.small_armor_aspect_min;
+ fs_armor["ARMOR_TYPE_TH"] >> armor_config_.armor_type_th;
+ fs_armor["ARMOR_BIG_ASPECT_MAX"] >> armor_config_.big_armor_aspect_max;
+ fs_armor.release();
- fmt::print("[{}] Info, Armor configuration initial success\n", idntifier_green);
-}
+ fmt::print("[{}] Info, Armor configuration initial success\n", idntifier_green);
+ }
-float Detector::getDistance(const cv::Point a, const cv::Point b) {
- return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
-}
+ float Detector::getDistance(const cv::Point a, const cv::Point b)
+ {
+ return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
+ }
-void Detector::freeMemory(const std::string _armor_config) {
- cv::FileStorage fs_armor(_armor_config, cv::FileStorage::WRITE);
- lost_armor_success = armor_success;
- armor_success = false;
+ void Detector::freeMemory(const std::string _armor_config)
+ {
+ cv::FileStorage fs_armor(_armor_config, cv::FileStorage::WRITE);
+ lost_armor_success = armor_success;
+ armor_success = false;
- light_.clear();
- light_.shrink_to_fit();
- armor_.clear();
- armor_.shrink_to_fit();
+ light_.clear();
+ light_.shrink_to_fit();
+ armor_.clear();
+ armor_.shrink_to_fit();
- fs_armor << "GRAY_EDIT" << image_config_.gray_edit;
- fs_armor << "COLOR_EDIT" << image_config_.color_edit;
- fs_armor << "METHOD" << image_config_.method;
- fs_armor << "BLUE_ARMOR_GRAY_TH" << image_config_.blue_armor_gray_th;
- fs_armor << "RED_ARMOR_GRAY_TH" << image_config_.red_armor_gray_th;
+ fs_armor << "GRAY_EDIT" << image_config_.gray_edit;
+ fs_armor << "COLOR_EDIT" << image_config_.color_edit;
+ fs_armor << "METHOD" << image_config_.method;
+ fs_armor << "BLUE_ARMOR_GRAY_TH" << image_config_.blue_armor_gray_th;
+ fs_armor << "RED_ARMOR_GRAY_TH" << image_config_.red_armor_gray_th;
- //if (image_config_.method == 0) {
+ // if (image_config_.method == 0) {
fs_armor << "RED_ARMOR_COLOR_TH" << image_config_.red_armor_color_th;
fs_armor << "BLUE_ARMOR_COLOR_TH" << image_config_.blue_armor_color_th;
fs_armor << "GREEN_ARMOR_COLOR_TH" << image_config_.green_armor_color_th;
fs_armor << "WHILE_ARMOR_COLOR_TH" << image_config_.while_armor_color_th;
- //} else {
+ //} else {
fs_armor << "H_RED_MIN" << image_config_.h_red_min;
fs_armor << "H_RED_MAX" << image_config_.h_red_max;
fs_armor << "S_RED_MIN" << image_config_.s_red_min;
@@ -113,57 +117,60 @@ void Detector::freeMemory(const std::string _armor_config) {
fs_armor << "S_BLUE_MAX" << image_config_.s_blue_max;
fs_armor << "V_BLUE_MIN" << image_config_.v_blue_min;
fs_armor << "V_BLUE_MAX" << image_config_.v_blue_max;
- //}
-
- fs_armor << "LIGHT_EDTI" << light_config_.light_edit;
- fs_armor << "LIGHT_DRAW" << light_config_.light_draw;
-
- fs_armor << "LIGHT_RATIO_W_H_MIN" << light_config_.ratio_w_h_min;
- fs_armor << "LIGHT_RATIO_W_H_MAX" << light_config_.ratio_w_h_max;
-
- fs_armor << "LIGHT_ANGLE_MIN" << light_config_.angle_min;
- fs_armor << "LIGHT_ANGLE_MAX" << light_config_.angle_max;
-
- fs_armor << "LIGHT_PERIMETER_MIN" << light_config_.perimeter_min;
- fs_armor << "LIGHT_PERIMETER_MAX" << light_config_.perimeter_max;
-
- fs_armor << "ARMOR_FORECAST" << armor_config_.armor_forecast;
- fs_armor << "ARMOR_EDIT" << armor_config_.armor_edit;
- fs_armor << "ARMOR_DRAW" << armor_config_.armor_draw;
- fs_armor << "ARMOR_HEIGHT_RATIO_MIN" << armor_config_.light_height_ratio_min;
- fs_armor << "ARMOR_HEIGHT_RATIO_MAX" << armor_config_.light_height_ratio_max;
-
- fs_armor << "ARMOR_WIDTH_RATIO_MIN" << armor_config_.light_width_ratio_min;
- fs_armor << "ARMOR_WIDTH_RATIO_MAX" << armor_config_.light_width_ratio_max;
-
- fs_armor << "ARMOR_Y_DIFFERENT" << armor_config_.light_y_different;
- fs_armor << "ARMOR_HEIGHT_DIFFERENT" << armor_config_.light_height_different;
- fs_armor << "ARMOR_ANGLE_DIFFERENT" << armor_config_.armor_angle_different;
-
- fs_armor << "ARMOR_SMALL_ASPECT_MIN" << armor_config_.small_armor_aspect_min;
- fs_armor << "ARMOR_TYPE_TH" << armor_config_.armor_type_th;
- fs_armor << "ARMOR_BIG_ASPECT_MAX" << armor_config_.big_armor_aspect_max;
-
- fs_armor.release();
-}
-
-bool Detector::findLight() {
- int perimeter = 0;
- cv::RotatedRect box;
- std::vector> contours;
- cv::findContours(bin_color_img,
- contours,
- cv::RETR_EXTERNAL,
- cv::CHAIN_APPROX_NONE);
-
- if (contours.size() < 2) {
- //fmt::print("[{}] Info, quantity of contours less than 2\n", idntifier_green);
- return false;
+ //}
+
+ fs_armor << "LIGHT_EDTI" << light_config_.light_edit;
+ fs_armor << "LIGHT_DRAW" << light_config_.light_draw;
+
+ fs_armor << "LIGHT_RATIO_W_H_MIN" << light_config_.ratio_w_h_min;
+ fs_armor << "LIGHT_RATIO_W_H_MAX" << light_config_.ratio_w_h_max;
+
+ fs_armor << "LIGHT_ANGLE_MIN" << light_config_.angle_min;
+ fs_armor << "LIGHT_ANGLE_MAX" << light_config_.angle_max;
+
+ fs_armor << "LIGHT_PERIMETER_MIN" << light_config_.perimeter_min;
+ fs_armor << "LIGHT_PERIMETER_MAX" << light_config_.perimeter_max;
+
+ fs_armor << "ARMOR_FORECAST" << armor_config_.armor_forecast;
+ fs_armor << "ARMOR_EDIT" << armor_config_.armor_edit;
+ fs_armor << "ARMOR_DRAW" << armor_config_.armor_draw;
+ fs_armor << "ARMOR_HEIGHT_RATIO_MIN" << armor_config_.light_height_ratio_min;
+ fs_armor << "ARMOR_HEIGHT_RATIO_MAX" << armor_config_.light_height_ratio_max;
+
+ fs_armor << "ARMOR_WIDTH_RATIO_MIN" << armor_config_.light_width_ratio_min;
+ fs_armor << "ARMOR_WIDTH_RATIO_MAX" << armor_config_.light_width_ratio_max;
+
+ fs_armor << "ARMOR_Y_DIFFERENT" << armor_config_.light_y_different;
+ fs_armor << "ARMOR_HEIGHT_DIFFERENT" << armor_config_.light_height_different;
+ fs_armor << "ARMOR_ANGLE_DIFFERENT" << armor_config_.armor_angle_different;
+
+ fs_armor << "ARMOR_SMALL_ASPECT_MIN" << armor_config_.small_armor_aspect_min;
+ fs_armor << "ARMOR_TYPE_TH" << armor_config_.armor_type_th;
+ fs_armor << "ARMOR_BIG_ASPECT_MAX" << armor_config_.big_armor_aspect_max;
+
+ fs_armor.release();
}
- // 调参开关
- if (light_config_.light_edit == 1) {
+
+ bool Detector::findLight()
+ {
+ int perimeter = 0;
+ cv::RotatedRect box;
+ std::vector> contours;
+ cv::findContours(bin_color_img,
+ contours,
+ cv::RETR_EXTERNAL,
+ cv::CHAIN_APPROX_NONE);
+
+ if (contours.size() < 2)
+ {
+ // fmt::print("[{}] Info, quantity of contours less than 2\n", idntifier_green);
+ return false;
+ }
+ // 调参开关
+ if (light_config_.light_edit == 1)
+ {
#ifndef RELEASE
- // std::string window_name = {"[basic_armor] findLight() -> light_trackbar"};
+ // std::string window_name = {"[basic_armor] findLight() -> light_trackbar"};
/*
cv::namedWindow(window_name);
@@ -185,156 +192,183 @@ bool Detector::findLight() {
cv::imshow(window_name, light_trackbar_);
*/
#endif
- }
+ }
- for (size_t i = 0; i != contours.size(); ++i) {
- perimeter = arcLength(contours[i], true);
+ for (size_t i = 0; i != contours.size(); ++i)
+ {
+ perimeter = arcLength(contours[i], true);
- if (perimeter < light_config_.perimeter_min ||
- perimeter > light_config_.perimeter_max ||
- contours[i].size() < 5) {
- continue;
- }
+ if (perimeter < light_config_.perimeter_min ||
+ perimeter > light_config_.perimeter_max ||
+ contours[i].size() < 5)
+ {
+ continue;
+ }
- box = cv::fitEllipse(cv::Mat(contours[i]));
+ box = cv::fitEllipse(cv::Mat(contours[i]));
- if (box.angle > 90.0f) {
- box.angle = box.angle - 180.0f;
- }
+ if (box.angle > 90.0f)
+ {
+ box.angle = box.angle - 180.0f;
+ }
- static float _h = MAX(box.size.width, box.size.height);
- static float _w = MIN(box.size.width, box.size.height);
- static float light_w_h = _h / _w;
- // 判断灯条的条件
- if (box.angle < light_config_.angle_max &&
- box.angle > -light_config_.angle_min &&
- light_w_h < light_config_.ratio_w_h_max &&
- light_w_h > light_config_.ratio_w_h_min) {
- light_.emplace_back(box);
- if (light_config_.light_draw == 1 || light_config_.light_edit == 1) {
- cv::Point2f vertex[4];
- box.points(vertex);
-
- for (size_t l = 0; l != 4; ++l) {
- cv::line(draw_img_, vertex[l], vertex[(l + 1) % 4], cv::Scalar(0, 255, 255), 3, 8);
+ static float _h = MAX(box.size.width, box.size.height);
+ static float _w = MIN(box.size.width, box.size.height);
+ static float light_w_h = _h / _w;
+ // 判断灯条的条件
+ if (box.angle < light_config_.angle_max &&
+ box.angle > -light_config_.angle_min &&
+ light_w_h < light_config_.ratio_w_h_max &&
+ light_w_h > light_config_.ratio_w_h_min)
+ {
+ light_.emplace_back(box);
+ if (light_config_.light_draw == 1 || light_config_.light_edit == 1)
+ {
+ cv::Point2f vertex[4];
+ box.points(vertex);
+
+ for (size_t l = 0; l != 4; ++l)
+ {
+ cv::line(draw_img_, vertex[l], vertex[(l + 1) % 4], cv::Scalar(0, 255, 255), 3, 8);
+ }
}
}
}
- }
-
- if (light_.size() < 2) {
- //fmt::print("[{}] Info, quantity of light bar less than two\n", idntifier_green);
-
- return false;
- }
-
- return true;
-}
-
-bool Detector::runBasicArmor(const cv::Mat& _src_img,
- const uart::Receive_Data _receive_data) {
- // 预处理
- runImage(_src_img, _receive_data.my_color);
- draw_img_ = _src_img.clone();
- //cv::imshow("auto aim-armor", _src_img);
- if (findLight()) {
-
- if (fittingArmor()) {
- finalArmor();
- lost_cnt_ = 10;
- // 画图开关
- if (armor_config_.armor_draw == 1 ||
- light_config_.light_draw == 1 ||
- armor_config_.armor_edit == 1 ||
- light_config_.light_edit == 1) {
-#ifndef RELEASE
- cv::imshow("auto aim-armor", draw_img_);
-#endif
- draw_img_ = cv::Mat::zeros(_src_img.size(), CV_8UC3);
- }
+ if (light_.size() < 2)
+ {
+ // fmt::print("[{}] Info, quantity of light bar less than two\n", idntifier_green);
- return true;
+ return false;
}
- }
- if (armor_config_.armor_draw == 1 ||
- light_config_.light_draw == 1 ||
- armor_config_.armor_edit == 1 ||
- light_config_.light_edit == 1) {
-#ifndef RELEASE
- cv::imshow("auto aim-armor", draw_img_);
-#endif
- draw_img_ = cv::Mat::zeros(_src_img.size(), CV_8UC3);
+ return true;
}
- return false;
-}
-
-bool Detector::sentryMode(const cv::Mat& _src_img,
- const uart::Receive_Data _receive_data) {
- if (sentry_cnt_ == 0) {
- // 更新哨兵预测方向及预测量
- initialPredictionData(_receive_data.pitch,
- _receive_data.bullet_velocity,
- _receive_data.yaw);
+
+ bool Detector::runBasicArmor(const cv::Mat &_src_img,
+ const uart::Receive_Data _receive_data)
+ {
+ // 预处理
runImage(_src_img, _receive_data.my_color);
draw_img_ = _src_img.clone();
- if (findLight()) {
- if (fittingArmor()) {
+ // cv::imshow("auto aim-armor", _src_img);
+ if (findLight())
+ {
+
+ if (fittingArmor())
+ {
finalArmor();
lost_cnt_ = 10;
- // 限制最大预测位置
- if (abs(forecast_pixels_) >
- armor_[0].armor_rect.size.width * forecast_max_size_ * 0.1) {
- forecast_pixels_ =
- armor_[0].armor_rect.size.width * forecast_max_size_ * 0.1;
- }
- // 添加预测值
- if (filter_direction_ > 0.3) {
- armor_[0].armor_rect.center.x -= abs(forecast_pixels_);
- } else if (filter_direction_ < -0.3) {
- armor_[0].armor_rect.center.x += abs(forecast_pixels_);
- }
- last_armor_rect_ = armor_[0].armor_rect;
- cv::rectangle(draw_img_, armor_[0].armor_rect.boundingRect(),
- cv::Scalar(0, 255, 0), 3, 8);
- if (armor_config_.armor_draw == 1 || light_config_.light_draw == 1 ||
- armor_config_.armor_edit == 1 || light_config_.light_edit == 1) {
+ // 画图开关
+ if (armor_config_.armor_draw == 1 ||
+ light_config_.light_draw == 1 ||
+ armor_config_.armor_edit == 1 ||
+ light_config_.light_edit == 1)
+ {
#ifndef RELEASE
cv::imshow("auto aim-armor", draw_img_);
#endif
+
draw_img_ = cv::Mat::zeros(_src_img.size(), CV_8UC3);
}
+
return true;
}
}
if (armor_config_.armor_draw == 1 ||
light_config_.light_draw == 1 ||
armor_config_.armor_edit == 1 ||
- light_config_.light_edit == 1) {
-
+ light_config_.light_edit == 1)
+ {
#ifndef RELEASE
- cv::imshow("[basic_armor] getWriteData() -> draw_img_", draw_img_);
+ cv::imshow("auto aim-armor", draw_img_);
#endif
draw_img_ = cv::Mat::zeros(_src_img.size(), CV_8UC3);
}
return false;
- } else {
- // 初始化陀螺仪零点
- if (sentry_cnt_ < 5) {
- initial_gyroscope_ += _receive_data.yaw;
- initial_gyroscope_ *= 0.5;
+ }
+
+ bool Detector::sentryMode(const cv::Mat &_src_img,
+ const uart::Receive_Data _receive_data)
+ {
+ if (sentry_cnt_ == 0)
+ {
+ // 更新哨兵预测方向及预测量
+ initialPredictionData(_receive_data.pitch,
+ _receive_data.bullet_velocity,
+ _receive_data.yaw);
+ runImage(_src_img, _receive_data.my_color);
+ draw_img_ = _src_img.clone();
+ if (findLight())
+ {
+ if (fittingArmor())
+ {
+ finalArmor();
+ lost_cnt_ = 10;
+ // 限制最大预测位置
+ if (abs(forecast_pixels_) >
+ armor_[0].armor_rect.size.width * forecast_max_size_ * 0.1)
+ {
+ forecast_pixels_ =
+ armor_[0].armor_rect.size.width * forecast_max_size_ * 0.1;
+ }
+ // 添加预测值
+ if (filter_direction_ > 0.3)
+ {
+ armor_[0].armor_rect.center.x -= abs(forecast_pixels_);
+ }
+ else if (filter_direction_ < -0.3)
+ {
+ armor_[0].armor_rect.center.x += abs(forecast_pixels_);
+ }
+ last_armor_rect_ = armor_[0].armor_rect;
+ cv::rectangle(draw_img_, armor_[0].armor_rect.boundingRect(),
+ cv::Scalar(0, 255, 0), 3, 8);
+ if (armor_config_.armor_draw == 1 || light_config_.light_draw == 1 ||
+ armor_config_.armor_edit == 1 || light_config_.light_edit == 1)
+ {
+#ifndef RELEASE
+ cv::imshow("auto aim-armor", draw_img_);
+#endif
+ draw_img_ = cv::Mat::zeros(_src_img.size(), CV_8UC3);
+ }
+ return true;
+ }
+ }
+ if (armor_config_.armor_draw == 1 ||
+ light_config_.light_draw == 1 ||
+ armor_config_.armor_edit == 1 ||
+ light_config_.light_edit == 1)
+ {
+
+#ifndef RELEASE
+ cv::imshow("[basic_armor] getWriteData() -> draw_img_", draw_img_);
+#endif
+
+ draw_img_ = cv::Mat::zeros(_src_img.size(), CV_8UC3);
+ }
+ return false;
+ }
+ else
+ {
+ // 初始化陀螺仪零点
+ if (sentry_cnt_ < 5)
+ {
+ initial_gyroscope_ += _receive_data.yaw;
+ initial_gyroscope_ *= 0.5;
+ }
+ sentry_cnt_--;
+ return false;
}
- sentry_cnt_--;
- return false;
}
-}
-void Detector::initialPredictionData(const float _gyro_speed_data,
- const float _bullet_velocity,
- const float _yaw_angle) {
- if (armor_config_.armor_forecast) {
+ void Detector::initialPredictionData(const float _gyro_speed_data,
+ const float _bullet_velocity,
+ const float _yaw_angle)
+ {
+ if (armor_config_.armor_forecast)
+ {
#ifndef RELEASE
/*
std::string window_name = {"[basic_armor] sentryMode() -> sentry_trackbar"};
@@ -348,86 +382,104 @@ void Detector::initialPredictionData(const float _gyro_speed_data,
cv::createTrackbar("R", window_name, &R_, 100, NULL);
cv::imshow(window_name, sentry_trackbar_);*/
#endif
- }
-
- num_cnt_++;
- // 隔帧计算
- if (num_cnt_ % 2 == 0) {
- // 判断方向
- if (_gyro_speed_data > judge_direction_ * 0.01) {
- current_direction_ = 1;
- } else if (_gyro_speed_data < -judge_direction_ * 0.01) {
- current_direction_ = -1;
- } else {
- current_direction_ = 0;
- }
- // 延时滤波
- filter_direction_ = (1 - proportion_direction_ * 0.01) * last_direction_ + proportion_direction_ * 0.01 * current_direction_;
- last_direction_ = filter_direction_;
- // 计算偏差角度
- deviation_angle_ = _yaw_angle - initial_gyroscope_;
- if (last_deviation_angle_ != 0) {
- deviation_angle_ = (deviation_angle_ + last_deviation_angle_) * 0.5;
}
- last_deviation_angle_ = deviation_angle_;
- // 计算水平深度
- actual_z_ = sentry_dist_ / cos(deviation_angle_ * CV_PI / 180);
+ num_cnt_++;
+ // 隔帧计算
+ if (num_cnt_ % 2 == 0)
+ {
+ // 判断方向
+ if (_gyro_speed_data > judge_direction_ * 0.01)
+ {
+ current_direction_ = 1;
+ }
+ else if (_gyro_speed_data < -judge_direction_ * 0.01)
+ {
+ current_direction_ = -1;
+ }
+ else
+ {
+ current_direction_ = 0;
+ }
+ // 延时滤波
+ filter_direction_ = (1 - proportion_direction_ * 0.01) * last_direction_ + proportion_direction_ * 0.01 * current_direction_;
+ last_direction_ = filter_direction_;
+ // 计算偏差角度
+ deviation_angle_ = _yaw_angle - initial_gyroscope_;
+ if (last_deviation_angle_ != 0)
+ {
+ deviation_angle_ = (deviation_angle_ + last_deviation_angle_) * 0.5;
+ }
+ last_deviation_angle_ = deviation_angle_;
+
+ // 计算水平深度
+ actual_z_ = sentry_dist_ / cos(deviation_angle_ * CV_PI / 180);
- // 计算实际深度
- actual_depth_ = std::sqrt(actual_z_ * actual_z_ + sentry_height_ * sentry_height_);
+ // 计算实际深度
+ actual_depth_ = std::sqrt(actual_z_ * actual_z_ + sentry_height_ * sentry_height_);
- // 计算预测角度 角速度 * 时间
- forecast_angle_ = static_cast(actual_depth_ * 0.001 / _bullet_velocity) * _gyro_speed_data;
+ // 计算预测角度 角速度 * 时间
+ forecast_angle_ = static_cast(actual_depth_ * 0.001 / _bullet_velocity) * _gyro_speed_data;
- // 计算像素点个数
- forecast_pixels_ = abs(camera_focal_ * tan(forecast_angle_ * CV_PI / 180) * forecast_size_ * 100 / 4.8);
+ // 计算像素点个数
+ forecast_pixels_ = abs(camera_focal_ * tan(forecast_angle_ * CV_PI / 180) * forecast_size_ * 100 / 4.8);
- kalman_.setParam(R_, Q_, 1.0);
+ kalman_.setParam(R_, Q_, 1.0);
- forecast_pixels_ = (last_last_forecast_pixels_ + last_forecast_pixels_ + forecast_pixels_) * 0.333;
+ forecast_pixels_ = (last_last_forecast_pixels_ + last_forecast_pixels_ + forecast_pixels_) * 0.333;
- if (forecast_pixels_ - last_forecast_pixels_ > abrupt_variable_) {
- forecast_pixels_ = last_forecast_pixels_ + abrupt_variable_;
- } else if (forecast_pixels_ - last_forecast_pixels_ < -abrupt_variable_) {
- forecast_pixels_ = last_forecast_pixels_ - abrupt_variable_;
+ if (forecast_pixels_ - last_forecast_pixels_ > abrupt_variable_)
+ {
+ forecast_pixels_ = last_forecast_pixels_ + abrupt_variable_;
+ }
+ else if (forecast_pixels_ - last_forecast_pixels_ < -abrupt_variable_)
+ {
+ forecast_pixels_ = last_forecast_pixels_ - abrupt_variable_;
+ }
+ last_last_forecast_pixels_ = last_forecast_pixels_;
+ last_forecast_pixels_ = forecast_pixels_;
+ // forecast_pixels_ = kalman_.run(forecast_pixels_);
+ }
+ // 计数器归零
+ if (num_cnt_ % 10 == 0)
+ {
+ num_cnt_ = 0;
}
- last_last_forecast_pixels_ = last_forecast_pixels_;
- last_forecast_pixels_ = forecast_pixels_;
- // forecast_pixels_ = kalman_.run(forecast_pixels_);
- }
- // 计数器归零
- if (num_cnt_ % 10 == 0) {
- num_cnt_ = 0;
}
-}
-void Detector::finalArmor() {
- armor_success = true;
+ void Detector::finalArmor()
+ {
+ armor_success = true;
- if (armor_.size() == 1) {
- fmt::print("[{}] Info, only one armor\n", idntifier_green);
- } else {
- fmt::print("[{}] Info, multiple armors\n", idntifier_green);
- // 离图像中心点大小排序从小到大
- std::sort(armor_.begin(), armor_.end(),
- [](Armor_Data _a, Armor_Data _b) {
- return _a.distance_center < _b.distance_center;
- });
+ if (armor_.size() == 1)
+ {
+ fmt::print("[{}] Info, only one armor\n", idntifier_green);
+ }
+ else
+ {
+ fmt::print("[{}] Info, multiple armors\n", idntifier_green);
+ // 离图像中心点大小排序从小到大
+ std::sort(armor_.begin(), armor_.end(),
+ [](Armor_Data _a, Armor_Data _b)
+ {
+ return _a.distance_center < _b.distance_center;
+ });
- if (armor_config_.armor_draw == 1 ||
- armor_config_.armor_edit == 1) {
- cv::rectangle(draw_img_, armor_[0].armor_rect.boundingRect(),
- cv::Scalar(0, 255, 0), 3, 8);
- //cv::circle(draw_img_, armor_[0].armor_rect.center, 4, cv::Scalar(255,0,0));
-
+ if (armor_config_.armor_draw == 1 ||
+ armor_config_.armor_edit == 1)
+ {
+ cv::rectangle(draw_img_, armor_[0].armor_rect.boundingRect(),
+ cv::Scalar(0, 255, 0), 3, 8);
+ // cv::circle(draw_img_, armor_[0].armor_rect.center, 4, cv::Scalar(255,0,0));
+ }
}
+ fmt::print("[info] height {}\n", armor_[0].armor_rect.size.height);
}
- fmt::print("[info] height {}\n", armor_[0].armor_rect.size.height);
-}
-bool Detector::fittingArmor() {
- if (armor_config_.armor_edit == 1) {
+ bool Detector::fittingArmor()
+ {
+ if (armor_config_.armor_edit == 1)
+ {
#ifndef RELEASE
/*
std::string window_name = {"[basic_armor] fittingArmor() -> armor_trackbar"};
@@ -457,199 +509,224 @@ bool Detector::fittingArmor() {
cv::imshow(window_name, armor_trackbar_);
*/
#endif
- }
- for (size_t i = 0; i != light_.size(); ++i) {
- for (size_t j = i + 1; j != light_.size(); ++j) {
- int light_left = 0, light_right = 0;
- // 区分左右灯条
- if (light_[i].center.x > light_[j].center.x) {
- light_left = j;
- light_right = i;
- } else {
- light_left = i;
- light_right = j;
- }
+ }
+ for (size_t i = 0; i != light_.size(); ++i)
+ {
+ for (size_t j = i + 1; j != light_.size(); ++j)
+ {
+ int light_left = 0, light_right = 0;
+ // 区分左右灯条
+ if (light_[i].center.x > light_[j].center.x)
+ {
+ light_left = j;
+ light_right = i;
+ }
+ else
+ {
+ light_left = i;
+ light_right = j;
+ }
- armor_data_.left_light = light_[light_left];
- armor_data_.right_light = light_[light_right];
- // 装甲板倾斜弧度
- float error_angle =
- atan((light_[light_right].center.y - light_[light_left].center.y) /
- (light_[light_right].center.x - light_[light_left].center.x));
- if (error_angle < 10.f) {
- armor_data_.tan_angle = atan(error_angle) * 180 / CV_PI;
- // 拟合装甲板条件判断
- if (lightJudge(light_left, light_right)) {
- // 装甲板内颜色平均强度
- if (averageColor() < 30) {
- // 储存装甲板
- //cv::line(draw_img_, armor_data_.right_light, vertex[(l + 1) % 4], cv::Scalar(0, 255, 255), 3, 8);
- armor_.push_back(armor_data_);
- if (armor_config_.armor_draw == 1 ||
- armor_config_.armor_edit == 1) {
- rectangle(draw_img_, armor_data_.armor_rect.boundingRect(),
- cv::Scalar(255, 255, 0), 5, 8);
+ armor_data_.left_light = light_[light_left];
+ armor_data_.right_light = light_[light_right];
+ // 装甲板倾斜弧度
+ float error_angle =
+ atan((light_[light_right].center.y - light_[light_left].center.y) /
+ (light_[light_right].center.x - light_[light_left].center.x));
+ if (error_angle < 10.f)
+ {
+ armor_data_.tan_angle = atan(error_angle) * 180 / CV_PI;
+ // 拟合装甲板条件判断
+ if (lightJudge(light_left, light_right))
+ {
+ // 装甲板内颜色平均强度
+ if (averageColor() < 30)
+ {
+ // 储存装甲板
+ // cv::line(draw_img_, armor_data_.right_light, vertex[(l + 1) % 4], cv::Scalar(0, 255, 255), 3, 8);
+ armor_.push_back(armor_data_);
+ if (armor_config_.armor_draw == 1 ||
+ armor_config_.armor_edit == 1)
+ {
+ rectangle(draw_img_, armor_data_.armor_rect.boundingRect(),
+ cv::Scalar(255, 255, 0), 5, 8);
+ }
}
}
}
}
}
+ if (armor_.size() < 1)
+ {
+ // fmt::print("[{}] Info, armor not found\n", idntifier_green);
+ return false;
+ }
+ return true;
}
- if (armor_.size() < 1) {
- //fmt::print("[{}] Info, armor not found\n", idntifier_green);
- return false;
- }
- return true;
-}
-
-
-
-bool Detector::lightJudge(const int i, const int j) {
- armor_data_.left_light_height =
- MAX(light_[i].size.height, light_[i].size.width);
- armor_data_.left_light_width =
- MIN(light_[i].size.height, light_[i].size.width);
- armor_data_.right_light_height =
- MAX(light_[j].size.height, light_[j].size.width);
- armor_data_.right_light_width =
- MIN(light_[j].size.height, light_[j].size.width);
- armor_data_.light_height_aspect =
- armor_data_.left_light_height / armor_data_.right_light_height;
- armor_data_.light_width_aspect =
- armor_data_.left_light_width / armor_data_.right_light_width;
-
- /*fmt::print("[info] {}<{} {}>{} {}<{} {}>{}\n", armor_data_.light_height_aspect, armor_config_.light_height_ratio_max * 0.1,
- armor_data_.light_height_aspect, armor_config_.light_height_ratio_min * 0.1,
- armor_data_.light_width_aspect, armor_config_.light_width_ratio_max * 0.1,
- armor_data_.light_width_aspect, armor_config_.light_height_ratio_min * 0.1);*/
- // 左右灯条高宽比
- if (armor_data_.light_height_aspect < armor_config_.light_height_ratio_max * 0.1 &&
- armor_data_.light_height_aspect > armor_config_.light_height_ratio_min * 0.1 &&
- armor_data_.light_width_aspect < armor_config_.light_width_ratio_max * 0.1 &&
- armor_data_.light_width_aspect > armor_config_.light_height_ratio_min * 0.1) {
- armor_data_.height =
- MIN(armor_data_.left_light.size.height, armor_data_.right_light.size.height);
- // 灯条 y 轴位置差
- if (fabs(armor_data_.left_light.center.y - armor_data_.right_light.center.y) <
- armor_data_.height * armor_config_.light_y_different * 0.1) {
- // 灯条高度差
- if (fabs(armor_data_.left_light.size.height - armor_data_.right_light.size.height) <
- armor_data_.height * armor_config_.light_height_different * 0.1) {
- armor_data_.width =
- getDistance(armor_data_.left_light.center,
- armor_data_.right_light.center);
-
- armor_data_.aspect_ratio = armor_data_.width / (MAX(armor_data_.left_light.size.height, armor_data_.right_light.size.height));
- // 灯条角度差
- if (fabs(armor_data_.left_light.angle - armor_data_.right_light.angle) <
- armor_config_.armor_angle_different * 0.1) {
-
- cv::RotatedRect rects = cv::RotatedRect(
- (armor_data_.left_light.center + armor_data_.right_light.center) / 2,
- cv::Size(armor_data_.width * 0.5 , armor_data_.height * 0.5 + 100),
- armor_data_.tan_angle);
-
- //(armor_data_.left_light.angle + armor_data_.right_light.angle) / 2
-
- armor_data_.armor_rect = rects;
- // 装甲板保存灯条离中心点的距离
- armor_data_.distance_center =
- getDistance(armor_data_.armor_rect.center,
- cv::Point(draw_img_.cols, draw_img_.rows));
- // 小装甲板比例范围
- /*if (armor_data_.aspect_ratio > armor_config_.small_armor_aspect_min * 0.1 && armor_data_.aspect_ratio < armor_config_.armor_type_th * 0.1) {
- armor_data_.distinguish = 0;
- return true;
- // 大装甲板比例范围
- } else if (armor_data_.aspect_ratio > armor_config_.armor_type_th * 0.1 && armor_data_.aspect_ratio < armor_config_.big_armor_aspect_max * 0.1) {
- armor_data_.distinguish = 1;
+
+ bool Detector::lightJudge(const int i, const int j)
+ {
+ armor_data_.left_light_height =
+ MAX(light_[i].size.height, light_[i].size.width);
+ armor_data_.left_light_width =
+ MIN(light_[i].size.height, light_[i].size.width);
+ armor_data_.right_light_height =
+ MAX(light_[j].size.height, light_[j].size.width);
+ armor_data_.right_light_width =
+ MIN(light_[j].size.height, light_[j].size.width);
+ armor_data_.light_height_aspect =
+ armor_data_.left_light_height / armor_data_.right_light_height;
+ armor_data_.light_width_aspect =
+ armor_data_.left_light_width / armor_data_.right_light_width;
+
+ /*fmt::print("[info] {}<{} {}>{} {}<{} {}>{}\n", armor_data_.light_height_aspect, armor_config_.light_height_ratio_max * 0.1,
+ armor_data_.light_height_aspect, armor_config_.light_height_ratio_min * 0.1,
+ armor_data_.light_width_aspect, armor_config_.light_width_ratio_max * 0.1,
+ armor_data_.light_width_aspect, armor_config_.light_height_ratio_min * 0.1);*/
+ // 左右灯条高宽比
+ if (armor_data_.light_height_aspect < armor_config_.light_height_ratio_max * 0.1 &&
+ armor_data_.light_height_aspect > armor_config_.light_height_ratio_min * 0.1 &&
+ armor_data_.light_width_aspect < armor_config_.light_width_ratio_max * 0.1 &&
+ armor_data_.light_width_aspect > armor_config_.light_height_ratio_min * 0.1)
+ {
+ armor_data_.height =
+ MIN(armor_data_.left_light.size.height, armor_data_.right_light.size.height);
+ // 灯条 y 轴位置差
+ if (fabs(armor_data_.left_light.center.y - armor_data_.right_light.center.y) <
+ armor_data_.height * armor_config_.light_y_different * 0.1)
+ {
+ // 灯条高度差
+ if (fabs(armor_data_.left_light.size.height - armor_data_.right_light.size.height) <
+ armor_data_.height * armor_config_.light_height_different * 0.1)
+ {
+ armor_data_.width =
+ getDistance(armor_data_.left_light.center,
+ armor_data_.right_light.center);
+
+ armor_data_.aspect_ratio = armor_data_.width / (MAX(armor_data_.left_light.size.height, armor_data_.right_light.size.height));
+ // 灯条角度差
+ if (fabs(armor_data_.left_light.angle - armor_data_.right_light.angle) <
+ armor_config_.armor_angle_different * 0.1)
+ {
+
+ cv::RotatedRect rects = cv::RotatedRect(
+ (armor_data_.left_light.center + armor_data_.right_light.center) / 2,
+ cv::Size(armor_data_.width * 0.5, armor_data_.height * 0.5 + 100),
+ armor_data_.tan_angle);
+
+ //(armor_data_.left_light.angle + armor_data_.right_light.angle) / 2
+
+ armor_data_.armor_rect = rects;
+ // 装甲板保存灯条离中心点的距离
+ armor_data_.distance_center =
+ getDistance(armor_data_.armor_rect.center,
+ cv::Point(draw_img_.cols, draw_img_.rows));
+ // 小装甲板比例范围
+ /*if (armor_data_.aspect_ratio > armor_config_.small_armor_aspect_min * 0.1 && armor_data_.aspect_ratio < armor_config_.armor_type_th * 0.1) {
+ armor_data_.distinguish = 0;
+ return true;
+ // 大装甲板比例范围
+ } else if (armor_data_.aspect_ratio > armor_config_.armor_type_th * 0.1 && armor_data_.aspect_ratio < armor_config_.big_armor_aspect_max * 0.1) {
+ armor_data_.distinguish = 1;
+ return true;
+ }*/
return true;
- }*/
- return true;
+ }
}
}
}
- }
- return false;
-}
-int Detector::averageColor() {
- armor_data_.left_light_height =
- MAX(armor_data_.left_light_height, armor_data_.left_light_width);
- armor_data_.left_light_width =
- MIN(armor_data_.left_light_height, armor_data_.left_light_width);
- armor_data_.right_light_height =
- MAX(armor_data_.right_light_height, armor_data_.right_light_width);
- armor_data_.right_light_width =
- MIN(armor_data_.right_light_height, armor_data_.right_light_width);
-
- cv::RotatedRect rects =
- cv::RotatedRect((armor_data_.left_light.center + armor_data_.right_light.center) / 2,
- cv::Size(armor_data_.width - (armor_data_.left_light_width + armor_data_.right_light_width),
- ((armor_data_.left_light_height + armor_data_.right_light_height) / 2)),
- armor_data_.tan_angle);
- cv::rectangle(draw_img_, rects.boundingRect(), cv::Scalar(255, 0, 0), 3, 8);
-
- armor_data_.armor_rect = rects;
- cv::Rect _rect = rects.boundingRect();
- // ROI 安全条件
- if (_rect.x <= 0) {
- _rect.x = 0;
+ return false;
}
- if (_rect.y <= 0) {
- _rect.y = 0;
+ int Detector::averageColor()
+ {
+ armor_data_.left_light_height =
+ MAX(armor_data_.left_light_height, armor_data_.left_light_width);
+ armor_data_.left_light_width =
+ MIN(armor_data_.left_light_height, armor_data_.left_light_width);
+ armor_data_.right_light_height =
+ MAX(armor_data_.right_light_height, armor_data_.right_light_width);
+ armor_data_.right_light_width =
+ MIN(armor_data_.right_light_height, armor_data_.right_light_width);
+
+ cv::RotatedRect rects =
+ cv::RotatedRect((armor_data_.left_light.center + armor_data_.right_light.center) / 2,
+ cv::Size(armor_data_.width - (armor_data_.left_light_width + armor_data_.right_light_width),
+ ((armor_data_.left_light_height + armor_data_.right_light_height) / 2)),
+ armor_data_.tan_angle);
+ cv::rectangle(draw_img_, rects.boundingRect(), cv::Scalar(255, 0, 0), 3, 8);
+
+ armor_data_.armor_rect = rects;
+ cv::Rect _rect = rects.boundingRect();
+ // ROI 安全条件
+ if (_rect.x <= 0)
+ {
+ _rect.x = 0;
+ }
+ if (_rect.y <= 0)
+ {
+ _rect.y = 0;
+ }
+ if (_rect.y + _rect.height >= bin_gray_img.rows)
+ {
+ _rect.height = bin_gray_img.rows - _rect.y;
+ }
+ if (_rect.x + _rect.width >= bin_gray_img.cols)
+ {
+ _rect.width = bin_gray_img.cols - _rect.x;
+ }
+ // 计算颜色平均强度
+ static cv::Mat roi = bin_gray_img(_rect);
+ int average_intensity = static_cast(mean(roi).val[0]);
+
+ return average_intensity;
}
- if (_rect.y + _rect.height >= bin_gray_img.rows) {
- _rect.height = bin_gray_img.rows - _rect.y;
+
+ void Detector::runImage(const cv::Mat &_src_img, const int _my_color)
+ {
+ // 选择预处理类型（ BGR / HSV ）
+ switch (image_config_.method)
+ {
+ case 0:
+ bin_color_img = fuseImage(grayPretreat(_src_img, _my_color),
+ bgrPretreat(_src_img, _my_color), whilePretreat(_src_img));
+ break;
+ default:
+ bin_color_img = fuseImage(grayPretreat(_src_img, _my_color),
+ hsvPretreat(_src_img, _my_color), whilePretreat(_src_img));
+ break;
+ }
}
- if (_rect.x + _rect.width >= bin_gray_img.cols) {
- _rect.width = bin_gray_img.cols - _rect.x;
+
+ cv::Mat Detector::fuseImage(const cv::Mat _bin_gray_img, const cv::Mat _bin_color_img, const cv::Mat _while_img)
+ {
+ cv::bitwise_and(_bin_color_img, _bin_gray_img, _bin_color_img);
+ cv::bitwise_and(_bin_color_img, _while_img, _bin_color_img);
+ cv::morphologyEx(_bin_color_img, _bin_color_img, cv::MORPH_DILATE, ele_);
+ cv::medianBlur(_bin_color_img, _bin_color_img, 5);
+ return _bin_color_img;
}
- // 计算颜色平均强度
- static cv::Mat roi = bin_gray_img(_rect);
- int average_intensity = static_cast(mean(roi).val[0]);
-
- return average_intensity;
-}
-
-void Detector::runImage(const cv::Mat& _src_img, const int _my_color) {
- // 选择预处理类型（ BGR / HSV ）
- switch (image_config_.method) {
- case 0:
- bin_color_img = fuseImage(grayPretreat(_src_img, _my_color),
- bgrPretreat(_src_img, _my_color), whilePretreat(_src_img));
- break;
- default:
- bin_color_img = fuseImage(grayPretreat(_src_img, _my_color),
- hsvPretreat(_src_img, _my_color), whilePretreat(_src_img));
- break;
+
+ inline cv::Mat Detector::whilePretreat(const cv::Mat &_src_img)
+ {
+ cv::cvtColor(_src_img, gray_while_img_, cv::COLOR_BGR2GRAY);
+ cv::threshold(gray_while_img_, while_img_, image_config_.while_armor_color_th, 255, cv::THRESH_BINARY);
+ cv::bitwise_not(while_img_, while_img_);
+ return while_img_;
}
-}
-
-cv::Mat Detector::fuseImage(const cv::Mat _bin_gray_img, const cv::Mat _bin_color_img, const cv::Mat _while_img) {
- cv::bitwise_and(_bin_color_img, _bin_gray_img, _bin_color_img);
- cv::bitwise_and(_bin_color_img, _while_img, _bin_color_img);
- cv::morphologyEx(_bin_color_img, _bin_color_img, cv::MORPH_DILATE, ele_);
- cv::medianBlur(_bin_color_img, _bin_color_img, 5);
- return _bin_color_img;
-}
-
-inline cv::Mat Detector::whilePretreat(const cv::Mat& _src_img) {
- cv::cvtColor(_src_img, gray_while_img_, cv::COLOR_BGR2GRAY);
- cv::threshold(gray_while_img_, while_img_, image_config_.while_armor_color_th, 255, cv::THRESH_BINARY);
- cv::bitwise_not(while_img_, while_img_);
- return while_img_;
-}
-
-inline cv::Mat Detector::grayPretreat(const cv::Mat& _src_img,
- const int _my_color) {
- cv::cvtColor(_src_img, gray_img_, cv::COLOR_BGR2GRAY);
-
- //std::string window_name = {"[basic_armor] grayPretreat() -> gray_trackbar"};
- switch (_my_color) {
+
+ inline cv::Mat Detector::grayPretreat(const cv::Mat &_src_img,
+ const int _my_color)
+ {
+ cv::cvtColor(_src_img, gray_img_, cv::COLOR_BGR2GRAY);
+
+ // std::string window_name = {"[basic_armor] grayPretreat() -> gray_trackbar"};
+ switch (_my_color)
+ {
case uart::RED:
// my_color 为红色，则处理蓝色的情况
- if (image_config_.gray_edit) {
+ if (image_config_.gray_edit)
+ {
#ifndef RELEASE
/*
cv::namedWindow(window_name);
@@ -665,7 +742,8 @@ inline cv::Mat Detector::grayPretreat(const cv::Mat& _src_img,
break;
case uart::BLUE:
// my_color 为蓝色，则处理红色的情况
- if (image_config_.gray_edit) {
+ if (image_config_.gray_edit)
+ {
#ifndef RELEASE
/*
cv::namedWindow(window_name);
@@ -681,7 +759,8 @@ inline cv::Mat Detector::grayPretreat(const cv::Mat& _src_img,
break;
default:
// my_color 为默认值，则处理红蓝双色的情况
- if (image_config_.gray_edit) {
+ if (image_config_.gray_edit)
+ {
#ifndef RELEASE
/*
cv::namedWindow(window_name);
@@ -699,32 +778,36 @@ inline cv::Mat Detector::grayPretreat(const cv::Mat& _src_img,
image_config_.blue_armor_gray_th, 255, cv::THRESH_BINARY);
cv::bitwise_or(bin_red_gray_img, bin_blue_gray_img, bin_gray_img);
break;
- }
+ }
- if (image_config_.gray_edit) {
+ if (image_config_.gray_edit)
+ {
#ifndef RELEASE
- //cv::imshow(window_name, bin_gray_img);
+ // cv::imshow(window_name, bin_gray_img);
#endif
- }
+ }
- return bin_gray_img;
-}
+ return bin_gray_img;
+ }
-inline cv::Mat Detector::bgrPretreat(const cv::Mat& _src_img, const int _my_color) {
- static std::vector _split;
- static cv::Mat bin_color_img;
- static cv::Mat bin_color_green_img;
+ inline cv::Mat Detector::bgrPretreat(const cv::Mat &_src_img, const int _my_color)
+ {
+ static std::vector _split;
+ static cv::Mat bin_color_img;
+ static cv::Mat bin_color_green_img;
- cv::split(_src_img, _split);
+ cv::split(_src_img, _split);
- //std::string window_name = {"[basic_armor] brgPretreat() -> color_trackbar"};
- switch (_my_color) {
- case uart::RED:
- // my_color 为红色，则处理蓝色的情况
- cv::subtract(_split[0], _split[2], bin_color_img);
- cv::subtract(_split[0], _split[1], bin_color_green_img);
+ // std::string window_name = {"[basic_armor] brgPretreat() -> color_trackbar"};
+ switch (_my_color)
+ {
+ case uart::RED:
+ // my_color 为红色，则处理蓝色的情况
+ cv::subtract(_split[0], _split[2], bin_color_img);
+ cv::subtract(_split[0], _split[1], bin_color_green_img);
- if (image_config_.color_edit) {
+ if (image_config_.color_edit)
+ {
#ifndef RELEASE
/*
cv::namedWindow(window_name);
@@ -732,19 +815,20 @@ inline cv::Mat Detector::bgrPretreat(const cv::Mat& _src_img, const int _my_colo
&image_config_.blue_armor_color_th, 255, NULL);
cv::imshow(window_name, this->bgr_trackbar_);*/
#endif
- }
+ }
+
+ cv::threshold(bin_color_img, bin_color_img, image_config_.blue_armor_color_th, 255, cv::THRESH_BINARY);
+ cv::threshold(bin_color_green_img, bin_color_green_img, image_config_.green_armor_color_th, 255, cv::THRESH_BINARY);
+ cv::dilate(bin_color_green_img, bin_color_green_img, ele_);
+ cv::bitwise_and(bin_color_green_img, bin_color_img, bin_color_img);
+ break;
+ case uart::BLUE:
+ // my_color 为蓝色，则处理红色的情况
+ cv::subtract(_split[2], _split[0], bin_color_img);
+ cv::subtract(_split[2], _split[1], bin_color_green_img);
- cv::threshold(bin_color_img, bin_color_img, image_config_.blue_armor_color_th, 255, cv::THRESH_BINARY);
- cv::threshold(bin_color_green_img, bin_color_green_img, image_config_.green_armor_color_th, 255, cv::THRESH_BINARY);
- cv::dilate(bin_color_green_img, bin_color_green_img, ele_);
- cv::bitwise_and(bin_color_green_img, bin_color_img, bin_color_img);
- break;
- case uart::BLUE:
- // my_color 为蓝色，则处理红色的情况
- cv::subtract(_split[2], _split[0], bin_color_img);
- cv::subtract(_split[2], _split[1], bin_color_green_img);
-
- if (image_config_.color_edit) {
+ if (image_config_.color_edit)
+ {
#ifndef RELEASE
/*
cv::namedWindow(window_name);
@@ -752,21 +836,22 @@ inline cv::Mat Detector::bgrPretreat(const cv::Mat& _src_img, const int _my_colo
&image_config_.red_armor_color_th, 255, NULL);
cv::imshow(window_name, this->bgr_trackbar_);*/
#endif
- }
+ }
- cv::threshold(bin_color_img, bin_color_img, image_config_.red_armor_color_th, 255, cv::THRESH_BINARY);
- cv::threshold(bin_color_green_img, bin_color_green_img, image_config_.green_armor_color_th, 255, cv::THRESH_BINARY);
- cv::dilate(bin_color_green_img, bin_color_green_img, ele_);
- cv::bitwise_and(bin_color_green_img, bin_color_img, bin_color_img);
- break;
- default:
- // my_color 为默认值，则处理红蓝双色的情况
- cv::subtract(_split[2], _split[0], bin_red_color_img);
- cv::subtract(_split[0], _split[2], bin_blue_color_img);
- cv::subtract(_split[2], _split[1], bin_red_green_img);
- cv::subtract(_split[0], _split[1], bin_blue_green_img);
-
- if (image_config_.color_edit) {
+ cv::threshold(bin_color_img, bin_color_img, image_config_.red_armor_color_th, 255, cv::THRESH_BINARY);
+ cv::threshold(bin_color_green_img, bin_color_green_img, image_config_.green_armor_color_th, 255, cv::THRESH_BINARY);
+ cv::dilate(bin_color_green_img, bin_color_green_img, ele_);
+ cv::bitwise_and(bin_color_green_img, bin_color_img, bin_color_img);
+ break;
+ default:
+ // my_color 为默认值，则处理红蓝双色的情况
+ cv::subtract(_split[2], _split[0], bin_red_color_img);
+ cv::subtract(_split[0], _split[2], bin_blue_color_img);
+ cv::subtract(_split[2], _split[1], bin_red_green_img);
+ cv::subtract(_split[0], _split[1], bin_blue_green_img);
+
+ if (image_config_.color_edit)
+ {
#ifndef RELEASE
/*
cv::namedWindow(window_name);
@@ -776,34 +861,38 @@ inline cv::Mat Detector::bgrPretreat(const cv::Mat& _src_img, const int _my_colo
&image_config_.blue_armor_color_th, 255, NULL);
cv::imshow(window_name, this->bgr_trackbar_);*/
#endif
+ }
+ cv::threshold(bin_blue_color_img, bin_blue_color_img, image_config_.blue_armor_color_th, 255, cv::THRESH_BINARY);
+ cv::threshold(bin_blue_green_img, bin_blue_green_img, image_config_.green_armor_color_th, 255, cv::THRESH_BINARY);
+ cv::threshold(bin_red_color_img, bin_red_color_img, image_config_.red_armor_color_th, 255, cv::THRESH_BINARY);
+ cv::threshold(bin_red_green_img, bin_red_green_img, image_config_.green_armor_color_th, 255, cv::THRESH_BINARY);
+ cv::bitwise_or(bin_blue_color_img, bin_red_color_img, bin_color_img);
+ cv::bitwise_or(bin_blue_green_img, bin_red_green_img, bin_color_green_img);
+ cv::bitwise_and(bin_color_img, bin_color_green_img, bin_color_img);
+ break;
}
- cv::threshold(bin_blue_color_img, bin_blue_color_img, image_config_.blue_armor_color_th, 255, cv::THRESH_BINARY);
- cv::threshold(bin_blue_green_img, bin_blue_green_img, image_config_.green_armor_color_th, 255, cv::THRESH_BINARY);
- cv::threshold(bin_red_color_img, bin_red_color_img, image_config_.red_armor_color_th, 255, cv::THRESH_BINARY);
- cv::threshold(bin_red_green_img, bin_red_green_img, image_config_.green_armor_color_th, 255, cv::THRESH_BINARY);
- cv::bitwise_or(bin_blue_color_img, bin_red_color_img, bin_color_img);
- cv::bitwise_or(bin_blue_green_img, bin_red_green_img, bin_color_green_img);
- cv::bitwise_and(bin_color_img, bin_color_green_img, bin_color_img);
- break;
- }
- if (image_config_.color_edit) {
+ if (image_config_.color_edit)
+ {
#ifndef RELEASE
- //cv::imshow(window_name, bin_color_img);
+ // cv::imshow(window_name, bin_color_img);
#endif
- }
+ }
- return bin_color_img;
-}
+ return bin_color_img;
+ }
-inline cv::Mat Detector::hsvPretreat(const cv::Mat& _src_img,
- const int _my_color) {
- cv::cvtColor(_src_img, hsv_img, cv::COLOR_BGR2HSV_FULL);
- //std::string window_name = {"[basic_armor] hsvPretreat() -> hsv_trackbar"};
- switch (_my_color) {
+ inline cv::Mat Detector::hsvPretreat(const cv::Mat &_src_img,
+ const int _my_color)
+ {
+ cv::cvtColor(_src_img, hsv_img, cv::COLOR_BGR2HSV_FULL);
+ // std::string window_name = {"[basic_armor] hsvPretreat() -> hsv_trackbar"};
+ switch (_my_color)
+ {
// my_color 为红色，则处理蓝色的情况
case uart::RED:
- if (image_config_.color_edit) {
+ if (image_config_.color_edit)
+ {
#ifndef RELEASE
/*
cv::namedWindow(window_name);
@@ -834,7 +923,8 @@ inline cv::Mat Detector::hsvPretreat(const cv::Mat& _src_img,
break;
case uart::BLUE:
// my_color 为蓝色，则处理红色的情况
- if (image_config_.color_edit) {
+ if (image_config_.color_edit)
+ {
#ifndef RELEASE
/*
cv::namedWindow("hsv_trackbar");
@@ -865,7 +955,8 @@ inline cv::Mat Detector::hsvPretreat(const cv::Mat& _src_img,
break;
default:
// my_color 为默认值，则处理红蓝双色的情况
- if (image_config_.color_edit) {
+ if (image_config_.color_edit)
+ {
#ifndef RELEASE
/*
cv::namedWindow("hsv_trackbar");
@@ -919,15 +1010,16 @@ inline cv::Mat Detector::hsvPretreat(const cv::Mat& _src_img,
cv::bitwise_or(bin_blue_color_img, bin_red_color_img, bin_color_img);
break;
- }
+ }
- if (image_config_.gray_edit) {
+ if (image_config_.gray_edit)
+ {
#ifndef RELEASE
- //cv::imshow(window_name, bin_color_img);
+ // cv::imshow(window_name, bin_color_img);
#endif
- }
+ }
- return bin_color_img;
-}
+ return bin_color_img;
+ }
-} // namespace basic_armor
+} // namespace basic_armor
diff --git a/module/armor/fan_armor.cpp b/module/armor/fan_armor.cpp
index ce18287..da4795a 100644
--- a/module/armor/fan_armor.cpp
+++ b/module/armor/fan_armor.cpp
@@ -3,29 +3,32 @@
* @author dgsyrc (yrcminecraft@foxmail.com)
* @brief 扇叶装甲板类
* @date 2023-01-18
- *
+ *
* @copyright Copyright (c) 2023 Sirius
- *
+ *
*/
#include "fan_armor.hpp"
-namespace fan_armor {
-Detector::Detector() {}
+namespace fan_armor
+{
+ Detector::Detector() {}
-Detector::~Detector() {}
+ Detector::~Detector() {}
-void Detector::inputParams(const std::vector& _contours) { abstract_object::Object::inputParams(_contours); }
+ void Detector::inputParams(const std::vector &_contours) { abstract_object::Object::inputParams(_contours); }
-void Detector::displayFanArmor(cv::Mat& _img) {
- cv::Scalar color = cv::Scalar(0, 255, 0);
+ void Detector::displayFanArmor(cv::Mat &_img)
+ {
+ cv::Scalar color = cv::Scalar(0, 255, 0);
- cv::Point put_fan_armor = cv::Point(_img.cols - 100, 10);
- cv::putText(_img, " Fan Armor ", put_fan_armor, cv::FONT_HERSHEY_PLAIN, 1, color, 1, 8, false);
+ cv::Point put_fan_armor = cv::Point(_img.cols - 100, 10);
+ cv::putText(_img, " Fan Armor ", put_fan_armor, cv::FONT_HERSHEY_PLAIN, 1, color, 1, 8, false);
- for (int k = 0; k < 4; ++k) {
- cv::line(_img, vertex_[k], vertex_[(k + 1) % 4], color, 2);
+ for (int k = 0; k < 4; ++k)
+ {
+ cv::line(_img, vertex_[k], vertex_[(k + 1) % 4], color, 2);
+ }
}
-}
-} // namespace fan_armor
+} // namespace fan_armor
diff --git a/module/armor/fan_armor.hpp b/module/armor/fan_armor.hpp
index 2921e68..207411b 100644
--- a/module/armor/fan_armor.hpp
+++ b/module/armor/fan_armor.hpp
@@ -3,9 +3,9 @@
* @author dgsyrc (yrcminecraft@foxmail.com)
* @brief 扇叶装甲板类
* @date 2023-01-18
- *
+ *
* @copyright Copyright (c) 2023 Sirius
- *
+ *
*/
#pragma once
@@ -14,28 +14,30 @@
#include "module/buff/abstract_object.hpp"
-namespace fan_armor {
-class Detector : public abstract_object::Object {
- public:
- Detector();
- ~Detector();
-
- /**
- * @brief 输入参数
- * @param[in] _contours 输入轮廓点集
- */
- void inputParams(const std::vector& _contours);
-
- /**
- * @brief 显示内轮廓
- * @param[out] _img 输出图像
- * @note 绿色
- * @note 图例显示在右侧
- */
- void displayFanArmor(cv::Mat& _img);
-
- private:
- /* anything else */
-};
-
-} // namespace fan_armor
+namespace fan_armor
+{
+ class Detector : public abstract_object::Object
+ {
+ public:
+ Detector();
+ ~Detector();
+
+ /**
+ * @brief 输入参数
+ * @param[in] _contours 输入轮廓点集
+ */
+ void inputParams(const std::vector &_contours);
+
+ /**
+ * @brief 显示内轮廓
+ * @param[out] _img 输出图像
+ * @note 绿色
+ * @note 图例显示在右侧
+ */
+ void displayFanArmor(cv::Mat &_img);
+
+ private:
+ /* anything else */
+ };
+
+} // namespace fan_armor
diff --git a/module/buff/abstract_blade.hpp b/module/buff/abstract_blade.hpp
index e628d56..34f4d03 100644
--- a/module/buff/abstract_blade.hpp
+++ b/module/buff/abstract_blade.hpp
@@ -3,9 +3,9 @@
* @author dgsyrc (yrcminecraft@foxmail.com)
* @brief 扇叶目标类
* @date 2023-01-18
- *
+ *
* @copyright Copyright (c) 2023 Sirius
- *
+ *
*/
#pragma once
@@ -15,55 +15,59 @@
#include
#include "abstract_object.hpp"
-namespace abstract_blade {
-
-class FanBlade : public abstract_object::Object {
- public:
- FanBlade();
- ~FanBlade();
-
- /**
- * @brief 输入外轮廓参数
- * @param[in] _contours 外轮廓点集
- */
- void inputParams(const std::vector& _contours);
-
- /**
- * @brief 显示外轮廓
- * @param[out] _img 输出图像
- * @note 黄色
- * @note 图例显示在右侧
- */
- void displayFanBlade(cv::Mat& _img);
-
- /**
- * @brief 返回周长
- * @return int
- */
- int Length();
-
- private:
- // 周长
- int length_;
-};
-
-void FanBlade::inputParams(const std::vector& _contours) { abstract_object::Object::inputParams(_contours); }
-
-FanBlade::FanBlade() { length_ = 0; }
-
-FanBlade::~FanBlade() {}
-
-void FanBlade::displayFanBlade(cv::Mat& _img) {
- cv::Scalar color = cv::Scalar(0, 255, 255);
-
- cv::Point put_fan_blade = cv::Point(_img.cols - 100, 30);
- cv::putText(_img, " Fan Blade ", put_fan_blade, cv::FONT_HERSHEY_PLAIN, 1, color, 1, 8, false);
-
- for (int k = 0; k < 4; ++k) {
- cv::line(_img, vertex_[k], vertex_[(k + 1) % 4], color, 4);
+namespace abstract_blade
+{
+
+ class FanBlade : public abstract_object::Object
+ {
+ public:
+ FanBlade();
+ ~FanBlade();
+
+ /**
+ * @brief 输入外轮廓参数
+ * @param[in] _contours 外轮廓点集
+ */
+ void inputParams(const std::vector &_contours);
+
+ /**
+ * @brief 显示外轮廓
+ * @param[out] _img 输出图像
+ * @note 黄色
+ * @note 图例显示在右侧
+ */
+ void displayFanBlade(cv::Mat &_img);
+
+ /**
+ * @brief 返回周长
+ * @return int
+ */
+ int Length();
+
+ private:
+ // 周长
+ int length_;
+ };
+
+ void FanBlade::inputParams(const std::vector &_contours) { abstract_object::Object::inputParams(_contours); }
+
+ FanBlade::FanBlade() { length_ = 0; }
+
+ FanBlade::~FanBlade() {}
+
+ void FanBlade::displayFanBlade(cv::Mat &_img)
+ {
+ cv::Scalar color = cv::Scalar(0, 255, 255);
+
+ cv::Point put_fan_blade = cv::Point(_img.cols - 100, 30);
+ cv::putText(_img, " Fan Blade ", put_fan_blade, cv::FONT_HERSHEY_PLAIN, 1, color, 1, 8, false);
+
+ for (int k = 0; k < 4; ++k)
+ {
+ cv::line(_img, vertex_[k], vertex_[(k + 1) % 4], color, 4);
+ }
}
-}
-inline int FanBlade::Length() { return length_; }
+ inline int FanBlade::Length() { return length_; }
-} // namespace abstract_blade
+} // namespace abstract_blade
diff --git a/module/buff/abstract_center_r.hpp b/module/buff/abstract_center_r.hpp
index 60006e3..62b6019 100644
--- a/module/buff/abstract_center_r.hpp
+++ b/module/buff/abstract_center_r.hpp
@@ -3,9 +3,9 @@
* @author dgsyrc (yrcminecraft@foxmail.com)
* @brief 圆心 R 类
* @date 2023-01-18
- *
+ *
* @copyright Copyright (c) 2023 Sirius
- *
+ *
*/
#pragma once
@@ -15,34 +15,37 @@
#include "abstract_object.hpp"
-namespace abstract_center_r {
-class Center_R : public abstract_object::Object {
- public:
- /**
- * @brief 输入参数
- * @param[in] _contours 轮廓点集
- * @param[in] _roi_img 圆形区域ROI
- */
- void inputParams(const std::vector& _contours, const cv::Mat& _roi_img);
-
- /**
- * @brief 返回目标距离ROI图像中心点的距离
- * @return float
- */
- float centerDist();
-
- private:
- // 中心距离
- float center_distance;
-};
-
-void Center_R::inputParams(const std::vector& _contours, const cv::Mat& _roi_img) {
- abstract_object::Object::inputParams(_contours);
-
- // 中心距离
- this->center_distance = abstract_object::centerDistance(this->rect_.center, cv::Point(_roi_img.cols * 0.5, _roi_img.rows * 0.5));
-}
-
-inline float Center_R::centerDist() { return this->center_distance; }
-
-} // namespace abstract_center_r
+namespace abstract_center_r
+{
+ class Center_R : public abstract_object::Object
+ {
+ public:
+ /**
+ * @brief 输入参数
+ * @param[in] _contours 轮廓点集
+ * @param[in] _roi_img 圆形区域ROI
+ */
+ void inputParams(const std::vector &_contours, const cv::Mat &_roi_img);
+
+ /**
+ * @brief 返回目标距离ROI图像中心点的距离
+ * @return float
+ */
+ float centerDist();
+
+ private:
+ // 中心距离
+ float center_distance;
+ };
+
+ void Center_R::inputParams(const std::vector &_contours, const cv::Mat &_roi_img)
+ {
+ abstract_object::Object::inputParams(_contours);
+
+ // 中心距离
+ this->center_distance = abstract_object::centerDistance(this->rect_.center, cv::Point(_roi_img.cols * 0.5, _roi_img.rows * 0.5));
+ }
+
+ inline float Center_R::centerDist() { return this->center_distance; }
+
+} // namespace abstract_center_r
diff --git a/module/buff/abstract_object.hpp b/module/buff/abstract_object.hpp
index facd633..00d7693 100644
--- a/module/buff/abstract_object.hpp
+++ b/module/buff/abstract_object.hpp
@@ -3,9 +3,9 @@
* @author dgsyrc (yrcminecraft@foxmail.com)
* @brief 目标基类
* @date 2023-01-18
- *
+ *
* @copyright Copyright (c) 2023 Sirius
- *
+ *
*/
#pragma once
@@ -13,168 +13,186 @@
#include
#include
-namespace abstract_object {
-// 扇叶状态
-enum ObjectType { UNKNOW, INACTION, ACTION };
-
-// 目标类（基类）
-class Object {
- public:
- /**
- * @brief Construct a new Object object
- */
- Object() {
- rect_ = cv::RotatedRect();
- width_ = 0.f;
- height_ = 0.f;
- index_ = 0;
- angle_ = 0.f;
- aspect_ratio_ = 0.f;
- area_ = 0.f;
- rect_.points(vertex_);
+namespace abstract_object
+{
+ // 扇叶状态
+ enum ObjectType
+ {
+ UNKNOW,
+ INACTION,
+ ACTION
+ };
+
+ // 目标类（基类）
+ class Object
+ {
+ public:
+ /**
+ * @brief Construct a new Object object
+ */
+ Object()
+ {
+ rect_ = cv::RotatedRect();
+ width_ = 0.f;
+ height_ = 0.f;
+ index_ = 0;
+ angle_ = 0.f;
+ aspect_ratio_ = 0.f;
+ area_ = 0.f;
+ rect_.points(vertex_);
+ }
+
+ /**
+ * @brief Destroy the Object object
+ */
+ ~Object() = default;
+
+ /**
+ * @brief 返回矩形
+ * @return RotatedRect
+ */
+ cv::RotatedRect Rect();
+
+ /**
+ * @brief 返回宽度
+ * @return float
+ */
+ float Width();
+
+ /**
+ * @brief 返回高度
+ * @return float
+ */
+ float Height();
+
+ /**
+ * @brief 返回索引号
+ * @return size_t
+ */
+ size_t Index();
+
+ /**
+ * @brief 返回角度
+ * @return float
+ */
+ float Angle();
+
+ /**
+ * @brief 返回宽高比
+ * @return float
+ */
+ float aspectRatio();
+
+ /**
+ * @brief 返回轮廓面积
+ * @return float
+ */
+ float Area();
+
+ /**
+ * @brief 返回顶点点集
+ * @return Point2f
+ */
+ cv::Point2f *Vertex();
+
+ /**
+ * @brief 返回特定顶点
+ * @param _i 顶点编号
+ * @return Point2f
+ */
+ cv::Point2f Vertex(const int &_i);
+
+ protected:
+ /**
+ * @brief 输入参数
+ * @param[in] _contours 轮廓点集
+ */
+ void inputParams(const std::vector &_contours);
+
+ protected:
+ // 旋转矩形
+ cv::RotatedRect rect_;
+ // 宽
+ float width_;
+ // 高
+ float height_;
+ // 索引号
+ size_t index_;
+ // 角度
+ float angle_;
+ // 宽高比
+ float aspect_ratio_;
+ // 轮廓面积
+ float area_;
+ // 顶点点集
+ cv::Point2f vertex_[4];
+ };
+
+ void Object::inputParams(const std::vector &_contours)
+ {
+ // 矩形
+ this->rect_ = cv::fitEllipse(_contours);
+ // 长宽
+ if (this->rect_.size.width > this->rect_.size.height)
+ {
+ this->width_ = this->rect_.size.width;
+ this->height_ = this->rect_.size.height;
+ }
+ else
+ {
+ this->width_ = this->rect_.size.height;
+ this->height_ = this->rect_.size.width;
+ }
+ // 长宽比
+ if (this->height_ != 0)
+ {
+ this->aspect_ratio_ = this->width_ / this->height_;
+ }
+ else
+ {
+ this->aspect_ratio_ = 0.f;
+ }
+ // 顶点
+ this->rect_.points(this->vertex_);
+ // 角度
+ this->angle_ = this->rect_.angle;
+ // 面积
+ this->area_ = cv::contourArea(_contours);
}
- /**
- * @brief Destroy the Object object
- */
- ~Object() = default;
-
- /**
- * @brief 返回矩形
- * @return RotatedRect
- */
- cv::RotatedRect Rect();
-
- /**
- * @brief 返回宽度
- * @return float
- */
- float Width();
-
- /**
- * @brief 返回高度
- * @return float
- */
- float Height();
-
- /**
- * @brief 返回索引号
- * @return size_t
- */
- size_t Index();
-
- /**
- * @brief 返回角度
- * @return float
- */
- float Angle();
-
- /**
- * @brief 返回宽高比
- * @return float
- */
- float aspectRatio();
-
- /**
- * @brief 返回轮廓面积
- * @return float
- */
- float Area();
-
- /**
- * @brief 返回顶点点集
- * @return Point2f
- */
- cv::Point2f* Vertex();
-
- /**
- * @brief 返回特定顶点
- * @param _i 顶点编号
- * @return Point2f
- */
- cv::Point2f Vertex(const int& _i);
-
- protected:
- /**
- * @brief 输入参数
- * @param[in] _contours 轮廓点集
- */
- void inputParams(const std::vector& _contours);
-
- protected:
- // 旋转矩形
- cv::RotatedRect rect_;
- // 宽
- float width_;
- // 高
- float height_;
- // 索引号
- size_t index_;
- // 角度
- float angle_;
- // 宽高比
- float aspect_ratio_;
- // 轮廓面积
- float area_;
- // 顶点点集
- cv::Point2f vertex_[4];
-};
-
-void Object::inputParams(const std::vector& _contours) {
- // 矩形
- this->rect_ = cv::fitEllipse(_contours);
- // 长宽
- if (this->rect_.size.width > this->rect_.size.height) {
- this->width_ = this->rect_.size.width;
- this->height_ = this->rect_.size.height;
- } else {
- this->width_ = this->rect_.size.height;
- this->height_ = this->rect_.size.width;
- }
- // 长宽比
- if (this->height_ != 0) {
- this->aspect_ratio_ = this->width_ / this->height_;
- } else {
- this->aspect_ratio_ = 0.f;
- }
- // 顶点
- this->rect_.points(this->vertex_);
- // 角度
- this->angle_ = this->rect_.angle;
- // 面积
- this->area_ = cv::contourArea(_contours);
-}
-
-inline cv::RotatedRect Object::Rect() { return rect_; }
+ inline cv::RotatedRect Object::Rect() { return rect_; }
-inline float Object::Width() { return width_; }
+ inline float Object::Width() { return width_; }
-inline float Object::Height() { return height_; }
+ inline float Object::Height() { return height_; }
-inline size_t Object::Index() { return index_; }
+ inline size_t Object::Index() { return index_; }
-inline float Object::Angle() { return angle_; }
+ inline float Object::Angle() { return angle_; }
-inline float Object::aspectRatio() { return aspect_ratio_; }
+ inline float Object::aspectRatio() { return aspect_ratio_; }
-inline float Object::Area() { return area_; }
+ inline float Object::Area() { return area_; }
-inline cv::Point2f* Object::Vertex() { return vertex_; }
+ inline cv::Point2f *Object::Vertex() { return vertex_; }
-inline cv::Point2f Object::Vertex(const int& _i) { return vertex_[_i]; }
+ inline cv::Point2f Object::Vertex(const int &_i) { return vertex_[_i]; }
-float centerDistance(const cv::Point& p1, const cv::Point& p2) {
- float D = static_cast(sqrt(((p1.x - p2.x) * (p1.x - p2.x)) + ((p1.y - p2.y) * (p1.y - p2.y))));
- return D;
-}
+ float centerDistance(const cv::Point &p1, const cv::Point &p2)
+ {
+ float D = static_cast(sqrt(((p1.x - p2.x) * (p1.x - p2.x)) + ((p1.y - p2.y) * (p1.y - p2.y))));
+ return D;
+ }
-int getRectIntensity(const cv::Mat& frame, cv::Rect rect) {
- if (rect.width < 1 || rect.height < 1 || rect.x < 1 || rect.y < 1 || rect.width + rect.x > frame.cols || rect.height + rect.y > frame.rows) return 255;
- cv::Mat roi = frame(cv::Range(rect.y, rect.y + rect.height), cv::Range(rect.x, rect.x + rect.width));
- // imshow("roi ", roi);
- int average_intensity = static_cast(cv::mean(roi).val[0]);
- roi.release();
- return average_intensity;
-}
+ int getRectIntensity(const cv::Mat &frame, cv::Rect rect)
+ {
+ if (rect.width < 1 || rect.height < 1 || rect.x < 1 || rect.y < 1 || rect.width + rect.x > frame.cols || rect.height + rect.y > frame.rows)
+ return 255;
+ cv::Mat roi = frame(cv::Range(rect.y, rect.y + rect.height), cv::Range(rect.x, rect.x + rect.width));
+ // imshow("roi ", roi);
+ int average_intensity = static_cast(cv::mean(roi).val[0]);
+ roi.release();
+ return average_intensity;
+ }
-} // namespace abstract_object
+} // namespace abstract_object
diff --git a/module/buff/abstract_target.hpp b/module/buff/abstract_target.hpp
index 9479bde..69cabec 100644
--- a/module/buff/abstract_target.hpp
+++ b/module/buff/abstract_target.hpp
@@ -3,9 +3,9 @@
* @author dgsyrc (yrcminecraft@foxmail.com)
* @brief 能量机关检测
* @date 2023-01-15
- *
+ *
* @copyright Copyright (c) 2023 Sirius
- *
+ *
*/
#pragma once
@@ -18,10 +18,11 @@
#include "new_buff.hpp"
-
-namespace abstract_target {
- auto debug_info = fmt::format(fg(fmt::color::red) | fmt::emphasis::bold, "debug_info");
- class Target {
+namespace abstract_target
+{
+ auto debug_info = fmt::format(fg(fmt::color::red) | fmt::emphasis::bold, "debug_info");
+ class Target
+ {
public:
// 构造函数 析构函数
Target();
@@ -32,21 +33,21 @@ namespace abstract_target {
* @param[in] _fan_blade 外轮廓（扇叶）
* @param[in] _fan_armor 内轮廓（装甲板）
*/
- void inputParams(const abstract_blade::FanBlade& _fan_blade, const fan_armor::Detector& _fan_armor);
+ void inputParams(const abstract_blade::FanBlade &_fan_blade, const fan_armor::Detector &_fan_armor);
/**
* @brief 更新装甲板的四个顶点编号
* @param[in] _img 输入绘制图像
* @note 将各个顶点绘制在输入图像上
*/
- void updateVertex(cv::Mat& _img);
+ void updateVertex(cv::Mat &_img);
/**
* @brief 设置运转类型
* @details 强制
* @param[in] _type 类型
*/
- void setType(const abstract_object::ObjectType& _type);
+ void setType(const abstract_object::ObjectType &_type);
void setType(const new_buff::Check_Moudle &check_moudle);
@@ -58,7 +59,7 @@ namespace abstract_target {
* @param[in] _img 输入绘制图像
* @note 装甲板为绿框，扇叶为黄框
*/
- void displayInactionTarget(cv::Mat& _img);
+ void displayInactionTarget(cv::Mat &_img);
/**
* @brief 返回扇叶大轮廓
@@ -109,7 +110,7 @@ namespace abstract_target {
* @param _i 顶点编号
* @return Point2f
*/
- cv::Point2f vector2DPoint(const int& _i);
+ cv::Point2f vector2DPoint(const int &_i);
/**
* @brief 返回装甲板高度点差
@@ -136,16 +137,15 @@ namespace abstract_target {
std::vector points_2d_;
// 装甲板高度点差
cv::Point2f delta_height_point_;
-
-
};
- Target::Target() {
- fan_blade_ = abstract_blade::FanBlade();
- fan_armor_ = fan_armor::Detector();
- angle_ = 0.f;
+ Target::Target()
+ {
+ fan_blade_ = abstract_blade::FanBlade();
+ fan_armor_ = fan_armor::Detector();
+ angle_ = 0.f;
diff_angle_ = 0.f;
- type_ = abstract_object::UNKNOW;
+ type_ = abstract_object::UNKNOW;
area_ratio_ = 0.f;
points_2d_.reserve(4);
delta_height_point_ = cv::Point2f(0.f, 0.f);
@@ -153,7 +153,8 @@ namespace abstract_target {
Target::~Target() {}
- void Target::inputParams(const abstract_blade::FanBlade& _fan_blade, const fan_armor::Detector& _fan_armor) {
+ void Target::inputParams(const abstract_blade::FanBlade &_fan_blade, const fan_armor::Detector &_fan_armor)
+ {
// 内轮廓
fan_armor_ = _fan_armor;
@@ -166,7 +167,8 @@ namespace abstract_target {
// 扇叶角度= 扇叶和装甲板的连线 TODO:未使用
angle_ = atan2((fan_armor_.Rect().center.y - fan_blade_.Rect().center.y), (fan_armor_.Rect().center.x - fan_blade_.Rect().center.x)) * 180 / static_cast(CV_PI);
// 过零处理
- if (angle_ < 0.f) {
+ if (angle_ < 0.f)
+ {
angle_ += 360.f;
}
@@ -180,23 +182,27 @@ namespace abstract_target {
// area_ratio_ = fan_armor_.Area() / fan_blade_.Area();
}
- void Target::updateVertex(cv::Mat& _img) {
+ void Target::updateVertex(cv::Mat &_img)
+ {
points_2d_.clear();
- cv::Point2f point_up_center = (fan_armor_.Vertex(0) + fan_armor_.Vertex(1)) * 0.5;
+ cv::Point2f point_up_center = (fan_armor_.Vertex(0) + fan_armor_.Vertex(1)) * 0.5;
cv::Point2f point_down_center = (fan_armor_.Vertex(2) + fan_armor_.Vertex(3)) * 0.5;
- float up_distance = abstract_object::centerDistance(point_up_center, fan_blade_.Rect().center);
+ float up_distance = abstract_object::centerDistance(point_up_center, fan_blade_.Rect().center);
float down_distance = abstract_object::centerDistance(point_down_center, fan_blade_.Rect().center);
- if (up_distance > down_distance) {
+ if (up_distance > down_distance)
+ {
// 0 1
// 3 2
points_2d_.emplace_back(fan_armor_.Vertex(0));
points_2d_.emplace_back(fan_armor_.Vertex(1));
points_2d_.emplace_back(fan_armor_.Vertex(2));
points_2d_.emplace_back(fan_armor_.Vertex(3));
- } else {
+ }
+ else
+ {
// 2 3
// 1 0
points_2d_.emplace_back(fan_armor_.Vertex(2));
@@ -212,34 +218,38 @@ namespace abstract_target {
cv::circle(_img, points_2d_[3], 2, cv::Scalar(0, 255, 0), -1);
}
- void Target::setType(const abstract_object::ObjectType& _type) { type_ = _type; }
+ void Target::setType(const abstract_object::ObjectType &_type) { type_ = _type; }
- void Target::setType(const new_buff::Check_Moudle& check_moudle) {
+ void Target::setType(const new_buff::Check_Moudle &check_moudle)
+ {
// 上层中心点 和 下层中心点
- cv::Point2f point_up_center = (fan_armor_.Vertex(0) + fan_armor_.Vertex(1)) * 0.5;
+ cv::Point2f point_up_center = (fan_armor_.Vertex(0) + fan_armor_.Vertex(1)) * 0.5;
cv::Point2f point_down_center = (fan_armor_.Vertex(2) + fan_armor_.Vertex(3)) * 0.5;
// 上层离外轮廓中点距离
// 下册离外轮廓中点距离
// TODO(fqjun) ：待修改为优化方案，不用重复判断方向
- float up_distance = abstract_object::centerDistance(point_up_center, fan_blade_.Rect().center);
+ float up_distance = abstract_object::centerDistance(point_up_center, fan_blade_.Rect().center);
float down_distance = abstract_object::centerDistance(point_down_center, fan_blade_.Rect().center);
// 计算装甲板的高度差，且方向朝向圆心
cv::Point2f vector_height;
- if (up_distance > down_distance) {
+ if (up_distance > down_distance)
+ {
vector_height = fan_armor_.Vertex(0) - fan_armor_.Vertex(3);
- } else {
+ }
+ else
+ {
vector_height = fan_armor_.Vertex(3) - fan_armor_.Vertex(0);
}
// 这里记得分清楚应该是在哪个点往哪个方向移动多少距离进行框取小ROI
// 计算两个小roi的中心点
- cv::Point left_center = vector2DPoint(3) - vector_height;
+ cv::Point left_center = vector2DPoint(3) - vector_height;
cv::Point right_center = vector2DPoint(2) - vector_height;
// 创建roi并绘制
- int width = 10;
+ int width = 10;
int height = 5;
cv::Point left1 = cv::Point(left_center.x - width, left_center.y - height);
@@ -248,12 +258,12 @@ namespace abstract_target {
cv::Point right1 = cv::Point(right_center.x - width, right_center.y - height);
cv::Point right2 = cv::Point(right_center.x + width, right_center.y + height);
- cv::Rect left_rect(left1, left2); // 画出左边小roi
- cv::Rect right_rect(right1, right2); // 画出右边小roi
+ cv::Rect left_rect(left1, left2); // 画出左边小roi
+ cv::Rect right_rect(right1, right2); // 画出右边小roi
// 计算光线强度
- //int left_intensity = abstract_object::getRectIntensity(_bin_img, left_rect);
- //int right_intensity = abstract_object::getRectIntensity(_bin_img, right_rect);
+ // int left_intensity = abstract_object::getRectIntensity(_bin_img, left_rect);
+ // int right_intensity = abstract_object::getRectIntensity(_bin_img, right_rect);
/*
fmt::print("[{}] left_intensity:{} right_intensity:{}\n", debug_info, left_intensity, right_intensity);
if (left_intensity <= 110 && right_intensity <= 110) { //15 15
@@ -264,23 +274,25 @@ namespace abstract_target {
*/
switch (check_moudle)
{
- case new_buff::ACTION_MODE:
- type_ = abstract_object::ACTION;
- break;
- case new_buff::INACTION_MODE:
- type_ = abstract_object::INACTION;
- new_buff::armor_last = new_buff::armor_now;
- new_buff::armor_now = fan_armor_.Rect().center;
- break;
- default:
- break;
+ case new_buff::ACTION_MODE:
+ type_ = abstract_object::ACTION;
+ break;
+ case new_buff::INACTION_MODE:
+ type_ = abstract_object::INACTION;
+ new_buff::armor_last = new_buff::armor_now;
+ new_buff::armor_now = fan_armor_.Rect().center;
+ break;
+ default:
+ break;
}
-
}
- void Target::displayInactionTarget(cv::Mat& _img) {
- if (type_ == abstract_object::INACTION) {
- for (int k = 0; k < 4; ++k) {
+ void Target::displayInactionTarget(cv::Mat &_img)
+ {
+ if (type_ == abstract_object::INACTION)
+ {
+ for (int k = 0; k < 4; ++k)
+ {
cv::line(_img, fan_armor_.Vertex(k), fan_armor_.Vertex((k + 1) % 4), cv::Scalar(0, 255, 0), 2);
cv::line(_img, fan_blade_.Vertex(k), fan_blade_.Vertex((k + 1) % 4), cv::Scalar(0, 255, 255), 2);
}
@@ -301,8 +313,8 @@ namespace abstract_target {
inline std::vector Target::vector2DPoint() { return points_2d_; }
- inline cv::Point2f Target::vector2DPoint(const int& _i) { return points_2d_[_i]; }
+ inline cv::Point2f Target::vector2DPoint(const int &_i) { return points_2d_[_i]; }
inline cv::Point2f Target::deltaPoint() { return points_2d_[0] - points_2d_[3]; }
-} // namespace abstract_target
+} // namespace abstract_target
diff --git a/module/buff/basic_buff.hpp b/module/buff/basic_buff.hpp
index 9dbbae1..c1a9189 100644
--- a/module/buff/basic_buff.hpp
+++ b/module/buff/basic_buff.hpp
@@ -3,9 +3,9 @@
* @author dgsyrc (yrcminecraft@foxmail.com)
* @brief 能量机关
* @date 2023-01-15
- *
+ *
* @copyright Copyright (c) 2023 Sirius
- *
+ *
*/
#pragma once
@@ -33,491 +33,489 @@ double fps::FPS::last_time;
// #define DEBUG_STATIC
// #define DEBUG_BARREL_OFFSET
-namespace basic_buff {
-
-
-
-auto idntifier_green = fmt::format(fg(fmt::color::green) | fmt::emphasis::bold, "basic_buff");
-auto idntifier_red = fmt::format(fg(fmt::color::red) | fmt::emphasis::bold, "basic_buff");
-auto idntifier_yellow = fmt::format(fg(fmt::color::yellow) | fmt::emphasis::bold, "basic_buff");
-auto process_yellow = fmt::format(fg(fmt::color::yellow) | fmt::emphasis::bold, "processing");
-auto target_yellow = fmt::format(fg(fmt::color::yellow) | fmt::emphasis::bold, "find_target");
-auto center_yellow = fmt::format(fg(fmt::color::yellow) | fmt::emphasis::bold, "center_r");
-auto judgement_yellow = fmt::format(fg(fmt::color::yellow) | fmt::emphasis::bold, "judgement");
-auto predict_yellow = fmt::format(fg(fmt::color::yellow) | fmt::emphasis::bold, "predict");
-auto final_target_yellow = fmt::format(fg(fmt::color::yellow) | fmt::emphasis::bold, "final_target");
-auto debug_info = fmt::format(fg(fmt::color::red) | fmt::emphasis::bold, "debug_info");
-auto debug_info_2 = fmt::format(fg(fmt::color::green) | fmt::emphasis::bold, "debug_info");
-
-cv::Scalar Color_Draw_For_Edge = CV_RGB(255,0,0);
-
-new_buff::buff big_buff;
-
-struct Buff_Param {
- // BGR
- int RED_BUFF_GRAY_TH;
- int RED_BUFF_COLOR_TH;
- int BLUE_BUFF_GRAY_TH;
- int BLUE_BUFF_COLOR_TH;
-
- // HSV-red
- int H_RED_MAX;
- int H_RED_MIN;
- int S_RED_MAX;
- int S_RED_MIN;
- int V_RED_MAX;
- int V_RED_MIN;
-
- // HSV-blue
- int H_BLUE_MAX;
- int H_BLUE_MIN;
- int S_BLUE_MAX;
- int S_BLUE_MIN;
- int V_BLUE_MAX;
- int V_BLUE_MIN;
-
- // 筛选条件
- // 面积 MAX MIN 大 小
- int SMALL_TARGET_AREA_MAX;
- int SMALL_TARGET_AREA_MIN;
- int BIG_TARGET_AREA_MAX;
- int BIG_TARGET_AREA_MIN;
- // 周长 MAX MIN
- int SMALL_TARGET_Length_MIN;
- int SMALL_TARGET_Length_MAX;
- int BIG_TARGET_Length_MIN;
- int BIG_TARGET_Length_MAX;
- // 角度差 MAX MIN
- int DIFF_ANGLE_MAX;
- int DIFF_ANGLE_MIN;
- // 长宽比 MAX MIN
- float SMALL_TARGET_ASPECT_RATIO_MAX;
- float SMALL_TARGET_ASPECT_RATIO_MIN;
- // 面积比 MAX MIN
- float AREA_RATIO_MAX;
- float AREA_RATIO_MIN;
- // 圆心R距离小轮廓中心的距离系数
- float BIG_LENTH_R;
-
- // 圆心限制条件
- // 圆心roi矩形大小
- int CENTER_R_ROI_SIZE;
-
- // 圆心矩形比例
- float CENTER_R_ASPECT_RATIO_MIN;
- float CENTER_R_ASPECT_RATIO_MAX;
-
- // 圆心矩形面积
- int CENTER_R_AREA_MIN;
- int CENTER_R_AREA_MAX;
-
- // 滤波器系数
- float FILTER_COEFFICIENT;
-
- // 能量机关打击模型参数，详情见 buff_config.xml
- float BUFF_H;
- float BUFF_RADIUS;
- float PLATFORM_H;
- float BARREL_ROBOT_H;
- float TARGET_X;
-
- // 预测量的补偿（弧度）
- float OFFSET_FIXED_RADIAN;
-
- // 模型深度补偿（左半边比右半边距离要远）
- float OFFSET_TARGET_Z;
-
- // yaw 和 pitch 轴弹道补偿
- float OFFSET_ARMOR_YAW;
- float OFFSET_ARMOR_PITCH;
-
- // 手算pitch 轴弹道补偿
- float OFFSET_MANUAL_ARMOR_PITCH;
-
-};
-
-struct Buff_Ctrl {
- int IS_PARAM_ADJUSTMENT;
- int IS_SHOW_BIN_IMG;
- int PROCESSING_MODE; // BGR_MODE=0 HSV_MODE=1
-};
-
-struct Buff_Cfg {
- Buff_Param param;
- Buff_Ctrl ctrl;
-};
-
-// 能量机关类 继承抽象类
-class Detector {
- public:
- Detector() = default;
- explicit Detector(const std::string& _buff_config_path);
-
- ~Detector() = default;
-
- abstract_target::Target candidated_target_; // 当前检测得到的打击目标（遍历使用）
- abstract_target::Target current_target_; // 当前检测打击目标（现在）
- std::vector target_box_; // 打击目标队列
-
- /**
- * @brief 总执行函数（接口）
- * @param _input_img 输入图像
- * @param _receive_info 串口接收结构体
- * @param _send_info 串口发送结构体
- */
- void runTask(cv::Mat& _input_img, const uart::Receive_Data& _receive_info, uart::Write_Data& _send_info);
-
- /**
- * @brief 总执行函数（接口）
- * @param _input_img 输入图像
- * @param _receive_info 串口接收结构体
- * @return uart::Write_Data 串口发送结构体
- */
- bool runTask(cv::Mat& _input_img, const uart::Receive_Data& _receive_info);
- //uart::Write_Data runTask(cv::Mat& _input_img, const uart::Receive_Data& _receive_info);
-
- cv::RotatedRect returnObjectRect();
-
- uart::Write_Data::node returnObjectforUart();
-
- bool isfire();
-
- private:
- /**
- * @brief 获取参数更新结构体
- * @param[in] _fs 文件对象
- */
- void readBuffConfig(const cv::FileStorage& _fs);
-
- /**
- * @brief 获取基本信息
- * @details 图像和颜色
- * @param[in] _input_img 输入图像
- * @param[in] _my_color 己方颜色
- */
- void getInput(cv::Mat& _input_img, const int& _my_color);
- void getInput_Action(cv::Mat& _input_img, const int& _my_color);
- void getInput_Inaction(cv::Mat& _input_img, const int& _my_color);
-
- /**
- * @brief 显示最终图像
- */
- void displayDst();
-
- // 类中全局变量
- cv::Mat src_img_; // 输入原图
- cv::Mat src_img_action; // 输入原图
- cv::Mat src_img_inaction; // 输入原图
- cv::Mat dst_img_; // 图像效果展示图
- std::vector target_2d_point_; // 目标二维点集
- float final_target_z_; // 最终打击目标的深度信息（预测点）
- cv::RotatedRect target_rect_; // 目标矩形
- int my_color_; // 当前自己的颜色
- Buff_Cfg buff_config_; // 参数结构体
-
- bool is_find_last_target_; // 上一帧是否发现目标 true：发现 false：未发现
- bool is_find_target_; // 是否发现目标 true：发现 false：未发现
-
-
-
- abstract_target::Target last_target_; // 上一个打击目标
-
- cv::Point2f final_object; // 预测打击点
-
- std::vector> contour;
- std::vector hierarchy;
-
-
-
- private:
- // 预处理
- /**
- * @brief 预处理的模式
- */
- enum Processing_Moudle {
- BGR_MODE,
- HSV_MODE,
+namespace basic_buff
+{
+
+ auto idntifier_green = fmt::format(fg(fmt::color::green) | fmt::emphasis::bold, "basic_buff");
+ auto idntifier_red = fmt::format(fg(fmt::color::red) | fmt::emphasis::bold, "basic_buff");
+ auto idntifier_yellow = fmt::format(fg(fmt::color::yellow) | fmt::emphasis::bold, "basic_buff");
+ auto process_yellow = fmt::format(fg(fmt::color::yellow) | fmt::emphasis::bold, "processing");
+ auto target_yellow = fmt::format(fg(fmt::color::yellow) | fmt::emphasis::bold, "find_target");
+ auto center_yellow = fmt::format(fg(fmt::color::yellow) | fmt::emphasis::bold, "center_r");
+ auto judgement_yellow = fmt::format(fg(fmt::color::yellow) | fmt::emphasis::bold, "judgement");
+ auto predict_yellow = fmt::format(fg(fmt::color::yellow) | fmt::emphasis::bold, "predict");
+ auto final_target_yellow = fmt::format(fg(fmt::color::yellow) | fmt::emphasis::bold, "final_target");
+ auto debug_info = fmt::format(fg(fmt::color::red) | fmt::emphasis::bold, "debug_info");
+ auto debug_info_2 = fmt::format(fg(fmt::color::green) | fmt::emphasis::bold, "debug_info");
+
+ cv::Scalar Color_Draw_For_Edge = CV_RGB(255, 0, 0);
+
+ new_buff::buff big_buff;
+
+ struct Buff_Param
+ {
+ // BGR
+ int RED_BUFF_GRAY_TH;
+ int RED_BUFF_COLOR_TH;
+ int BLUE_BUFF_GRAY_TH;
+ int BLUE_BUFF_COLOR_TH;
+
+ // HSV-red
+ int H_RED_MAX;
+ int H_RED_MIN;
+ int S_RED_MAX;
+ int S_RED_MIN;
+ int V_RED_MAX;
+ int V_RED_MIN;
+
+ // HSV-blue
+ int H_BLUE_MAX;
+ int H_BLUE_MIN;
+ int S_BLUE_MAX;
+ int S_BLUE_MIN;
+ int V_BLUE_MAX;
+ int V_BLUE_MIN;
+
+ // 筛选条件
+ // 面积 MAX MIN 大 小
+ int SMALL_TARGET_AREA_MAX;
+ int SMALL_TARGET_AREA_MIN;
+ int BIG_TARGET_AREA_MAX;
+ int BIG_TARGET_AREA_MIN;
+ // 周长 MAX MIN
+ int SMALL_TARGET_Length_MIN;
+ int SMALL_TARGET_Length_MAX;
+ int BIG_TARGET_Length_MIN;
+ int BIG_TARGET_Length_MAX;
+ // 角度差 MAX MIN
+ int DIFF_ANGLE_MAX;
+ int DIFF_ANGLE_MIN;
+ // 长宽比 MAX MIN
+ float SMALL_TARGET_ASPECT_RATIO_MAX;
+ float SMALL_TARGET_ASPECT_RATIO_MIN;
+ // 面积比 MAX MIN
+ float AREA_RATIO_MAX;
+ float AREA_RATIO_MIN;
+ // 圆心R距离小轮廓中心的距离系数
+ float BIG_LENTH_R;
+
+ // 圆心限制条件
+ // 圆心roi矩形大小
+ int CENTER_R_ROI_SIZE;
+
+ // 圆心矩形比例
+ float CENTER_R_ASPECT_RATIO_MIN;
+ float CENTER_R_ASPECT_RATIO_MAX;
+
+ // 圆心矩形面积
+ int CENTER_R_AREA_MIN;
+ int CENTER_R_AREA_MAX;
+
+ // 滤波器系数
+ float FILTER_COEFFICIENT;
+
+ // 能量机关打击模型参数，详情见 buff_config.xml
+ float BUFF_H;
+ float BUFF_RADIUS;
+ float PLATFORM_H;
+ float BARREL_ROBOT_H;
+ float TARGET_X;
+
+ // 预测量的补偿（弧度）
+ float OFFSET_FIXED_RADIAN;
+
+ // 模型深度补偿（左半边比右半边距离要远）
+ float OFFSET_TARGET_Z;
+
+ // yaw 和 pitch 轴弹道补偿
+ float OFFSET_ARMOR_YAW;
+ float OFFSET_ARMOR_PITCH;
+
+ // 手算pitch 轴弹道补偿
+ float OFFSET_MANUAL_ARMOR_PITCH;
};
+ struct Buff_Ctrl
+ {
+ int IS_PARAM_ADJUSTMENT;
+ int IS_SHOW_BIN_IMG;
+ int PROCESSING_MODE; // BGR_MODE=0 HSV_MODE=1
+ };
- /**
- * @brief 预处理执行函数
- * @param[in] _input_img 输入图像（src）
- * @param[out] _output_img 输出图像（bin）
- * @param[in] _my_color 颜色参数
- * @param[in] _process_moudle 预处理模式
- */
- void imageProcessing(cv::Mat& _input_img, cv::Mat& _output_img, const int& _my_color, const Processing_Moudle& _process_moudle, const new_buff::Check_Moudle& check_moudle);
-
- /**
- * @brief BGR颜色空间预处理
- * @param _my_color 颜色参数
- */
- void bgrProcessing(const int& _my_color);
-
- /**
- * @brief HSV颜色空间预处理
- * @param _my_color 颜色参数
- */
- void hsvProcessing(const int& _my_color);
-
- cv::Mat gray_img_; // 灰度图
- //cv::Mat bin_img_; // 最终二值图
- cv::Mat bin_img_color_; // 颜色二值图
- cv::Mat bin_img_color1_; // 颜色二值图一
- cv::Mat bin_img_color2_; // 颜色二值图二
- cv::Mat bin_img_gray_; // 灰度二值图
-
- cv::Mat bin_img_action; // 激活识别二值图
- cv::Mat bin_img_inaction; // 未激活识别二值图
+ struct Buff_Cfg
+ {
+ Buff_Param param;
+ Buff_Ctrl ctrl;
+ };
- cv::Mat ele_ = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)); // 椭圆内核
+ // 能量机关类 继承抽象类
+ class Detector
+ {
+ public:
+ Detector() = default;
+ explicit Detector(const std::string &_buff_config_path);
+
+ ~Detector() = default;
+
+ abstract_target::Target candidated_target_; // 当前检测得到的打击目标（遍历使用）
+ abstract_target::Target current_target_; // 当前检测打击目标（现在）
+ std::vector target_box_; // 打击目标队列
+
+ /**
+ * @brief 总执行函数（接口）
+ * @param _input_img 输入图像
+ * @param _receive_info 串口接收结构体
+ * @param _send_info 串口发送结构体
+ */
+ void runTask(cv::Mat &_input_img, const uart::Receive_Data &_receive_info, uart::Write_Data &_send_info);
+
+ /**
+ * @brief 总执行函数（接口）
+ * @param _input_img 输入图像
+ * @param _receive_info 串口接收结构体
+ * @return uart::Write_Data 串口发送结构体
+ */
+ bool runTask(cv::Mat &_input_img, const uart::Receive_Data &_receive_info);
+ // uart::Write_Data runTask(cv::Mat& _input_img, const uart::Receive_Data& _receive_info);
+
+ cv::RotatedRect returnObjectRect();
+
+ uart::Write_Data::node returnObjectforUart();
+
+ bool isfire();
+
+ private:
+ /**
+ * @brief 获取参数更新结构体
+ * @param[in] _fs 文件对象
+ */
+ void readBuffConfig(const cv::FileStorage &_fs);
+
+ /**
+ * @brief 获取基本信息
+ * @details 图像和颜色
+ * @param[in] _input_img 输入图像
+ * @param[in] _my_color 己方颜色
+ */
+ void getInput(cv::Mat &_input_img, const int &_my_color);
+ void getInput_Action(cv::Mat &_input_img, const int &_my_color);
+ void getInput_Inaction(cv::Mat &_input_img, const int &_my_color);
+
+ /**
+ * @brief 显示最终图像
+ */
+ void displayDst();
+
+ // 类中全局变量
+ cv::Mat src_img_; // 输入原图
+ cv::Mat src_img_action; // 输入原图
+ cv::Mat src_img_inaction; // 输入原图
+ cv::Mat dst_img_; // 图像效果展示图
+ std::vector target_2d_point_; // 目标二维点集
+ float final_target_z_; // 最终打击目标的深度信息（预测点）
+ cv::RotatedRect target_rect_; // 目标矩形
+ int my_color_; // 当前自己的颜色
+ Buff_Cfg buff_config_; // 参数结构体
+
+ bool is_find_last_target_; // 上一帧是否发现目标 true：发现 false：未发现
+ bool is_find_target_; // 是否发现目标 true：发现 false：未发现
+
+ abstract_target::Target last_target_; // 上一个打击目标
+
+ cv::Point2f final_object; // 预测打击点
+
+ std::vector> contour;
+ std::vector hierarchy;
+
+ private:
+ // 预处理
+ /**
+ * @brief 预处理的模式
+ */
+ enum Processing_Moudle
+ {
+ BGR_MODE,
+ HSV_MODE,
+ };
+
+ /**
+ * @brief 预处理执行函数
+ * @param[in] _input_img 输入图像（src）
+ * @param[out] _output_img 输出图像（bin）
+ * @param[in] _my_color 颜色参数
+ * @param[in] _process_moudle 预处理模式
+ */
+ void imageProcessing(cv::Mat &_input_img, cv::Mat &_output_img, const int &_my_color, const Processing_Moudle &_process_moudle, const new_buff::Check_Moudle &check_moudle);
+
+ /**
+ * @brief BGR颜色空间预处理
+ * @param _my_color 颜色参数
+ */
+ void bgrProcessing(const int &_my_color);
+
+ /**
+ * @brief HSV颜色空间预处理
+ * @param _my_color 颜色参数
+ */
+ void hsvProcessing(const int &_my_color);
+
+ cv::Mat gray_img_; // 灰度图
+ // cv::Mat bin_img_; // 最终二值图
+ cv::Mat bin_img_color_; // 颜色二值图
+ cv::Mat bin_img_color1_; // 颜色二值图一
+ cv::Mat bin_img_color2_; // 颜色二值图二
+ cv::Mat bin_img_gray_; // 灰度二值图
+
+ cv::Mat bin_img_action; // 激活识别二值图
+ cv::Mat bin_img_inaction; // 未激活识别二值图
+
+ cv::Mat ele_ = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)); // 椭圆内核
// BGR
#ifdef DEBUG_STATIC
- static std::vector split_img_; // 分通道图
+ static std::vector split_img_; // 分通道图
#else
- std::vector split_img_; // 分通道图
-#endif // DEBUG_STATIC
- int average_th_; // 平均阈值
+ std::vector split_img_; // 分通道图
+#endif // DEBUG_STATIC
+ int average_th_; // 平均阈值
- // HSV
- cv::Mat hsv_img_; // hsv预处理输入图
+ // HSV
+ cv::Mat hsv_img_; // hsv预处理输入图
#ifndef RELEASE
- // 滑动条窗口
- cv::Mat trackbar_img_ = cv::Mat::zeros(1, 300, CV_8UC1); // 预处理
-#endif // !RELEASE
-
- private:
- // 查找目标
- /**
- * @brief 查找目标
- * @param _input_dst_img 输入图像
- * @param _input_bin_img 输入二值图
- * @param _target_box 输出目标容器
- */
- void findTarget(cv::Mat& _input_dst_img, cv::Mat& _input_bin_img, std::vector& _target_box, const new_buff::Check_Moudle &check_moudle);
- void findTarget_new(cv::Mat& _input_dst_img, cv::Mat& _input_bin_img, std::vector& _target_box, const new_buff::Check_Moudle &check_moudle);
-
- fan_armor::Detector small_target_; // 内轮廓
- abstract_blade::FanBlade big_target_; // 外轮廓
-
- int action_cnt_; // 已打击扇叶数
- int inaction_cnt_; // 未击打扇叶数
-
- // 轮廓点集 轮廓关系
- std::vector> contours_;
- std::vector hierarchy_;
-
- // 小轮廓条件(area and length)
- float small_rect_area_;
- float small_rect_length_;
-
- // 大轮廓条件(area and length)
- float big_rect_area_;
- float big_rect_length_;
+ // 滑动条窗口
+ cv::Mat trackbar_img_ = cv::Mat::zeros(1, 300, CV_8UC1); // 预处理
+#endif // !RELEASE
+
+ private:
+ // 查找目标
+ /**
+ * @brief 查找目标
+ * @param _input_dst_img 输入图像
+ * @param _input_bin_img 输入二值图
+ * @param _target_box 输出目标容器
+ */
+ void findTarget(cv::Mat &_input_dst_img, cv::Mat &_input_bin_img, std::vector &_target_box, const new_buff::Check_Moudle &check_moudle);
+ void findTarget_new(cv::Mat &_input_dst_img, cv::Mat &_input_bin_img, std::vector &_target_box, const new_buff::Check_Moudle &check_moudle);
+
+ fan_armor::Detector small_target_; // 内轮廓
+ abstract_blade::FanBlade big_target_; // 外轮廓
+
+ int action_cnt_; // 已打击扇叶数
+ int inaction_cnt_; // 未击打扇叶数
+
+ // 轮廓点集 轮廓关系
+ std::vector> contours_;
+ std::vector hierarchy_;
+
+ // 小轮廓条件(area and length)
+ float small_rect_area_;
+ float small_rect_length_;
+
+ // 大轮廓条件(area and length)
+ float big_rect_area_;
+ float big_rect_length_;
#ifndef RELEASE
- // 滑动条窗口
- cv::Mat target_trackbar_img_ = cv::Mat::zeros(1, 300, CV_8UC1);
-
- int small_target_aspect_ratio_max_int_;
- int small_target_aspect_ratio_min_int_;
- int area_ratio_max_int_;
- int area_ratio_min_int_;
-#endif // !RELEASE
-
- private:
- /**
- * @brief 判断是否有目标
- * @param[in] _input_img 绘制未激活的目标
- * @param[in] _target_box 扇叶目标容器
- * @return true 发现目标
- * @return false 丢失目标
- */
- bool isFindTarget(cv::Mat& _input_img, std::vector& _target_box);
-
- private:
- /**
- * @brief 查找圆心
- * @param _input_src_img 输入src原图
- * @param _input_bin_img 输入bin二值图
- * @param _dst_img 输入dst画板图
- * @param _is_find_target 是否发现扇叶目标
- * @return cv::Point2f 返回圆心R中点坐标
- */
- cv::Point2f findCircleR(cv::Mat& _input_src_img, cv::Mat& _input_bin_img, cv::Mat& _dst_img, const bool& _is_find_target);
-
- bool is_circle_; // 是否找到圆心
- cv::Point2f delta_height_point_; // 获取装甲板的高度点差
- cv::Point2f roi_global_center_; // roi 圆心中点位置(在原图中)
- cv::Mat result_img_; // 二值图
- cv::Mat roi_img_; // 截取原图
- basic_roi::RoI roi_tool_; // roi截取工具
-
- abstract_center_r::Center_R center_r_; // 候选圆心R
- std::vector> contours_r_; // 中心R的遍历点集
- cv::Point2f roi_local_center_; // 截取roi的图像中心点(在roi_img中)
- std::vector center_r_box_; // 第一次筛选之后得到的待选中心R
- cv::Point2f final_center_r_; // 最终圆心（假定/真实）
-
- private:
- // 计算运转状态值：速度、方向、角度
- /**
- * @brief 计算运转状态值：速度、方向、角度
- * @param _is_find_target 是否发现目标
- */
- void judgeCondition(const bool& _is_find_target);
-
- /**
- * @brief 计算角度和角度差
- */
- void calAngle();
-
- /**
- * @brief 计算转动方向
- * @details 1：顺时针 -1：逆时针 0：不转动
- */
- void calDirection();
-
- /**
- * @brief 获取风车转向
- * @return int
- */
- int getState();
-
- /**
- * @brief 计算当前扇叶转动速度
- */
- void calVelocity();
-
- void edit_param();
-
- // 角度
- float current_angle_; // 当前目标角度
- float last_angle_; // 上一次目标角度
- float diff_angle_; // 两帧之间角度差
- float last_diff_angle_; // 上一帧扇叶移动的角度
- float last_last_diff_angle_; // 上上帧扇叶移动的角度
- bool is_change_blade_; // 判断是否切换扇叶
-
- // 方向
- float filter_direction_; // 第二次滤波方向
- int final_direction_; // 最终的方向
- int last_final_direction_; // 上一次最终的方向
- float current_direction_; // 当前方向
- float last_direction_; // 上一次方向
- int find_cnt_; // 发现目标次数
- float d_angle_; // 滤波器系数
- int confirm_cnt_; // 记录达到条件次数
- bool is_confirm_; // 判断是否达到条件
-
- // 速度
- float current_speed_; // 当前转速
- double last_time_; // 上一帧的时间
- double last_last_time_; // 上上帧的时间
-
- private:
- // 计算预测量
-
- /**
- * @brief 计算预测量
- * @param[in] _bullet_velocity 子弹速度
- * @param[in] _is_find_target 是否发现目标
- * @return float 预测量
- */
- float doPredict(const float& _bullet_velocity, const bool& _is_find_target);
-
- /**
- * @brief 计算固定预测量
- * @param[in] _bullet_velocity 子弹速度
- * @return float 预测量
- */
- float fixedPredict(const float& _bullet_velocity);
-
- // 变化预测量 TODO(fqjun)
- void mutativePredict(const float& _input_predict_quantity, float& _output_predict_quantity);
-
- float current_radian_; // 当前弧度
- float barrel_buff_botton_h_; // 枪口到扇叶底部高度
- float target_buff_h_; // 目标在扇叶上的高度
- float target_y_; // 当前扇叶目标高度
- float target_x_; // 当前扇叶目标到高台的直线距离
- float target_z_; // 当前扇叶目标直线距离
-
- float bullet_tof_; // 子弹飞行时间
- float fixed_forecast_quantity_; // 固定预测量（扇叶的单帧移动量）
- float final_forecast_quantity_; // 最终合成的预测量
+ // 滑动条窗口
+ cv::Mat target_trackbar_img_ = cv::Mat::zeros(1, 300, CV_8UC1);
+
+ int small_target_aspect_ratio_max_int_;
+ int small_target_aspect_ratio_min_int_;
+ int area_ratio_max_int_;
+ int area_ratio_min_int_;
+#endif // !RELEASE
+
+ private:
+ /**
+ * @brief 判断是否有目标
+ * @param[in] _input_img 绘制未激活的目标
+ * @param[in] _target_box 扇叶目标容器
+ * @return true 发现目标
+ * @return false 丢失目标
+ */
+ bool isFindTarget(cv::Mat &_input_img, std::vector &_target_box);
+
+ private:
+ /**
+ * @brief 查找圆心
+ * @param _input_src_img 输入src原图
+ * @param _input_bin_img 输入bin二值图
+ * @param _dst_img 输入dst画板图
+ * @param _is_find_target 是否发现扇叶目标
+ * @return cv::Point2f 返回圆心R中点坐标
+ */
+ cv::Point2f findCircleR(cv::Mat &_input_src_img, cv::Mat &_input_bin_img, cv::Mat &_dst_img, const bool &_is_find_target);
+
+ bool is_circle_; // 是否找到圆心
+ cv::Point2f delta_height_point_; // 获取装甲板的高度点差
+ cv::Point2f roi_global_center_; // roi 圆心中点位置(在原图中)
+ cv::Mat result_img_; // 二值图
+ cv::Mat roi_img_; // 截取原图
+ basic_roi::RoI roi_tool_; // roi截取工具
+
+ abstract_center_r::Center_R center_r_; // 候选圆心R
+ std::vector> contours_r_; // 中心R的遍历点集
+ cv::Point2f roi_local_center_; // 截取roi的图像中心点(在roi_img中)
+ std::vector center_r_box_; // 第一次筛选之后得到的待选中心R
+ cv::Point2f final_center_r_; // 最终圆心（假定/真实）
+
+ private:
+ // 计算运转状态值：速度、方向、角度
+ /**
+ * @brief 计算运转状态值：速度、方向、角度
+ * @param _is_find_target 是否发现目标
+ */
+ void judgeCondition(const bool &_is_find_target);
+
+ /**
+ * @brief 计算角度和角度差
+ */
+ void calAngle();
+
+ /**
+ * @brief 计算转动方向
+ * @details 1：顺时针 -1：逆时针 0：不转动
+ */
+ void calDirection();
+
+ /**
+ * @brief 获取风车转向
+ * @return int
+ */
+ int getState();
+
+ /**
+ * @brief 计算当前扇叶转动速度
+ */
+ void calVelocity();
+
+ void edit_param();
+
+ // 角度
+ float current_angle_; // 当前目标角度
+ float last_angle_; // 上一次目标角度
+ float diff_angle_; // 两帧之间角度差
+ float last_diff_angle_; // 上一帧扇叶移动的角度
+ float last_last_diff_angle_; // 上上帧扇叶移动的角度
+ bool is_change_blade_; // 判断是否切换扇叶
+
+ // 方向
+ float filter_direction_; // 第二次滤波方向
+ int final_direction_; // 最终的方向
+ int last_final_direction_; // 上一次最终的方向
+ float current_direction_; // 当前方向
+ float last_direction_; // 上一次方向
+ int find_cnt_; // 发现目标次数
+ float d_angle_; // 滤波器系数
+ int confirm_cnt_; // 记录达到条件次数
+ bool is_confirm_; // 判断是否达到条件
+
+ // 速度
+ float current_speed_; // 当前转速
+ double last_time_; // 上一帧的时间
+ double last_last_time_; // 上上帧的时间
+
+ private:
+ // 计算预测量
+
+ /**
+ * @brief 计算预测量
+ * @param[in] _bullet_velocity 子弹速度
+ * @param[in] _is_find_target 是否发现目标
+ * @return float 预测量
+ */
+ float doPredict(const float &_bullet_velocity, const bool &_is_find_target);
+
+ /**
+ * @brief 计算固定预测量
+ * @param[in] _bullet_velocity 子弹速度
+ * @return float 预测量
+ */
+ float fixedPredict(const float &_bullet_velocity);
+
+ // 变化预测量 TODO(fqjun)
+ void mutativePredict(const float &_input_predict_quantity, float &_output_predict_quantity);
+
+ float current_radian_; // 当前弧度
+ float barrel_buff_botton_h_; // 枪口到扇叶底部高度
+ float target_buff_h_; // 目标在扇叶上的高度
+ float target_y_; // 当前扇叶目标高度
+ float target_x_; // 当前扇叶目标到高台的直线距离
+ float target_z_; // 当前扇叶目标直线距离
+
+ float bullet_tof_; // 子弹飞行时间
+ float fixed_forecast_quantity_; // 固定预测量（扇叶的单帧移动量）
+ float final_forecast_quantity_; // 最终合成的预测量
#ifdef DEBUG_KALMAN
- private:
- // 卡尔曼滤波器
- // basic_kalman::firstKalman buff_filter_ = basic_kalman::firstKalman(0.01f, 0.03f, 1.f, 0.f, 0.f);
- basic_kalman::firstKalman buff_filter_ = basic_kalman::firstKalman(0.02f, 0.03f, 1.f, 0.f, 0.f);
-
- int Q = 3;
- int R = 2;
- float current_predict_quantity = 0.f;
-
- cv::Mat kalman_trackbar_img_ = cv::Mat::zeros(1, 300, CV_8UC1); // 预处理
- float last_final_radian_ = 0.f;
-#endif // DEBUG_KALMAN
-
- private:
- // 计算获取最终目标（矩形、顶点）
-
- /**
- * @brief 计算最终目标矩形顶点点集
- * @param _predict_quantity 预测量
- * @param _final_center_r 圆心坐标（src）
- * @param _target_2d_point 目标矩形顶点容器
- * @param _input_dst_img 输入画板
- * @param _is_find_target 是否有目标
- */
- void calculateTargetPointSet(const float& _predict_quantity, const cv::Point2f& _final_center_r, std::vector& _target_2d_point, cv::Mat& _input_dst_img, const bool& _is_find_target);
-
- double theta_; // 特殊的弧度
- float final_angle_; // 最终角度
- float final_radian_; // 最终弧度
- float sin_calcu_; // 计算的sin值
- float cos_calcu_; // 计算的cos值
- cv::Point2f pre_center_; // 最终预测点
- float radio_; // 轨迹圆半径
-
- private:
- // 计算云台角度
-
- basic_pnp::PnP buff_pnp_ = basic_pnp::PnP(fmt::format("{}{}", CONFIG_FILE_PATH, "/camera/mv_camera_config_407.xml"), fmt::format("{}{}", CONFIG_FILE_PATH, "/angle_solve/basic_pnp_config.xml"));
-
- // 手动计算云台角度
- /**
- * @brief 手动计算云台角度
- * @param _target_center 目标中心点
- * @param _unit_pixel_length 成像像素元件长度 mm
- * @param _image_size 图像大小
- * @param _focal_length 相机焦距 mm
- * @return cv::Point2f
- */
- cv::Point2f angleCalculation(const cv::Point2f& _target_center, const float& _unit_pixel_length, const cv::Size& _image_size, const float& _focal_length);
-
- private:
- // 自动控制
-
- private:
- // 更新上一帧数据
- /**
- * @brief 更新上一帧数据
- * @param _is_find_target 是否有目标
- */
- void updateLastData(const bool& _is_find_target);
-
- private:
- // 帧率测试
- fps::FPS buff_fps_1_{"Part 1"};
- fps::FPS buff_fps_2_{"Part 2"};
- fps::FPS buff_fps_;
-};
-
-} // namespace basic_buff
+ private:
+ // 卡尔曼滤波器
+ // basic_kalman::firstKalman buff_filter_ = basic_kalman::firstKalman(0.01f, 0.03f, 1.f, 0.f, 0.f);
+ basic_kalman::firstKalman buff_filter_ = basic_kalman::firstKalman(0.02f, 0.03f, 1.f, 0.f, 0.f);
+
+ int Q = 3;
+ int R = 2;
+ float current_predict_quantity = 0.f;
+
+ cv::Mat kalman_trackbar_img_ = cv::Mat::zeros(1, 300, CV_8UC1); // 预处理
+ float last_final_radian_ = 0.f;
+#endif // DEBUG_KALMAN
+
+ private:
+ // 计算获取最终目标（矩形、顶点）
+
+ /**
+ * @brief 计算最终目标矩形顶点点集
+ * @param _predict_quantity 预测量
+ * @param _final_center_r 圆心坐标（src）
+ * @param _target_2d_point 目标矩形顶点容器
+ * @param _input_dst_img 输入画板
+ * @param _is_find_target 是否有目标
+ */
+ void calculateTargetPointSet(const float &_predict_quantity, const cv::Point2f &_final_center_r, std::vector &_target_2d_point, cv::Mat &_input_dst_img, const bool &_is_find_target);
+
+ double theta_; // 特殊的弧度
+ float final_angle_; // 最终角度
+ float final_radian_; // 最终弧度
+ float sin_calcu_; // 计算的sin值
+ float cos_calcu_; // 计算的cos值
+ cv::Point2f pre_center_; // 最终预测点
+ float radio_; // 轨迹圆半径
+
+ private:
+ // 计算云台角度
+
+ basic_pnp::PnP buff_pnp_ = basic_pnp::PnP(fmt::format("{}{}", CONFIG_FILE_PATH, "/camera/mv_camera_config_407.xml"), fmt::format("{}{}", CONFIG_FILE_PATH, "/angle_solve/basic_pnp_config.xml"));
+
+ // 手动计算云台角度
+ /**
+ * @brief 手动计算云台角度
+ * @param _target_center 目标中心点
+ * @param _unit_pixel_length 成像像素元件长度 mm
+ * @param _image_size 图像大小
+ * @param _focal_length 相机焦距 mm
+ * @return cv::Point2f
+ */
+ cv::Point2f angleCalculation(const cv::Point2f &_target_center, const float &_unit_pixel_length, const cv::Size &_image_size, const float &_focal_length);
+
+ private:
+ // 自动控制
+
+ private:
+ // 更新上一帧数据
+ /**
+ * @brief 更新上一帧数据
+ * @param _is_find_target 是否有目标
+ */
+ void updateLastData(const bool &_is_find_target);
+
+ private:
+ // 帧率测试
+ fps::FPS buff_fps_1_{"Part 1"};
+ fps::FPS buff_fps_2_{"Part 2"};
+ fps::FPS buff_fps_;
+ };
+
+} // namespace basic_buff
diff --git a/module/buff/new_buff.cpp b/module/buff/new_buff.cpp
index 555d7b8..eadf32d 100644
--- a/module/buff/new_buff.cpp
+++ b/module/buff/new_buff.cpp
@@ -3,19 +3,20 @@
* @author dgsyrc (yrcminecraft@foxmail.com)
* @brief 新能量机关检测
* @date 2023-01-15
- *
+ *
* @copyright Copyright (c) 2023 Sirius
- *
+ *
*/
-#define gimbal_delay 800.0f //ms
-//#define RELEASE
+#define gimbal_delay 800.0f // ms
+// #define RELEASE
-#include "new_buff.hpp"
+#include "new_buff.hpp"
-
-namespace new_buff {
- void buff::set_config(std::string config_path) {
+namespace new_buff
+{
+ void buff::set_config(std::string config_path)
+ {
cv::FileStorage conf(config_path, cv::FileStorage::READ);
conf["ARMOR_HEIGHT"] >> config.armor_height;
conf["ARMOR_LENGHT"] >> config.armor_lenght;
@@ -26,161 +27,167 @@ namespace new_buff {
conf.release();
}
- void buff::getcontour(cv::Mat imgDil,cv::Mat imgDil2, cv::Mat& img_src,new_buff::Check_Moudle moudle)//判断轮廓
+ void buff::getcontour(cv::Mat imgDil, cv::Mat imgDil2, cv::Mat &img_src, new_buff::Check_Moudle moudle) // 判断轮廓
{
- int objCor,cnt_obj;
- cnt_obj=0;
+ int objCor, cnt_obj;
+ cnt_obj = 0;
std::vector> contour;
std::vector hierarchy;
findContours(imgDil, contour, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_TC89_KCOS);
-
+
std::vector> conPoly(contour.size());
std::vector boundRect(contour.size());
std::vector inaction_check;
- //fmt::print("[img process] contour.size() {}\n",contour.size());
- area_circles=0.0f;
+ // fmt::print("[img process] contour.size() {}\n",contour.size());
+ area_circles = 0.0f;
for (int i = 0; i < contour.size(); i++)
{
int area;
int area_rect;
- area = contourArea(contour[i]);//轮廓面积用于排除黑点
-
- std::string objectType;//形状
-
- if (area > 10)//排除小黑圆点的干扰
+ area = contourArea(contour[i]); // 轮廓面积用于排除黑点
+
+ std::string objectType; // 形状
+
+ if (area > 10) // 排除小黑圆点的干扰
{
float peri = cv::arcLength(contour[i], true);
- approxPolyDP(contour[i], conPoly[i], 0.02 * peri, true);//把一个连续光滑曲线折线化
- //std::cout << conPoly[i].size() << endl;//边数
- //fmt::print("[img process] conPoly.size {}\n",conPoly[i].size());
+ approxPolyDP(contour[i], conPoly[i], 0.02 * peri, true); // 把一个连续光滑曲线折线化
+ // std::cout << conPoly[i].size() << endl;//边数
+ // fmt::print("[img process] conPoly.size {}\n",conPoly[i].size());
boundRect[i] = boundingRect(conPoly[i]);
-
- area_rect = (boundRect[i].br().x-boundRect[i].tl().x)*(boundRect[i].br().y-boundRect[i].tl().y);
+
+ area_rect = (boundRect[i].br().x - boundRect[i].tl().x) * (boundRect[i].br().y - boundRect[i].tl().y);
objCor = (int)conPoly[i].size();
-
- //fmt::print("[img process] Circle {} area {}\n",objCor,area);
-
- cv::imshow("imgdil2",imgDil2);
- if (objCor > 4&&area<4800) {
-
- if(moudle==INACTION_MODE){
- //if(area>300){
- //cv::rectangle(img_src, boundRect[i].tl() ,boundRect[i].br(),cv::Scalar(200, 255, 125), 2, 8);
+
+ // fmt::print("[img process] Circle {} area {}\n",objCor,area);
+
+ cv::imshow("imgdil2", imgDil2);
+ if (objCor > 4 && area < 4800)
+ {
+
+ if (moudle == INACTION_MODE)
+ {
+ // if(area>300){
+ // cv::rectangle(img_src, boundRect[i].tl() ,boundRect[i].br(),cv::Scalar(200, 255, 125), 2, 8);
//}
-
- cv::imshow("img_inaction",img_src);
- double ratio,dis,max_dist;
- cv::Point P_a,P_b;
+
+ cv::imshow("img_inaction", img_src);
+ double ratio, dis, max_dist;
+ cv::Point P_a, P_b;
cv::Point2f P_re;
-
- max_dist=0;
+
+ max_dist = 0;
// 找最长两点连线
- for(int j=0;j < conPoly[i].size();++j){
- for(int k=j+1;k < conPoly[i].size();++k){
+ for (int j = 0; j < conPoly[i].size(); ++j)
+ {
+ for (int k = j + 1; k < conPoly[i].size(); ++k)
+ {
double dist;
-
- dist=returnDistance(conPoly[i][j],conPoly[i][k]);
- if(dist>max_dist){
- max_dist=dist;
- P_a=conPoly[i][j];
- P_b=conPoly[i][k];
+
+ dist = returnDistance(conPoly[i][j], conPoly[i][k]);
+ if (dist > max_dist)
+ {
+ max_dist = dist;
+ P_a = conPoly[i][j];
+ P_b = conPoly[i][k];
}
}
}
- P_a.x=(int)kal.run(P_a.x);
- P_a.y=(int)kal.run(P_a.y);
- P_b.x=(int)kal.run(P_b.x);
- P_b.y=(int)kal.run(P_b.y);
-
-
- max_dist=returnDistance(P_a,P_b);
- if(fabs(fabs(returnDistance((P_a+P_b)/2,circles)/max_dist-56.0/25.0))<0.2/*&&dis>60.0*/){
+ P_a.x = (int)kal.run(P_a.x);
+ P_a.y = (int)kal.run(P_a.y);
+ P_b.x = (int)kal.run(P_b.x);
+ P_b.y = (int)kal.run(P_b.y);
+
+ max_dist = returnDistance(P_a, P_b);
+ if (fabs(fabs(returnDistance((P_a + P_b) / 2, circles) / max_dist - 56.0 / 25.0)) < 0.2 /*&&dis>60.0*/)
+ {
armor_check tmp;
-
- //dis=returnDistance((boundRect[i].tl()+boundRect[i].br())/2,circles);
- dis=returnDistance((P_a+P_b)/2,circles);
- //cv:line(img_src,boundRect[i].tl(),boundRect[i].br(),cv::Scalar(100, 255, 125),2);
- cv:line(img_src,P_a,P_b,cv::Scalar(100, 255, 125),2);
- ratio=fabs(fabs(returnDistance((P_a+P_b)/2,circles)/max_dist-56.0/25.0));
- tmp.cord[0].x=((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*50.0/56.0+circles.x;
- tmp.cord[0].y=((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*50.0/56.0+circles.y;
- tmp.cord[1].x=((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*54.0/56.0+circles.x;
- tmp.cord[1].y=((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*54.0/56.0+circles.y;
- tmp.cord[2].x=((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*56.0/56.0+circles.x;
- tmp.cord[2].y=((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*56.0/56.0+circles.y;
- tmp.cord[3].x=((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*53.0/56.0+circles.x;
- tmp.cord[3].y=((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*53.0/56.0+circles.y;
- tmp.obj.x=((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*47.0/56.0+circles.x;
- tmp.obj.y=((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*47.0/56.0+circles.y;
- tmp.d=returnDistance(boundRect[i].tl(),boundRect[i].br())*0.5;
- tmp.isPass=true;
+
+ // dis=returnDistance((boundRect[i].tl()+boundRect[i].br())/2,circles);
+ dis = returnDistance((P_a + P_b) / 2, circles);
+ // cv:line(img_src,boundRect[i].tl(),boundRect[i].br(),cv::Scalar(100, 255, 125),2);
+ cv:
+ line(img_src, P_a, P_b, cv::Scalar(100, 255, 125), 2);
+ ratio = fabs(fabs(returnDistance((P_a + P_b) / 2, circles) / max_dist - 56.0 / 25.0));
+ tmp.cord[0].x = ((boundRect[i].tl().x + boundRect[i].br().x) / 2 - circles.x) * 50.0 / 56.0 + circles.x;
+ tmp.cord[0].y = ((boundRect[i].tl().y + boundRect[i].br().y) / 2 - circles.y) * 50.0 / 56.0 + circles.y;
+ tmp.cord[1].x = ((boundRect[i].tl().x + boundRect[i].br().x) / 2 - circles.x) * 54.0 / 56.0 + circles.x;
+ tmp.cord[1].y = ((boundRect[i].tl().y + boundRect[i].br().y) / 2 - circles.y) * 54.0 / 56.0 + circles.y;
+ tmp.cord[2].x = ((boundRect[i].tl().x + boundRect[i].br().x) / 2 - circles.x) * 56.0 / 56.0 + circles.x;
+ tmp.cord[2].y = ((boundRect[i].tl().y + boundRect[i].br().y) / 2 - circles.y) * 56.0 / 56.0 + circles.y;
+ tmp.cord[3].x = ((boundRect[i].tl().x + boundRect[i].br().x) / 2 - circles.x) * 53.0 / 56.0 + circles.x;
+ tmp.cord[3].y = ((boundRect[i].tl().y + boundRect[i].br().y) / 2 - circles.y) * 53.0 / 56.0 + circles.y;
+ tmp.obj.x = ((boundRect[i].tl().x + boundRect[i].br().x) / 2 - circles.x) * 47.0 / 56.0 + circles.x;
+ tmp.obj.y = ((boundRect[i].tl().y + boundRect[i].br().y) / 2 - circles.y) * 47.0 / 56.0 + circles.y;
+ tmp.d = returnDistance(boundRect[i].tl(), boundRect[i].br()) * 0.5;
+ tmp.isPass = true;
inaction_check.push_back(tmp);
- cv::circle(img_src,{tmp.cord[0].x,tmp.cord[0].y},1,cv::Scalar(100, 255, 125),2);
- cv::circle(img_src,{tmp.cord[1].x,tmp.cord[1].y},1,cv::Scalar(100, 255, 125),2);
- cv::circle(img_src,{tmp.cord[2].x,tmp.cord[2].y},1,cv::Scalar(100, 255, 125),2);
- cv::circle(img_src,{tmp.obj.x,tmp.obj.y},1,cv::Scalar(100, 0, 125),2);
- cv::circle(img_src,P_a,1,cv::Scalar(100, 255, 125),2);
- cv::circle(img_src,P_b,1,cv::Scalar(100, 255, 125),2);
- //cv::circle(img_src,(P_a+P_b)/2,1,cv::Scalar(100, 255, 125),2);
- cv::rectangle(img_src, boundRect[i].tl() ,boundRect[i].br(),cv::Scalar(200, 255, 125), 2, 8);
- cv::line(img_src,circles,tmp.obj,cv::Scalar(100, 255, 125),2);
- cv::putText(img_src, std::to_string(ratio), { boundRect[i].x,boundRect[i].y - 5 },1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
- cv::imshow("inaction",img_src);
- //cv::circle(img_src,{((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*48.0/56.0+circles.x,((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*48.0/56.0+circles.y},2,cv::Scalar(255,0,255),1);
- }/*else{
- line(img_src,P_a,P_b,cv::Scalar(100, 255, 125),2);
- if(returnDistance(P_a,circles)((cv::Point)((P_re-circles)*0.5+circles))!=0){
- P_re.x = (P_a.x-circles.x) * cos(PI/180*15*(-1.0)) - (P_a.y-circles.y) * sin(PI/180*15*(-1.0)) + circles.x;
- P_re.y = (P_a.x-circles.x) * sin(PI/180*15*(-1.0)) + (P_a.y-circles.y) * cos(PI/180*15*(-1.0)) + circles.y;
- line(img_src,P_re,P_b,cv::Scalar(0, 96, 255),2);
- }else{
- line(img_src,P_re,P_b,cv::Scalar(0, 96, 255),2);
- }
-
- }else{
- P_re.x = (P_b.x-circles.x) * cos(PI/180*15) - (P_b.y-circles.y) * sin(PI/180*15) + circles.x;
- P_re.y = (P_b.x-circles.x) * sin(PI/180*15) + (P_b.y-circles.y) * cos(PI/180*15) + circles.y;
- if(imgDil.at((cv::Point)((P_re-circles)*0.5+circles))!=0){
- P_re.x = (P_b.x-circles.x) * cos(PI/180*15*(-1.0)) - (P_b.y-circles.y) * sin(PI/180*15*(-1.0)) + circles.x;
- P_re.y = (P_b.x-circles.x) * sin(PI/180*15*(-1.0)) + (P_b.y-circles.y) * cos(PI/180*15*(-1.0)) + circles.y;
- line(img_src,P_a,P_re,cv::Scalar(0, 96, 255),2);
- }else{
- line(img_src,P_a,P_re,cv::Scalar(0, 96, 255),2);
- }
-
- }
- if(abs((boundRect[i].br().x-boundRect[i].tl().x)*1.0/(boundRect[i].br().y-boundRect[i].tl().y)*1.0-1.0)<0.2){
- armor_check tmp;
- tmp.cord[0].x=((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*38.0/39.0+circles.x;
- tmp.cord[0].y=((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*38.0/39.0+circles.y;
- tmp.cord[1].x=((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*36.0/39.0+circles.x;
- tmp.cord[1].y=((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*36.0/39.0+circles.y;
- tmp.cord[2].x=((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*41.0/39.0+circles.x;
- tmp.cord[2].y=((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*41.0/39.0+circles.y;
- tmp.cord[3].x=((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*43.0/39.0+circles.x;
- tmp.cord[3].y=((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*43.0/39.0+circles.y;
- tmp.obj.x=(boundRect[i].tl().x+boundRect[i].br().x)/2;
- tmp.obj.y=(boundRect[i].tl().y+boundRect[i].br().y)/2;
- cv::circle(img_src,{tmp.cord[0].x,tmp.cord[0].y},1,cv::Scalar(100, 255, 125),2);
- cv::circle(img_src,{tmp.cord[1].x,tmp.cord[1].y},1,cv::Scalar(100, 255, 125),2);
- cv::circle(img_src,{tmp.cord[2].x,tmp.cord[2].y},1,cv::Scalar(100, 255, 125),2);
- cv::rectangle(img_src, boundRect[i].tl() ,boundRect[i].br(),cv::Scalar(200, 255, 125), 2, 8);
- cv::putText(img_src, std::to_string(area), { boundRect[i].x,boundRect[i].y - 5 },1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
- tmp.d=returnDistance(boundRect[i].tl(),boundRect[i].br());
- cv::putText(img_src, std::to_string(tmp.d), { tmp.obj.x,tmp.obj.y },1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
- if(tmp.d<100.0){
- tmp.isPass=true;
- inaction_check.push_back(tmp);
- cv::circle(img_src,{tmp.cord[2].x,tmp.cord[2].y},1,cv::Scalar(255, 255, 125),2);
- }
- cv::imshow("inaction",img_src);
-
- }
- }*/
+ cv::circle(img_src, {tmp.cord[0].x, tmp.cord[0].y}, 1, cv::Scalar(100, 255, 125), 2);
+ cv::circle(img_src, {tmp.cord[1].x, tmp.cord[1].y}, 1, cv::Scalar(100, 255, 125), 2);
+ cv::circle(img_src, {tmp.cord[2].x, tmp.cord[2].y}, 1, cv::Scalar(100, 255, 125), 2);
+ cv::circle(img_src, {tmp.obj.x, tmp.obj.y}, 1, cv::Scalar(100, 0, 125), 2);
+ cv::circle(img_src, P_a, 1, cv::Scalar(100, 255, 125), 2);
+ cv::circle(img_src, P_b, 1, cv::Scalar(100, 255, 125), 2);
+ // cv::circle(img_src,(P_a+P_b)/2,1,cv::Scalar(100, 255, 125),2);
+ cv::rectangle(img_src, boundRect[i].tl(), boundRect[i].br(), cv::Scalar(200, 255, 125), 2, 8);
+ cv::line(img_src, circles, tmp.obj, cv::Scalar(100, 255, 125), 2);
+ cv::putText(img_src, std::to_string(ratio), {boundRect[i].x, boundRect[i].y - 5}, 1, cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
+ cv::imshow("inaction", img_src);
+ // cv::circle(img_src,{((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*48.0/56.0+circles.x,((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*48.0/56.0+circles.y},2,cv::Scalar(255,0,255),1);
+ } /*else{
+ line(img_src,P_a,P_b,cv::Scalar(100, 255, 125),2);
+ if(returnDistance(P_a,circles)((cv::Point)((P_re-circles)*0.5+circles))!=0){
+ P_re.x = (P_a.x-circles.x) * cos(PI/180*15*(-1.0)) - (P_a.y-circles.y) * sin(PI/180*15*(-1.0)) + circles.x;
+ P_re.y = (P_a.x-circles.x) * sin(PI/180*15*(-1.0)) + (P_a.y-circles.y) * cos(PI/180*15*(-1.0)) + circles.y;
+ line(img_src,P_re,P_b,cv::Scalar(0, 96, 255),2);
+ }else{
+ line(img_src,P_re,P_b,cv::Scalar(0, 96, 255),2);
+ }
+
+ }else{
+ P_re.x = (P_b.x-circles.x) * cos(PI/180*15) - (P_b.y-circles.y) * sin(PI/180*15) + circles.x;
+ P_re.y = (P_b.x-circles.x) * sin(PI/180*15) + (P_b.y-circles.y) * cos(PI/180*15) + circles.y;
+ if(imgDil.at((cv::Point)((P_re-circles)*0.5+circles))!=0){
+ P_re.x = (P_b.x-circles.x) * cos(PI/180*15*(-1.0)) - (P_b.y-circles.y) * sin(PI/180*15*(-1.0)) + circles.x;
+ P_re.y = (P_b.x-circles.x) * sin(PI/180*15*(-1.0)) + (P_b.y-circles.y) * cos(PI/180*15*(-1.0)) + circles.y;
+ line(img_src,P_a,P_re,cv::Scalar(0, 96, 255),2);
+ }else{
+ line(img_src,P_a,P_re,cv::Scalar(0, 96, 255),2);
+ }
+
+ }
+ if(abs((boundRect[i].br().x-boundRect[i].tl().x)*1.0/(boundRect[i].br().y-boundRect[i].tl().y)*1.0-1.0)<0.2){
+ armor_check tmp;
+ tmp.cord[0].x=((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*38.0/39.0+circles.x;
+ tmp.cord[0].y=((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*38.0/39.0+circles.y;
+ tmp.cord[1].x=((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*36.0/39.0+circles.x;
+ tmp.cord[1].y=((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*36.0/39.0+circles.y;
+ tmp.cord[2].x=((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*41.0/39.0+circles.x;
+ tmp.cord[2].y=((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*41.0/39.0+circles.y;
+ tmp.cord[3].x=((boundRect[i].tl().x+boundRect[i].br().x)/2-circles.x)*43.0/39.0+circles.x;
+ tmp.cord[3].y=((boundRect[i].tl().y+boundRect[i].br().y)/2-circles.y)*43.0/39.0+circles.y;
+ tmp.obj.x=(boundRect[i].tl().x+boundRect[i].br().x)/2;
+ tmp.obj.y=(boundRect[i].tl().y+boundRect[i].br().y)/2;
+ cv::circle(img_src,{tmp.cord[0].x,tmp.cord[0].y},1,cv::Scalar(100, 255, 125),2);
+ cv::circle(img_src,{tmp.cord[1].x,tmp.cord[1].y},1,cv::Scalar(100, 255, 125),2);
+ cv::circle(img_src,{tmp.cord[2].x,tmp.cord[2].y},1,cv::Scalar(100, 255, 125),2);
+ cv::rectangle(img_src, boundRect[i].tl() ,boundRect[i].br(),cv::Scalar(200, 255, 125), 2, 8);
+ cv::putText(img_src, std::to_string(area), { boundRect[i].x,boundRect[i].y - 5 },1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
+ tmp.d=returnDistance(boundRect[i].tl(),boundRect[i].br());
+ cv::putText(img_src, std::to_string(tmp.d), { tmp.obj.x,tmp.obj.y },1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
+ if(tmp.d<100.0){
+ tmp.isPass=true;
+ inaction_check.push_back(tmp);
+ cv::circle(img_src,{tmp.cord[2].x,tmp.cord[2].y},1,cv::Scalar(255, 255, 125),2);
+ }
+ cv::imshow("inaction",img_src);
+
+ }
+ }*/
}
/*
if(moudle==INACTION_MODE&&area>150&&area<300&&area_rect>200&&area_rect<390&&hierarchy[i][3]>= 1)
@@ -210,11 +217,11 @@ namespace new_buff {
cv::line(img_src,{(tlx+brx)/2,(tly+bry)/2},circles,cv::Scalar(0,255,255),2);
fmt::print("[armor action] cir {} {}\n",circles.x,circles.y);
cv::putText(img_src, std::to_string(imgDil.at(inaction_check[j].cord[0])), { inaction_check[j].cord[0].x,inaction_check[j].cord[0].y-5},1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1); //}
-
+
cv::putText(img_src, std::to_string(area_rect), { boundRect[i].x,boundRect[i].y - 5 },1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
armor_last_rects = armor_now_rects;
tan_angle = atan(abs(armor_now.y-circles.y)/abs(armor_now.x-circles.x));
-
+
armor_now_rects = cv::RotatedRect(armor_now , cv::Size(abs(brx-tlx), abs(bry-tly)) , tan_angle);
cv::rectangle(img_src, armor_now_rects.boundingRect(),cv::Scalar(0, 255, 0), 2, 8);
}
@@ -226,66 +233,64 @@ namespace new_buff {
}*/
/*if(moudle==ACTION_MODE&&((area>1700&&area<1770))&&(hierarchy[i][3]>= 0&&hierarchy[i][3]<=3))
{
-
+
objectType = "Circle";
//fmt::print("[img process] Circle {} area {}\n",objCor,area);
drawContours(img, conPoly, i, cv::Scalar(255, 0, 255), 2);
rectangle(img_src, boundRect[i].tl(), boundRect[i].br(), cv::Scalar(255, 255, 0), 5);//框出图形
cv::putText(img_src, std::to_string(area), { boundRect[i].x,boundRect[i].y - 5 },1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
}*/
- isfindcircleR=false;
- //cv::imshow("cir_img",imgDil);
- if(moudle==CIRCLE_MODE&&area>10&&area<15&&abs((boundRect[i].br().x-boundRect[i].tl().x)*1.0/(boundRect[i].br().y-boundRect[i].tl().y)*1.0-1.0)<0.3&&area_rect>10&&area_rect<32&&hierarchy[i][3]==-1)
- //if(moudle==CIRCLE_MODE&&area>400&&area<500&&abs((boundRect[i].br().x-boundRect[i].tl().x)*1.0/(boundRect[i].br().y-boundRect[i].tl().y)*1.0-1.0)<0.3&&area_rect>500&&area_rect<1000/*&&hierarchy[i][3]==1*/)
+ isfindcircleR = false;
+ // cv::imshow("cir_img",imgDil);
+ if (moudle == CIRCLE_MODE && area > 10 && area < 15 && abs((boundRect[i].br().x - boundRect[i].tl().x) * 1.0 / (boundRect[i].br().y - boundRect[i].tl().y) * 1.0 - 1.0) < 0.3 && area_rect > 10 && area_rect < 32 && hierarchy[i][3] == -1)
+ // if(moudle==CIRCLE_MODE&&area>400&&area<500&&abs((boundRect[i].br().x-boundRect[i].tl().x)*1.0/(boundRect[i].br().y-boundRect[i].tl().y)*1.0-1.0)<0.3&&area_rect>500&&area_rect<1000/*&&hierarchy[i][3]==1*/)
{
- cv::imshow("img_cir",imgDil);
+ cv::imshow("img_cir", imgDil);
objectType = "Circle";
- fmt::print("[img process] Circle {} area {}\n",objCor,area);
- fmt::print("[img process] hier {}\n",hierarchy[i][3]);
- //drawContours(img, conPoly, i, cv::Scalar(255, 0, 255), 2);
-
- cv::putText(img_src, std::to_string(area_rect), { boundRect[i].x,boundRect[i].y - 5 },1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
+ fmt::print("[img process] Circle {} area {}\n", objCor, area);
+ fmt::print("[img process] hier {}\n", hierarchy[i][3]);
+ // drawContours(img, conPoly, i, cv::Scalar(255, 0, 255), 2);
+
+ cv::putText(img_src, std::to_string(area_rect), {boundRect[i].x, boundRect[i].y - 5}, 1, cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
tlx = boundRect[i].tl().x;
- tlx =(int)kal.run(boundRect[i].tl().x);
+ tlx = (int)kal.run(boundRect[i].tl().x);
tly = boundRect[i].tl().y;
- tly =(int)kal.run(boundRect[i].tl().y);
+ tly = (int)kal.run(boundRect[i].tl().y);
brx = boundRect[i].br().x;
- brx =(int)kal.run(boundRect[i].br().x);
+ brx = (int)kal.run(boundRect[i].br().x);
bry = boundRect[i].br().y;
- bry =(int)kal.run(boundRect[i].br().y);
- //circles=(boundRect[i].tl()+boundRect[i].br())/2;
- last_r=returnDistance(circles,armor_last);
- now_r=returnDistance(circles,armor_now);
- last_r=(last_r+now_r)/2.0;
- now_r=returnDistance({(tlx+brx)/2,(tly+bry)/2},armor_now);
+ bry = (int)kal.run(boundRect[i].br().y);
+ // circles=(boundRect[i].tl()+boundRect[i].br())/2;
+ last_r = returnDistance(circles, armor_last);
+ now_r = returnDistance(circles, armor_now);
+ last_r = (last_r + now_r) / 2.0;
+ now_r = returnDistance({(tlx + brx) / 2, (tly + bry) / 2}, armor_now);
/*if(fabs(last_r-now_r)/((last_r+now_r)/2.0)>0.1&&fabs(armor_last.x-0.0)>0.01&&fabs(armor_last.y-0.0)>0.01)
{
fmt::print("[check] Err 1\n");
continue;
}*/
- rectangle(img_src, boundRect[i].tl(), boundRect[i].br(), cv::Scalar(0, 255, 255), 2);//框出图形
- cv::imshow("circle",img_src);
+ rectangle(img_src, boundRect[i].tl(), boundRect[i].br(), cv::Scalar(0, 255, 255), 2); // 框出图形
+ cv::imshow("circle", img_src);
/*if(returnDistance(circles,{tlx,tly})<700.0&&returnDistance(circles,{tlx,tly})>50.0&&last_circles.x!=0&&last_circles.y!=0){
continue;
}*/
- //if(returnDistance({(tlx+brx)/2,(tly+bry)/2},armor_now))
-
-
- last_circles=circles;
- circles.x=(tlx+brx)/2;
- circles.y=(tly+bry)/2;
- fmt::print("[img process] circle {} {}\n", circles.x,circles.y);
- //cv::line(img_src,armor_now,circles,cv::Scalar(0,255,255),2);
-
- isfindcircleR=true;
-
+ // if(returnDistance({(tlx+brx)/2,(tly+bry)/2},armor_now))
+ last_circles = circles;
+ circles.x = (tlx + brx) / 2;
+ circles.y = (tly + bry) / 2;
+ fmt::print("[img process] circle {} {}\n", circles.x, circles.y);
+ // cv::line(img_src,armor_now,circles,cv::Scalar(0,255,255),2);
+
+ isfindcircleR = true;
}
- now_r=0;
- for(int j=0;j < inaction_check.size(); ++j){
+ now_r = 0;
+ for (int j = 0; j < inaction_check.size(); ++j)
+ {
int col_cnt;
- col_cnt=0;
-
+ col_cnt = 0;
+
/*for(int k=0;kinaction_check[j].cord.x&&boundRect[k].br().y>inaction_check[j].cord.y){
@@ -294,70 +299,71 @@ namespace new_buff {
break;
}
}*/
- //cv::circle(img_src,inaction_check[j].obj,2,cv::Scalar(255,0,255),1);
- cv::imshow("check",img_src);
- for(int k=38;k<=54;k++){
- cv::circle(img_src,{(inaction_check[j].obj.x-circles.x)*k*1.0/47.0+circles.x,(inaction_check[j].obj.y-circles.y)*k*1.0/47.0+circles.y},3,cv::Scalar(255,0,255),3);
- col_cnt=col_cnt+imgDil.at({(inaction_check[j].obj.x-circles.x)*k*1.0/47.0+circles.x,(inaction_check[j].obj.y-circles.y)*k*1.0/47.0+circles.y});
+ // cv::circle(img_src,inaction_check[j].obj,2,cv::Scalar(255,0,255),1);
+ cv::imshow("check", img_src);
+ for (int k = 38; k <= 54; k++)
+ {
+ cv::circle(img_src, {(inaction_check[j].obj.x - circles.x) * k * 1.0 / 47.0 + circles.x, (inaction_check[j].obj.y - circles.y) * k * 1.0 / 47.0 + circles.y}, 3, cv::Scalar(255, 0, 255), 3);
+ col_cnt = col_cnt + imgDil.at({(inaction_check[j].obj.x - circles.x) * k * 1.0 / 47.0 + circles.x, (inaction_check[j].obj.y - circles.y) * k * 1.0 / 47.0 + circles.y});
}
- cv::putText(img_src, std::to_string(col_cnt/(24.0*255.0)), { inaction_check[j].obj.x,inaction_check[j].obj.y-5},1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 255, 255), 2);
- fmt::print("[check] {}\n",col_cnt);
-
- if(col_cnt/(16.0*255.0)<0.29||returnDistance(inaction_check[j].obj,circles)<43.0){
- inaction_check[j].isPass=false;
+ cv::putText(img_src, std::to_string(col_cnt / (24.0 * 255.0)), {inaction_check[j].obj.x, inaction_check[j].obj.y - 5}, 1, cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 255, 255), 2);
+ fmt::print("[check] {}\n", col_cnt);
+
+ if (col_cnt / (16.0 * 255.0) < 0.29 || returnDistance(inaction_check[j].obj, circles) < 43.0)
+ {
+ inaction_check[j].isPass = false;
continue;
}
fmt::print("[check] pass\n");
- inaction_check[j].isPass=true;
- //cv::putText(img_src, std::to_string(imgDil.at(inaction_check[j].cord[0])), { 30,20 },1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
- //cv::putText(img_src, std::to_string(imgDil.at(inaction_check[j].cord[0])), { inaction_check[j].cord[0].x,inaction_check[j].cord[0].y-5},1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
- //cv::putText(img_src, std::to_string(imgDil.at(inaction_check[j].cord[1])), { inaction_check[j].cord[1].x,inaction_check[j].cord[1].y-5},1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
- //cv::putText(img_src, std::to_string(imgDil.at(inaction_check[j].cord[2])), { inaction_check[j].cord[2].x,inaction_check[j].cord[2].y-5},1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
- //cv::putText(img_src, std::to_string(imgDil.at(inaction_check[j].cord[3])), { inaction_check[j].cord[3].x,inaction_check[j].cord[3].y-5},1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
+ inaction_check[j].isPass = true;
+ // cv::putText(img_src, std::to_string(imgDil.at(inaction_check[j].cord[0])), { 30,20 },1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
+ // cv::putText(img_src, std::to_string(imgDil.at(inaction_check[j].cord[0])), { inaction_check[j].cord[0].x,inaction_check[j].cord[0].y-5},1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
+ // cv::putText(img_src, std::to_string(imgDil.at(inaction_check[j].cord[1])), { inaction_check[j].cord[1].x,inaction_check[j].cord[1].y-5},1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
+ // cv::putText(img_src, std::to_string(imgDil.at(inaction_check[j].cord[2])), { inaction_check[j].cord[2].x,inaction_check[j].cord[2].y-5},1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
+ // cv::putText(img_src, std::to_string(imgDil.at(inaction_check[j].cord[3])), { inaction_check[j].cord[3].x,inaction_check[j].cord[3].y-5},1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
/*if(imgDil.at(inaction_check[j].cord[0])+imgDil.at(inaction_check[j].cord[1])+imgDil.at(inaction_check[j].cord[2])+imgDil.at(inaction_check[j].cord[3])==0){
inaction_check[j].isPass=true;
//cv::putText(img_src, std::to_string(imgDil.at(inaction_check[j].cord[0])+imgDil.at(inaction_check[j].cord[1])+imgDil.at(inaction_check[j].cord[2])+imgDil.at(inaction_check[j].cord[3])), { inaction_check[j].obj.x,inaction_check[j].obj.y-5},1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
-
+
}else{
inaction_check[j].isPass=false;
}*/
}
-
- //circles.x=439;
- //circles.y=388;
+
+ // circles.x=439;
+ // circles.y=388;
/*if(!isfindcircleR)
{
circles.x=0;
circles.y=0;
}*/
- }
+ }
}
}
- for(int i=0;i0.1&&fabs(armor_last.x-0.0)>0.01&&fabs(armor_last.y-0.0)>0.01)
{
@@ -365,22 +371,22 @@ namespace new_buff {
circles={0,0};
continue;
}*/
- cv::line(img_src,armor_now,circles,cv::Scalar(0,255,255),2);
+ cv::line(img_src, armor_now, circles, cv::Scalar(0, 255, 255), 2);
armor_last_rects = armor_now_rects;
-
- tan_angle = atan(abs(armor_now.y-circles.y)/abs(armor_now.x-circles.x));
-
- armor_now_rects = cv::RotatedRect(armor_now , cv::Size((int)inaction_check[i].d, (int)inaction_check[i].d) , tan_angle);
- //cv::putText(img_src, std::to_string(inaction_check[i].d), { inaction_check[i].obj.x,inaction_check[i].obj.y-5},1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
- cv::rectangle(img_src, armor_now_rects.boundingRect(),cv::Scalar(0, 200, 0), 2, 8);
-
- cv::putText(img_src, "Dectected", {10,10},1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 255, 255), 2);
- cv::imshow("[dec]",img_src);
+
+ tan_angle = atan(abs(armor_now.y - circles.y) / abs(armor_now.x - circles.x));
+
+ armor_now_rects = cv::RotatedRect(armor_now, cv::Size((int)inaction_check[i].d, (int)inaction_check[i].d), tan_angle);
+ // cv::putText(img_src, std::to_string(inaction_check[i].d), { inaction_check[i].obj.x,inaction_check[i].obj.y-5},1,cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 69, 255), 1);
+ cv::rectangle(img_src, armor_now_rects.boundingRect(), cv::Scalar(0, 200, 0), 2, 8);
+
+ cv::putText(img_src, "Dectected", {10, 10}, 1, cv::FONT_HERSHEY_PLAIN, cv::Scalar(0, 255, 255), 2);
+ cv::imshow("[dec]", img_src);
}
}
- if(cnt_obj==0)
+ if (cnt_obj == 0)
{
- isFindTarget=false;
+ isFindTarget = false;
}
/*for(int i=0;i velocity) {
- if(min_velocity > velocity) {
+ if (now_velocity > velocity)
+ {
+ if (min_velocity > velocity)
+ {
min_velocity = velocity;
}
last_timex = cv::getTickCount();
- if(status == MAX) { // 若上一状态为增，则上一状态所在点定为极大值
+ if (status == MAX)
+ { // 若上一状态为增，则上一状态所在点定为极大值
start_T = true;
sign = -1;
timex = last_timex; // 函数初始时间
- } else {
+ }
+ else
+ {
status = MIN;
}
last_timex = cv::getTickCount();
@@ -516,44 +542,42 @@ namespace new_buff {
last_velocity = now_velocity;
now_velocity = velocity;
}
-
}
- cv::Point2f buff::calculateCord(cv::Point2f &circle_r) {
+ cv::Point2f buff::calculateCord(cv::Point2f &circle_r)
+ {
fmt::print("[buff] A:{} omega:{} sign:{} alpha:{}\n", A, omega, sign, alpha);
- beta = - A / omega * (cos(omega * ((cv::getTickCount() - timex) / cv::getTickFrequency() + gimbal_delay / 1000.0) + sign * PI / 4.0) -
- cos(omega * ((cv::getTickCount() - timex) / cv::getTickFrequency()) + sign * PI / 4.0))
- + (2.090 - A) * (gimbal_delay / 1000.0);
- fmt::print("[buff cal] {}\n",beta);
+ beta = -A / omega * (cos(omega * ((cv::getTickCount() - timex) / cv::getTickFrequency() + gimbal_delay / 1000.0) + sign * PI / 4.0) - cos(omega * ((cv::getTickCount() - timex) / cv::getTickFrequency()) + sign * PI / 4.0)) + (2.090 - A) * (gimbal_delay / 1000.0);
+ fmt::print("[buff cal] {}\n", beta);
fmt::print("[buff] beta:{}\n", beta);
object.x = armor_now.x * cos(beta) - armor_now.y * sin(beta);
object.y = armor_now.x * sin(beta) + armor_now.y * cos(beta);
return object;
}
- void buff::main_buff_checker(cv::Mat imgGray,cv::Mat img_src,new_buff::Check_Moudle moudle){
- cv::Mat /*imgGray, */imgBlur, imgCanny, imgDila, imgErode, imgDila2, imgGray2;
- cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
+ void buff::main_buff_checker(cv::Mat imgGray, cv::Mat img_src, new_buff::Check_Moudle moudle)
+ {
+ cv::Mat /*imgGray, */ imgBlur, imgCanny, imgDila, imgErode, imgDila2, imgGray2;
+ cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
imgDila = imgGray.clone();
imgGray2 = imgGray.clone();
- cv::imshow("act_img_11",imgGray2);
-
- //preprocessing
- //cvtColor(img, imgGray,cv::COLOR_BGR2GRAY, 0);
- /*GaussianBlur(imgGray, imgBlur, cv::Size(65, 65), 1, 1);
- Canny(imgBlur, imgCanny, 40, 120);
- dilate(imgCanny, imgDila, kernel);*/
- //morphologyEx(imgDila, imgDila, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 5);
- //morphologyEx(imgDila, imgDila, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 2);
- //morphologyEx(imgDila, imgDila, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 7);
- //morphologyEx(imgDila, imgDila, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 1);
- if(moudle==INACTION_MODE)
+ cv::imshow("act_img_11", imgGray2);
+
+ // preprocessing
+ // cvtColor(img, imgGray,cv::COLOR_BGR2GRAY, 0);
+ /*GaussianBlur(imgGray, imgBlur, cv::Size(65, 65), 1, 1);
+ Canny(imgBlur, imgCanny, 40, 120);
+ dilate(imgCanny, imgDila, kernel);*/
+ // morphologyEx(imgDila, imgDila, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 5);
+ // morphologyEx(imgDila, imgDila, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 2);
+ // morphologyEx(imgDila, imgDila, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 7);
+ // morphologyEx(imgDila, imgDila, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 1);
+ if (moudle == INACTION_MODE)
{
- //morphologyEx(imgGray, imgGray, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 1);
-
- //morphologyEx(imgGray, imgGray, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 1);
- //morphologyEx(imgGray, imgGray, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 1);
+ // morphologyEx(imgGray, imgGray, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 1);
+ // morphologyEx(imgGray, imgGray, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 1);
+ // morphologyEx(imgGray, imgGray, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 1);
/*morphologyEx(imgGray, imgGray, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 6);
morphologyEx(imgGray, imgGray, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 2);
@@ -561,85 +585,88 @@ namespace new_buff {
morphologyEx(imgGray, imgGray, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 1);
*/
-
/*morphologyEx(imgGray, imgGray, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 10);
morphologyEx(imgGray, imgGray, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 3);
morphologyEx(imgGray, imgGray, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 7);
morphologyEx(imgGray, imgGray, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 1);*/
- //morphologyEx(imgGray, imgGray, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 4);
- //morphologyEx(imgGray, imgGray, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 1);
- //GaussianBlur(imgGray, imgBlur, cv::Size(65, 65), 1, 1);
- //Canny(imgBlur, imgCanny, 40, 120);
- //dilate(imgCanny, imgDila, kernel);
-
+ // morphologyEx(imgGray, imgGray, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 4);
+ // morphologyEx(imgGray, imgGray, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 1);
+ // GaussianBlur(imgGray, imgBlur, cv::Size(65, 65), 1, 1);
+ // Canny(imgBlur, imgCanny, 40, 120);
+ // dilate(imgCanny, imgDila, kernel);
}
/*if(moudle==ACTION_MODE)
{
morphologyEx(imgGray, imgGray, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 3);
morphologyEx(imgGray, imgGray, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 2);
}*/
- if(moudle==CIRCLE_MODE)
+ if (moudle == CIRCLE_MODE)
{
morphologyEx(imgGray, imgGray, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 1);
morphologyEx(imgGray, imgGray, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 1);
-
-/*
- morphologyEx(imgGray, imgGray, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 2);
- morphologyEx(imgGray, imgGray, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 2);*/
+
+ /*
+ morphologyEx(imgGray, imgGray, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 2);
+ morphologyEx(imgGray, imgGray, cv::MORPH_ERODE, ele_, cv::Point(-1, -1), 2);*/
/*GaussianBlur(imgGray, imgBlur, cv::Size(65, 65), 1, 1);
- Canny(imgBlur, imgCanny, 40, 120);
- dilate(imgCanny, imgDila, kernel);
+ Canny(imgBlur, imgCanny, 40, 120);
+ dilate(imgCanny, imgDila, kernel);
cv::imshow("cir_img",imgDila);*/
}
GaussianBlur(imgGray, imgBlur, cv::Size(65, 65), 1, 1);
- //Canny(imgBlur, imgCanny, 40, 120);
- //dilate(imgCanny, imgDila, kernel);
-
- cv::imshow("act_img",imgDila);
-//灰度->高斯滤波->Canny边缘算法->膨胀
- //morphologyEx(imgGray2, imgGray2, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 1);
- buff::getcontour(imgDila,imgGray2,img_src,moudle);
+ // Canny(imgBlur, imgCanny, 40, 120);
+ // dilate(imgCanny, imgDila, kernel);
+
+ cv::imshow("act_img", imgDila);
+ // 灰度->高斯滤波->Canny边缘算法->膨胀
+ // morphologyEx(imgGray2, imgGray2, cv::MORPH_DILATE, ele_, cv::Point(-1, -1), 1);
+ buff::getcontour(imgDila, imgGray2, img_src, moudle);
#ifndef RELEASE
- //imshow("Image", imgGray);
- //imshow("Image2", imgDila);
- //imshow("detect inaction", img_src);
+ // imshow("Image", imgGray);
+ // imshow("Image2", imgDila);
+ // imshow("detect inaction", img_src);
#endif
- //imshow("Image Gray", imgGray);
- //imshow("Image Blur", imgBlur);
- //imshow("Image Canny", imgCanny);
- //imshow("Image dila", imgDila);
+ // imshow("Image Gray", imgGray);
+ // imshow("Image Blur", imgBlur);
+ // imshow("Image Canny", imgCanny);
+ // imshow("Image dila", imgDila);
}
- cv::Point2f buff::calculateCircleR(cv::Point2f P1,cv::Point2f P2,cv::Point2f P3){
- float a,b,c,d,e,f;
+ cv::Point2f buff::calculateCircleR(cv::Point2f P1, cv::Point2f P2, cv::Point2f P3)
+ {
+ float a, b, c, d, e, f;
cv::Point2f cir;
- a=2*(P2.x-P1.x);
- b=2*(P2.y-P1.y);
- c=P2.x*P2.x+P2.y*P2.y-P1.x*P1.x-P1.y*P1.y;
- d=2*(P3.x-P2.x);
- e=2*(P3.y-P2.y);
- f=P3.x*P3.x+P3.y*P3.y-P2.x*P2.x-P2.y*P2.y;
- cir.x=(b*f-e*c)/(b*d-e*a);
- cir.y=(d*c-a*f)/(b*d-e*a);
+ a = 2 * (P2.x - P1.x);
+ b = 2 * (P2.y - P1.y);
+ c = P2.x * P2.x + P2.y * P2.y - P1.x * P1.x - P1.y * P1.y;
+ d = 2 * (P3.x - P2.x);
+ e = 2 * (P3.y - P2.y);
+ f = P3.x * P3.x + P3.y * P3.y - P2.x * P2.x - P2.y * P2.y;
+ cir.x = (b * f - e * c) / (b * d - e * a);
+ cir.y = (d * c - a * f) / (b * d - e * a);
return cir;
}
- cv::Point2f buff::returnCircleR(){
+ cv::Point2f buff::returnCircleR()
+ {
return circles;
}
- double buff::returnDistance(cv::Point2f x,cv::Point2f y){
- return sqrt((x.x-y.x)*(x.x-y.x)+(x.y-y.y)*(x.y-y.y));
+ double buff::returnDistance(cv::Point2f x, cv::Point2f y)
+ {
+ return sqrt((x.x - y.x) * (x.x - y.x) + (x.y - y.y) * (x.y - y.y));
}
- cv::RotatedRect buff::returnArmorRect() {
- fmt::print("[obj rect] out {}\n",object_rects.size.height);
+ cv::RotatedRect buff::returnArmorRect()
+ {
+ fmt::print("[obj rect] out {}\n", object_rects.size.height);
return object_rects;
}
- bool buff::returnCenterDistance(){
+ bool buff::returnCenterDistance()
+ {
return center_dist;
}
}
diff --git a/module/buff/new_buff.hpp b/module/buff/new_buff.hpp
index dcb3797..3eab210 100644
--- a/module/buff/new_buff.hpp
+++ b/module/buff/new_buff.hpp
@@ -3,9 +3,9 @@
* @author dgsyrc (yrcminecraft@foxmail.com)
* @brief 新能量机关检测
* @date 2023-01-15
- *
+ *
* @copyright Copyright (c) 2023 Sirius
- *
+ *
*/
#pragma once
@@ -25,15 +25,14 @@
#include "utils/fps.hpp"
#include "module/filter/basic_kalman.hpp"
-
-
-
-namespace new_buff {
+namespace new_buff
+{
/***
* @brief 检测模式
*/
- cv::Mat ele_ = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)); // 椭圆内核
- enum Check_Moudle {
+ cv::Mat ele_ = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)); // 椭圆内核
+ enum Check_Moudle
+ {
ACTION_MODE,
INACTION_MODE,
CIRCLE_MODE,
@@ -50,7 +49,7 @@ namespace new_buff {
basic_kalman::firstKalman kal;
float tan_angle;
-
+
bool isfindcircleR = false;
bool isFindTarget = false;
@@ -65,96 +64,91 @@ namespace new_buff {
int tlx;
int tly;
- double now_r=0.0f;
- double last_r=0.0f;
+ double now_r = 0.0f;
+ double last_r = 0.0f;
- struct armor_check{
+ struct armor_check
+ {
cv::Point2f cord[4];
cv::Point2f obj;
bool isPass = false;
float d;
};
- class buff {
- public:
-
- enum predict_status {
- MAX,
- MIN,
- NONE,
- };
- void getcontour(cv::Mat imgDil,cv::Mat img, cv::Mat& img_src,new_buff::Check_Moudle moudle);
-
- void set_config(std::string config_path);
+ class buff
+ {
+ public:
+ enum predict_status
+ {
+ MAX,
+ MIN,
+ NONE,
+ };
+ void getcontour(cv::Mat imgDil, cv::Mat img, cv::Mat &img_src, new_buff::Check_Moudle moudle);
- cv::Point2f stablePerdict(cv::Point2f &circle_r,cv::Mat &img);
+ void set_config(std::string config_path);
- void predict(cv::Point2f &circle_r,cv::Mat &img);
+ cv::Point2f stablePerdict(cv::Point2f &circle_r, cv::Mat &img);
- cv::Point2f calculateCord(cv::Point2f &circle_r);
+ void predict(cv::Point2f &circle_r, cv::Mat &img);
- void main_buff_checker(cv::Mat img, cv::Mat img_src,new_buff::Check_Moudle moudle);
+ cv::Point2f calculateCord(cv::Point2f &circle_r);
- cv::Point2f calculateCircleR(cv::Point2f P1,cv::Point2f P2,cv::Point2f P3);
+ void main_buff_checker(cv::Mat img, cv::Mat img_src, new_buff::Check_Moudle moudle);
- cv::Point2f returnCircleR();
+ cv::Point2f calculateCircleR(cv::Point2f P1, cv::Point2f P2, cv::Point2f P3);
- double returnDistance(cv::Point2f x,cv::Point2f y);
+ cv::Point2f returnCircleR();
- cv::RotatedRect returnArmorRect();
+ double returnDistance(cv::Point2f x, cv::Point2f y);
- bool returnCenterDistance();
+ cv::RotatedRect returnArmorRect();
- private:
+ bool returnCenterDistance();
- struct info {
- float armor_height;
- float armor_lenght;
- float pic_armor_height;
- float pic_armor_lenght;
- float pic_distance;
- float armor_distance;
- } config;
+ private:
+ struct info
+ {
+ float armor_height;
+ float armor_lenght;
+ float pic_armor_height;
+ float pic_armor_lenght;
+ float pic_distance;
+ float armor_distance;
+ } config;
-
- predict_status status = NONE;
+ predict_status status = NONE;
-
+ float last_velocity = 0.0;
+ float now_velocity = 0.0;
+ float min_velocity = 0.0;
+ float max_velocity = 0.0;
- float last_velocity= 0.0;
- float now_velocity = 0.0;
- float min_velocity = 0.0;
- float max_velocity = 0.0;
+ double timex;
+ double last_timex;
+ float T;
- double timex;
- double last_timex;
- float T;
+ bool start_T = false; // 检测到第一个最值标志
- bool start_T = false; // 检测到第一个最值标志
+ cv::Point2f relay;
+ bool first_T_finish = false;
+ float arg[3] = {0};
+ float alpha = 0.0;
+ float omega = 0.0;
+ float max_v = 0.0;
+ float min_v = 0.0;
+ float d_t;
+ float velocity;
+ float A = 1.0;
- cv::Point2f relay;
- bool first_T_finish = false;
- float arg[3]= {0};
- float alpha = 0.0;
- float omega = 0.0;
- float max_v = 0.0;
- float min_v = 0.0;
- float d_t;
- float velocity;
- float A = 1.0;
+ float R;
- float R;
+ int sign = 1;
- int sign = 1;
+ float beta = 0.0;
+ cv::Point2f object;
- float beta = 0.0;
- cv::Point2f object;
-
- double center_dist;
+ double center_dist;
};
}
-
-
-
-
diff --git a/module/camera/camera_calibration.cpp b/module/camera/camera_calibration.cpp
index 429ed81..bd5e432 100644
--- a/module/camera/camera_calibration.cpp
+++ b/module/camera/camera_calibration.cpp
@@ -9,41 +9,48 @@
#include "camera_calibration.hpp"
#define RELEASE
-namespace cam{
+namespace cam
+{
- void create_images(cv::Mat& frame) {
+ void create_images(cv::Mat &frame)
+ {
#ifndef RELEASE
imshow("frame", frame);
ch = cv::waitKey(50);
#endif
-
+
printf("%d ", ch);
- img=frame.clone();
+ img = frame.clone();
s = img.size();
cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
cv::drawChessboardCorners(img, cv::Size(7, 7), corners, ret);
imagePoints.push_back(corners);
objectPoints.push_back(obj);
- //cv::imshow("calibration-demo", img);
- if (ch == 'e') { // 获取图像
- cv::imwrite(fmt::format("{}/photos/{}.png",SAVE_FILE_PATH, std::to_string(index)) , frame);
+ // cv::imshow("calibration-demo", img);
+ if (ch == 'e')
+ { // 获取图像
+ cv::imwrite(fmt::format("{}/photos/{}.png", SAVE_FILE_PATH, std::to_string(index)), frame);
fmt::print("SUCCESS\n");
index += 1;
}
}
- void calibrate(){
- cv::glob(fmt::format("{}/photos",SAVE_FILE_PATH), files);
-
+ void calibrate()
+ {
+ cv::glob(fmt::format("{}/photos", SAVE_FILE_PATH), files);
+
// 定义变量
- for (int i = 0; i < numCornersHor; i++){
- for (int j = 0; j < numCornersVer; j++){
+ for (int i = 0; i < numCornersHor; i++)
+ {
+ for (int j = 0; j < numCornersVer; j++)
+ {
obj.push_back(cv::Point3f((float)j * numSquares, (float)i * numSquares, 0));
}
}
// 发现棋盘格与绘制
- for (int i = 0; i < files.size(); i++) {
+ for (int i = 0; i < files.size(); i++)
+ {
fmt::print("image file:{}\n", files[i].c_str());
img = cv::imread(files[i]);
s = img.size();
@@ -52,7 +59,8 @@ namespace cam{
cv::imshow("calibration-demo", gray);
#endif
ret = cv::findChessboardCorners(gray, cv::Size(7, 7), corners, cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FILTER_QUADS);
- if (ret) {
+ if (ret)
+ {
cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1), criteria);
cv::drawChessboardCorners(img, cv::Size(7, 7), corners, ret);
imagePoints.push_back(corners);
@@ -74,9 +82,12 @@ namespace cam{
conf_file.release();
}
- void auto_create_images(cv::Mat& image){
- for (int i = 0; i < numCornersHor; i++){
- for (int j = 0; j < numCornersVer; j++){
+ void auto_create_images(cv::Mat &image)
+ {
+ for (int i = 0; i < numCornersHor; i++)
+ {
+ for (int j = 0; j < numCornersVer; j++)
+ {
obj.push_back(cv::Point3f((float)j * numSquares, (float)i * numSquares, 0));
}
}
@@ -86,8 +97,9 @@ namespace cam{
cv::imshow("calibration-demo", gray);
#endif
ret = cv::findChessboardCorners(gray, cv::Size(7, 7), corners, cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FILTER_QUADS);
- if (ret) {
- cv::imwrite(fmt::format("{}/photos/{}.png",SAVE_FILE_PATH, std::to_string(index)) , image);
+ if (ret)
+ {
+ cv::imwrite(fmt::format("{}/photos/{}.png", SAVE_FILE_PATH, std::to_string(index)), image);
cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1), criteria);
cv::drawChessboardCorners(image, cv::Size(7, 7), corners, ret);
imagePoints.push_back(corners);
@@ -98,10 +110,10 @@ namespace cam{
fmt::print("SUCCESS\n");
index += 1;
}
-
}
- void assess(cv::Mat& image){
+ void assess(cv::Mat &image)
+ {
cv::FileStorage camera_config(fmt::format("{}/mv_camera_config_337.xml", SAVE_FILE_PATH), cv::FileStorage::READ);
camera_config["camera-matrix"] >> intrinsic;
camera_config["distortion"] >> distCoeffs;
@@ -113,89 +125,76 @@ namespace cam{
camera_config.release();
}
- void CalibrationEvaluate()//标定结束后进行评价 NOT FIXED
- {
+ void CalibrationEvaluate() // 标定结束后进行评价 NOT FIXED
+ {
std::vector> objRealPoint;
- double err=0;
- double total_err=0;
- //calibrateCamera(objRealPoint, corners, Size(imageWidth, imageHeight), intrinsic, distCoeffs, rvecs, tvecs, 0);
+ double err = 0;
+ double total_err = 0;
+ // calibrateCamera(objRealPoint, corners, Size(imageWidth, imageHeight), intrinsic, distCoeffs, rvecs, tvecs, 0);
cv::FileStorage camera_config(fmt::format("{}/mv_camera_config_new.xml", SAVE_FILE_PATH), cv::FileStorage::READ);
camera_config["camera-matrix"] >> intrinsic;
camera_config["distortion"] >> distCoeffs;
- cv::glob(fmt::format("{}/photos",SAVE_FILE_PATH), files);
+ cv::glob(fmt::format("{}/photos", SAVE_FILE_PATH), files);
calRealPoint(objRealPoint, 7, 7, 7, 14);
-
- for (int i = 0; i < files.size(); i++) {
+
+ for (int i = 0; i < files.size(); i++)
+ {
fmt::print("image file:{}\n", files[i].c_str());
img = cv::imread(files[i]);
s = img.size();
- if(i == 0){
+ if (i == 0)
+ {
cv::calibrateCamera(objRealPoint, corners, s, intrinsic, distCoeffs, rvecs, tvecs, 0);
}
cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
ret = cv::findChessboardCorners(gray, cv::Size(7, 7), corners, cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FILTER_QUADS);
- if (ret) {
+ if (ret)
+ {
cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1), criteria);
corners_.push_back(corners);
}
}
-
- fmt::print("每幅图像的定标误差:");
- for (int i = 0; i < corners_.size(); i++)
- {
- std::vector image_points2;
- std::vector tempPointSet = objRealPoint[i];
- projectPoints(tempPointSet, rvecs[i], tvecs[i], intrinsic, distCoeffs, image_points2);
- std::vector tempImagePoint = corners_[i];
- cv::Mat tempImagePointMat = cv::Mat(1, tempImagePoint.size(), CV_32FC2);
- cv::Mat image_points2Mat = cv::Mat(1, image_points2.size(), CV_32FC2);
- for (int j = 0; j < tempImagePoint.size(); j++)
- {
- image_points2Mat.at(0, j) = cv::Vec2f(image_points2[j].x, image_points2[j].y);
- tempImagePointMat.at(0, j) = cv::Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
- }
- err = cv::norm(image_points2Mat, tempImagePointMat, cv::NORM_L2);
- total_err = err + total_err;
- fmt::print("第{}幅图像的平均误差:{}像素\n", std::to_string(i + 1), std::to_string(i + 1));
- }
- fmt::print("总体平均误差:{}像素\n", std::to_string(total_err / (corners.size() + 1)));
+
+ fmt::print("每幅图像的定标误差:");
+ for (int i = 0; i < corners_.size(); i++)
+ {
+ std::vector image_points2;
+ std::vector