VSAG/Catalyst-MDVS
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

项目结构大变

Browse Source
This commit is contained in:
Luo Yufei
2025-11-20 19:29:00 +08:00
parent 6a7ab65597
commit a8791ab3df
17 changed files with 549 additions and 287 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored
View File

@@ -34,4 +34,5 @@
# BUILD
build/
.cache/
.cache/
.vscode/

5
CMakeLists.txt
View File

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.10)
project(armor_detector_mdv)
# Set C++ standard
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD 23)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# Find OpenCV package
@@ -15,7 +15,7 @@ include_directories(./inc)
# Add executable for MindVision version
add_executable(armor_detector_mdv
src/main.cpp
src/MindVisionMain.cpp
src/MindVisionCamera.cpp
src/ImagePreprocessor.cpp
src/ArmorDetector.cpp
@@ -29,7 +29,6 @@ add_executable(armor_detector_mdv
target_link_libraries(armor_detector_mdv ${OpenCV_LIBS} -lutil -lpthread)
# Link MindVision SDK library
link_directories("/lib")
target_link_libraries(armor_detector_mdv MVSDK)
# Additional flags for better compilation

13
inc/BallisticPredictor.h
View File

@@ -6,21 +6,22 @@
class BallisticPredictor {
public:
BallisticPredictor(float bullet_speed = 16.0f);
~BallisticPredictor(); // Added destructor to properly manage memory
// Predict ballistic point considering target movement and flight time
cv::Point2f* predict_ballistic_point(const cv::Point2f* predicted_center,
const cv::Point2f& img_center,
cv::Point2f* predict_ballistic_point(const cv::Point2f* predicted_center,
const cv::Point2f& img_center,
const cv::Point2f& target_speed);
// Get last ballistic point
const cv::Point2f* get_last_ballistic_point() const { return last_ballistic_point; }
private:
float bullet_speed; // Bullet speed (m/s)
float armor_half_width; // Armor half width (converted to meters)
float armor_half_height; // Armor half height (converted to meters)
cv::Point2f* last_ballistic_point; // Last ballistic prediction
// Calculate distance to target
float calculate_distance(const cv::Point2f& armor_center, const cv::Point2f& img_center, float focal_length = 800.0f);
};

16
inc/ImagePreprocessor.h
View File

@@ -8,16 +8,12 @@ class ImagePreprocessor {
public:
ImagePreprocessor();
~ImagePreprocessor();
// Get mask based on target color
cv::Mat get_mask(const cv::Mat& frame, const std::string& target_color);
// Get frame with only the target color
cv::Mat get_color_only_frame(const cv::Mat& frame, const std::string& target_color);
// Combined function to return both mask and color-only frame
void process_frame(const cv::Mat& frame, const std::string& target_color,
cv::Mat& mask, cv::Mat& color_only_frame);
// Apply morphological operations to a mask
void apply_morphology(const cv::Mat& input_mask, cv::Mat& output_mask);
// Apply Gaussian blur to reduce noise
void apply_gaussian_blur(const cv::Mat& input, cv::Mat& output, int kernel_size = 6);
};
#endif // IMAGEPREPROCESSOR_H

18
inc/KalmanFilter.h
View File

@@ -6,12 +6,16 @@
class KalmanFilter {
public:
KalmanFilter();
void update(const cv::Point2f& measurement);
cv::Point2f predict();
// Method to update the process noise based on target movement characteristics
void updateProcessNoise(const cv::Point2f& current_measurement);
cv::Point2f get_last_measurement() const { return last_measurement; }
cv::Point2f get_last_prediction() const { return last_prediction; }
cv::Point2f get_current_velocity() const; // New method to get current velocity
bool is_initialized() const { return initialized; }
private:
@@ -19,8 +23,16 @@ private:
bool initialized;
cv::Point2f last_measurement;
cv::Point2f last_prediction;
cv::Point2f prev_measurement;
bool has_prev_measurement;
// Store previous states for velocity and acceleration estimation
cv::Point2f prev_velocity;
double time_elapsed; // Time in seconds between measurements
bool has_prev_velocity;
void init_params();
cv::Point2f estimate_velocity(const cv::Point2f& current, const cv::Point2f& previous);
};
#endif // KALMANFILTER_H

5
inc/MindVisionCamera.h
View File

@@ -3,6 +3,7 @@
#include <opencv2/opencv.hpp>
#include <string>
#include <memory>
// MindVision SDK 头文件 - 可能需要根据实际SDK文件调整
extern "C" {
@@ -17,12 +18,14 @@ public:
int width;
int height;
int fps;
unsigned char* g_pRgbBuffer; // 处理后数据缓存区
MindVisionCamera(int cam_id = 0, const std::string& target_color = "red");
~MindVisionCamera();
void set_cam_params();
bool set_cam_params();
bool read_frame(cv::Mat& frame);
bool read_frame_with_color_filter(cv::Mat& frame, cv::Mat& mask, const std::string& target_color);
void release();
bool switch_color(const std::string& target_color);

15
inc/TTLCommunicator.h
View File

@@ -3,17 +3,11 @@
#include <string>
// Placeholder for TTL communication class
// In a real implementation, you would use a C++ serial library like:
// - serial (https://github.com/wjwwood/serial)
// - Boost.Asio
// - or a system-specific approach
class TTLCommunicator {
public:
TTLCommunicator(int baudrate = 115200);
TTLCommunicator(const std::string& port_name = "/dev/ttyUSB0", int baudrate = 115200);
~TTLCommunicator();
bool connect();
void close();
bool send_data(const std::string& data);
@@ -23,8 +17,9 @@ private:
std::string port_name;
int baudrate;
bool connected;
// These would be implemented with actual serial communication code
int serial_fd;
bool open_serial_port();
void close_serial_port();
};

4
inc/config.h
View File

@@ -28,9 +28,9 @@ const float HORIZONTAL_ANGLE_THRESHOLD_RAD = HORIZONTAL_ANGLE_THRESHOLD * CV_PI
const float NEARBY_LIGHT_BAR_THRESHOLD = 500.0f; // Light bar merging distance threshold (pixels)
const float LIGHT_BAR_IOU_THRESHOLD = 0.05f; // Light bar merging IOU threshold
const float ARMOR_DISTANCE_RATIO_MIN = 0.6f; // Armor light bar distance ratio range
const float ARMOR_DISTANCE_RATIO_MAX = 3.0f;
const float ARMOR_DISTANCE_RATIO_MAX = 4.0f;
const float ARMOR_LENGTH_DIFF_RATIO = 0.5f; // Armor light bar length difference ratio
const float ARMOR_ANGLE_DIFF_THRESHOLD = 20.0f * CV_PI / 180.0f; // Armor light bar angle difference threshold (radians)
const float ARMOR_ANGLE_DIFF_THRESHOLD = 15.0f * CV_PI / 180.0f; // Armor light bar angle difference threshold (radians)
// Kalman filter parameters
const float KF_PROCESS_NOISE = 0.02f; // Process noise covariance

7
src/ArmorDetector.cpp
View File

@@ -1,4 +1,5 @@
#include "ArmorDetector.h"
#include <iostream>
#include <cmath>
#include <algorithm>
@@ -143,8 +144,10 @@ std::vector<LightBar> ArmorDetector::filter_valid_light_bars(const std::vector<s
float bar_width = width > height ? height : width;
float main_angle = rect.angle;
if (width <= height) {
main_angle += 90.0;
if (width > height) {
// 保持原始角度不变
} else {
main_angle += 90;
}
// Normalize angle to [-90, 90]

26
src/BallisticPredictor.cpp
View File

@@ -2,20 +2,29 @@
#include "config.h"
#include <cmath>
BallisticPredictor::BallisticPredictor(float bullet_speed)
BallisticPredictor::BallisticPredictor(float bullet_speed)
: bullet_speed(bullet_speed), last_ballistic_point(nullptr) {
armor_half_width = ARMOR_WIDTH / 2000.0f; // Convert to meters
armor_half_height = ARMOR_HEIGHT / 2000.0f; // Convert to meters
}
cv::Point2f* BallisticPredictor::predict_ballistic_point(const cv::Point2f* predicted_center,
const cv::Point2f& img_center,
BallisticPredictor::~BallisticPredictor() {
if (last_ballistic_point != nullptr) {
delete last_ballistic_point;
last_ballistic_point = nullptr;
}
}
cv::Point2f* BallisticPredictor::predict_ballistic_point(const cv::Point2f* predicted_center,
const cv::Point2f& img_center,
const cv::Point2f& target_speed) {
// Clean up the last ballistic point before creating a new one
if (last_ballistic_point != nullptr) {
delete last_ballistic_point;
last_ballistic_point = nullptr;
}
if (predicted_center == nullptr) {
if (last_ballistic_point != nullptr) {
delete last_ballistic_point;
last_ballistic_point = nullptr;
}
return nullptr;
}
@@ -36,9 +45,6 @@ cv::Point2f* BallisticPredictor::predict_ballistic_point(const cv::Point2f* pred
cv::Point2f ballistic_point(ballistic_x, ballistic_y);
// Update last ballistic point
if (last_ballistic_point != nullptr) {
delete last_ballistic_point;
}
last_ballistic_point = new cv::Point2f(ballistic_point);
return last_ballistic_point;

48
src/ImagePreprocessor.cpp
View File

@@ -1,4 +1,5 @@
#include "ImagePreprocessor.h"
#include <algorithm>
#include <cctype>
ImagePreprocessor::ImagePreprocessor() {
@@ -9,47 +10,16 @@ ImagePreprocessor::~ImagePreprocessor() {
// Destructor implementation
}
cv::Mat ImagePreprocessor::get_mask(const cv::Mat& frame, const std::string& target_color) {
cv::Mat hsv_frame;
cv::cvtColor(frame, hsv_frame, cv::COLOR_BGR2HSV);
cv::Mat mask;
if (target_color == "red") {
// Red color range in HSV
cv::Mat mask1, mask2;
cv::inRange(hsv_frame, cv::Scalar(0, 43, 49), cv::Scalar(25, 255, 255), mask1);
cv::inRange(hsv_frame, cv::Scalar(156, 46, 49), cv::Scalar(180, 255, 255), mask2);
mask = mask1 | mask2;
} else if (target_color == "blue") {
// Blue color range in HSV
cv::inRange(hsv_frame, cv::Scalar(83, 200, 44), cv::Scalar(130, 255, 255), mask);
} else {
// Default to empty mask if color not recognized
mask = cv::Mat::zeros(frame.size(), CV_8UC1);
}
void ImagePreprocessor::apply_morphology(const cv::Mat& input_mask, cv::Mat& output_mask) {
// Apply morphological operations to reduce noise
cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
cv::morphologyEx(mask, mask, cv::MORPH_OPEN, kernel);
cv::morphologyEx(mask, mask, cv::MORPH_CLOSE, kernel);
return mask;
cv::morphologyEx(input_mask, output_mask, cv::MORPH_OPEN, kernel);
cv::morphologyEx(output_mask, output_mask, cv::MORPH_CLOSE, kernel);
}
cv::Mat ImagePreprocessor::get_color_only_frame(const cv::Mat& frame, const std::string& target_color) {
cv::Mat mask = get_mask(frame, target_color);
// 不再反转掩膜，直接使用原始掩膜
// 目标颜色区域为白色(255)，非目标颜色区域为黑色(0)
cv::Mat color_only_frame;
frame.copyTo(color_only_frame, mask); // 使用掩膜复制原始帧，只保留目标颜色部分
return color_only_frame;
}
void ImagePreprocessor::process_frame(const cv::Mat& frame, const std::string& target_color,
cv::Mat& mask, cv::Mat& color_only_frame) {
mask = get_mask(frame, target_color);
color_only_frame = get_color_only_frame(frame, target_color);
void ImagePreprocessor::apply_gaussian_blur(const cv::Mat& input, cv::Mat& output, int kernel_size) {
// Apply Gaussian blur to reduce noise
// Ensure kernel size is odd for Gaussian blur
int adjusted_kernel_size = kernel_size % 2 == 0 ? kernel_size + 1 : kernel_size;
cv::GaussianBlur(input, output, cv::Size(adjusted_kernel_size, adjusted_kernel_size), 0);
}

140
src/KalmanFilter.cpp
View File

@@ -1,38 +1,90 @@
#include "KalmanFilter.h"
#include "config.h"
#include <iostream>
#include <chrono>
KalmanFilter::KalmanFilter() {
// 4 state variables (x, y, dx, dy), 2 measurements (x, y)
kf.init(4, 2, 0);
// 6 state variables (x, y, vx, vy, ax, ay), 2 measurements (x, y)
kf.init(6, 2, 0);
init_params();
initialized = false;
has_prev_measurement = false;
has_prev_velocity = false;
time_elapsed = 1.0 / 100.0; // Assuming 100 FPS as default
last_measurement = cv::Point2f(0, 0);
last_prediction = cv::Point2f(0, 0);
prev_measurement = cv::Point2f(0, 0);
prev_velocity = cv::Point2f(0, 0);
}
void KalmanFilter::init_params() {
// Transition matrix: x(k+1) = x(k) + dx(k), y(k+1) = y(k) + dy(k), dx(k+1) = dx(k), dy(k+1) = dy(k)
kf.transitionMatrix = (cv::Mat_<float>(4, 4) <<
1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1
// Transition matrix for constant acceleration model
// x(k+1) = x(k) + vx(k)*dt + 0.5*ax(k)*dt^2
// y(k+1) = y(k) + vy(k)*dt + 0.5*ay(k)*dt^2
// vx(k+1) = vx(k) + ax(k)*dt
// vy(k+1) = vy(k) + ay(k)*dt
// ax(k+1) = ax(k)
// ay(k+1) = ay(k)
double dt = time_elapsed;
kf.transitionMatrix = (cv::Mat_<float>(6, 6) <<
1, 0, dt, 0, 0.5*dt*dt, 0,
0, 1, 0, dt, 0, 0.5*dt*dt,
0, 0, 1, 0, dt, 0,
0, 0, 0, 1, 0, dt,
0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 1
);
// Measurement matrix: measuring x and y
kf.measurementMatrix = (cv::Mat_<float>(2, 4) <<
1, 0, 0, 0,
0, 1, 0, 0
// Measurement matrix: measuring x and y only
kf.measurementMatrix = (cv::Mat_<float>(2, 6) <<
1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0
);
// Process noise covariance
kf.processNoiseCov = cv::Mat::eye(4, 4, CV_32F) * KF_PROCESS_NOISE;
// Process noise covariance - higher for acceleration components
kf.processNoiseCov = cv::Mat::zeros(6, 6, CV_32F);
kf.processNoiseCov.at<float>(0, 0) = 1.0f; // x position noise
kf.processNoiseCov.at<float>(1, 1) = 1.0f; // y position noise
kf.processNoiseCov.at<float>(2, 2) = 4.0f; // x velocity noise
kf.processNoiseCov.at<float>(3, 3) = 4.0f; // y velocity noise
kf.processNoiseCov.at<float>(4, 4) = 8.0f; // x acceleration noise
kf.processNoiseCov.at<float>(5, 5) = 8.0f; // y acceleration noise
// Measurement noise covariance
kf.measurementNoiseCov = cv::Mat::eye(2, 2, CV_32F) * KF_MEASUREMENT_NOISE;
kf.measurementNoiseCov = cv::Mat::eye(2, 2, CV_32F) * 0.1f; // Lower measurement noise
// Error covariance post
kf.errorCovPost = cv::Mat::eye(4, 4, CV_32F);
kf.errorCovPost = cv::Mat::eye(6, 6, CV_32F) * 0.1f;
}
cv::Point2f KalmanFilter::estimate_velocity(const cv::Point2f& current, const cv::Point2f& previous) {
// Calculate velocity based on position difference
cv::Point2f velocity = current - previous;
velocity.x /= (float)time_elapsed;
velocity.y /= (float)time_elapsed;
return velocity;
}
void KalmanFilter::updateProcessNoise(const cv::Point2f& current_measurement) {
if (has_prev_measurement && has_prev_velocity) {
// Estimate current velocity
cv::Point2f current_velocity = estimate_velocity(current_measurement, prev_measurement);
// Calculate acceleration based on change in velocity
cv::Point2f acceleration = current_velocity - prev_velocity;
acceleration.x /= (float)time_elapsed;
acceleration.y /= (float)time_elapsed;
// Adjust process noise based on estimated acceleration (higher acceleration = higher uncertainty)
float acc_magnitude = std::sqrt(acceleration.x * acceleration.x + acceleration.y * acceleration.y);
// Increase process noise for rapid maneuvers
float noise_factor = 1.0f + 0.5f * acc_magnitude;
kf.processNoiseCov.at<float>(4, 4) = 8.0f * noise_factor; // ax noise
kf.processNoiseCov.at<float>(5, 5) = 8.0f * noise_factor; // ay noise
}
}
void KalmanFilter::update(const cv::Point2f& measurement) {
@@ -40,19 +92,65 @@ void KalmanFilter::update(const cv::Point2f& measurement) {
if (!initialized) {
// Initialize state if not done yet
kf.statePost = (cv::Mat_<float>(4, 1) <<
measurement.x,
measurement.y,
0.0f,
0.0f);
kf.statePost = (cv::Mat_<float>(6, 1) <<
measurement.x, // x position
measurement.y, // y position
0.0f, // x velocity
0.0f, // y velocity
0.0f, // x acceleration
0.0f // y acceleration
);
initialized = true;
} else {
// Update process noise based on target movement characteristics
updateProcessNoise(measurement);
// Perform correction step
kf.correct(measurement_mat);
}
// Update previous state for next iteration
if (has_prev_measurement) {
// Calculate velocity based on position change
cv::Point2f current_velocity = estimate_velocity(measurement, prev_measurement);
if (has_prev_velocity) {
// Estimate acceleration based on velocity change
cv::Point2f acceleration = current_velocity - prev_velocity;
acceleration.x /= (float)time_elapsed;
acceleration.y /= (float)time_elapsed;
// Update state with estimated acceleration
kf.statePost.at<float>(4) = acceleration.x; // x acceleration
kf.statePost.at<float>(5) = acceleration.y; // y acceleration
}
// Update previous velocity
prev_velocity = current_velocity;
has_prev_velocity = true;
} else {
// Initialize previous velocity as zero
prev_velocity = cv::Point2f(0, 0);
has_prev_velocity = true;
}
// Update stored measurements
prev_measurement = last_measurement;
has_prev_measurement = true;
last_measurement = measurement;
}
cv::Point2f KalmanFilter::get_current_velocity() const {
if (!initialized) {
return cv::Point2f(0.0f, 0.0f);
}
// Return the velocity components from the state vector
float vx = kf.statePost.at<float>(2);
float vy = kf.statePost.at<float>(3);
return cv::Point2f(vx, vy);
}
cv::Point2f KalmanFilter::predict() {
if (!initialized) {
return cv::Point2f(0, 0);

184
src/MindVisionCamera.cpp
View File

@@ -1,20 +1,27 @@
#include "MindVisionCamera.h"
#include <iostream>
#include <memory>
MindVisionCamera::MindVisionCamera(int cam_id, const std::string& target_color)
: camera_handle(-1), is_opened(false), width(640), height(480), fps(100), g_pRgbBuffer(nullptr) {
MindVisionCamera::MindVisionCamera(int cam_id, const std::string& target_color)
: camera_handle(-1), is_opened(false), width(640), height(480), fps(30) {
if (!initialize_camera(cam_id)) {
// 即使初始化失败，也要清理可能已分配的资源
release();
throw std::runtime_error("Cannot initialize MindVision camera!");
}
is_opened = true;
this->target_color = target_color;
if (this->target_color != "red" && this->target_color != "blue") {
release(); // 清理已初始化的资源
throw std::invalid_argument("Only 'red' or 'blue' colors are supported");
}
set_cam_params();
if (!set_cam_params()) { // 修改：set_cam_params现在返回bool值
release(); // 清理已初始化的资源
throw std::runtime_error("Failed to set camera parameters!");
}
}
bool MindVisionCamera::initialize_camera(int cam_id) {
@@ -59,16 +66,12 @@ bool MindVisionCamera::initialize_camera(int cam_id) {
return false;
}
// 设置输出格式
if (capability.sIspCapacity.bMonoSensor) {
CameraSetIspOutFormat(camera_handle, CAMERA_MEDIA_TYPE_MONO8);
} else {
CameraSetIspOutFormat(camera_handle, CAMERA_MEDIA_TYPE_BGR8);
}
// 让SDK进入工作模式
// 让SDK进入工作模式 - 根据原始OpenCv项目，应该在设置格式前调用
CameraPlay(camera_handle);
// 设置输出格式 - 保持与原始工作项目一致，直接设置为BGR8格式
CameraSetIspOutFormat(camera_handle, CAMERA_MEDIA_TYPE_BGR8);
// 获取并设置分辨率为 640x480
tSdkImageResolution image_resolution;
int status = CameraGetImageResolution(camera_handle, &image_resolution);
@@ -95,59 +98,62 @@ bool MindVisionCamera::initialize_camera(int cam_id) {
width = image_resolution.iWidth;
height = image_resolution.iHeight;
// 开启自动曝光
CameraSetAeState(camera_handle, true);
// 分配处理后的数据缓存区 - 使用最大分辨率，与原始项目一致
g_pRgbBuffer = (unsigned char *)malloc(capability.sResolutionRange.iHeightMax *
capability.sResolutionRange.iWidthMax * 3);
// 设置帧率
CameraSetFrameSpeed(camera_handle, 10); // 设置为中等帧率
// 设置帧率（虽然稍后 set_cam_params 会再次设置，但这里先设定）
CameraSetFrameSpeed(camera_handle, 2); // 设置为较高帧率
std::cout << "Initialized camera with resolution: " << width << "x" << height << std::endl;
if (!g_pRgbBuffer) {
std::cerr << "Failed to allocate global RGB buffer" << std::endl;
return false;
}
return true;
}
MindVisionCamera::~MindVisionCamera() {
release();
try {
if (g_pRgbBuffer) {
free(g_pRgbBuffer);
g_pRgbBuffer = nullptr;
}
release();
} catch (...) {
// 析构函数中捕获异常但不重新抛出
// 确保即使在异常情况下也能正确清理资源
}
}
void MindVisionCamera::set_camera_parameters() {
// 先设置为手动曝光模式
CameraSetAeState(camera_handle, false);
// 根据颜色设置曝光参数
// 根据颜色设置曝光参数，以优化装甲板检测效果
if (target_color == "red") {
CameraSetExposureTime(camera_handle, 10000); // 5ms曝光时间
CameraSetExposureTime(camera_handle, 5000); // 红色目标，曝光时间5ms (5000微秒)
} else if (target_color == "blue") {
CameraSetExposureTime(camera_handle, 4000); // 8ms曝光时间
CameraSetExposureTime(camera_handle, 8000); // 蓝色目标，曝光时间8ms (8000微秒)
}
// 设置白平衡
CameraSetWbMode(camera_handle, false); // 关闭自动白平衡
CameraSetOnceWB(camera_handle); // 一次性白平衡
// 设置其他参数
CameraSetTriggerMode(camera_handle, 0); // 连续采集模式
// 设置帧率为100FPS
CameraSetFrameSpeed(camera_handle, 2); // 2代表较高帧率，具体值可能需要调整
}
void MindVisionCamera::set_cam_params() {
bool MindVisionCamera::set_cam_params() {
// 只设置相机参数（如曝光、白平衡等），不重新设置分辨率
set_camera_parameters();
// Ensure resolution remains 640x480 after setting other parameters
tSdkImageResolution image_resolution;
image_resolution.iIndex = 0xFF; // Use custom resolution (0xFF typically indicates custom resolution)
image_resolution.iWidth = 640;
image_resolution.iHeight = 480;
image_resolution.iWidthFOV = image_resolution.iWidth;
image_resolution.iHeightFOV = image_resolution.iHeight;
image_resolution.iHOffsetFOV = 0;
image_resolution.iVOffsetFOV = 0;
int status = CameraSetImageResolution(camera_handle, &image_resolution);
if (status != CAMERA_STATUS_SUCCESS) {
std::cout << "Failed to maintain resolution at 640x480 in set_cam_params" << std::endl;
} else {
std::cout << "Confirmed camera resolution: " << image_resolution.iWidth << "x" << image_resolution.iHeight << std::endl;
}
return true;
}
bool MindVisionCamera::read_frame(cv::Mat& frame) {
@@ -157,39 +163,88 @@ bool MindVisionCamera::read_frame(cv::Mat& frame) {
tSdkFrameHead sFrameInfo;
BYTE* pbyBuffer;
// 获取一帧数据
if (CameraGetImageBuffer(camera_handle, &sFrameInfo, &pbyBuffer, 1000) == CAMERA_STATUS_SUCCESS) {
// 处理图像数据
unsigned char* g_pRgbBuffer = new unsigned char[sFrameInfo.iWidth * sFrameInfo.iHeight * 3];
CameraImageProcess(camera_handle, pbyBuffer, g_pRgbBuffer, &sFrameInfo);
// 创建OpenCV Mat对象
frame = cv::Mat(
cv::Size(sFrameInfo.iWidth, sFrameInfo.iHeight),
sFrameInfo.uiMediaType == CAMERA_MEDIA_TYPE_MONO8 ? CV_8UC1 : CV_8UC3,
g_pRgbBuffer
).clone(); // clone()确保数据被复制，而不是共享指针
// 使用全局缓冲区处理图像数据 - 与原始项目一致
int status = CameraImageProcess(camera_handle, pbyBuffer, g_pRgbBuffer, &sFrameInfo);
if (status != CAMERA_STATUS_SUCCESS) {
std::cerr << "Failed to process image: error code " << status << std::endl;
CameraReleaseImageBuffer(camera_handle, pbyBuffer);
return false;
}
// 创建OpenCV Mat对象 - 使用全局缓冲区，固定为CV_8UC3格式
cv::Mat temp_mat(sFrameInfo.iHeight, sFrameInfo.iWidth, CV_8UC3, g_pRgbBuffer);
frame = temp_mat.clone(); // clone()确保数据被复制，而不是共享指针
// 释放原始缓冲区
CameraReleaseImageBuffer(camera_handle, pbyBuffer);
// 释放临时缓冲区
delete[] g_pRgbBuffer;
return true;
}
return false;
}
bool MindVisionCamera::read_frame_with_color_filter(cv::Mat& frame, cv::Mat& raw_mask, const std::string& target_color) {
if (!is_opened) {
return false;
}
tSdkFrameHead sFrameInfo;
BYTE* pbyBuffer;
// 获取一帧数据
if (CameraGetImageBuffer(camera_handle, &sFrameInfo, &pbyBuffer, 1000) == CAMERA_STATUS_SUCCESS) {
// 使用全局缓冲区处理图像数据 - 与原始项目一致
int status = CameraImageProcess(camera_handle, pbyBuffer, g_pRgbBuffer, &sFrameInfo);
if (status != CAMERA_STATUS_SUCCESS) {
std::cerr << "Failed to process image: error code " << status << std::endl;
CameraReleaseImageBuffer(camera_handle, pbyBuffer);
return false;
}
// 创建原始RGB图像 - 使用全局缓冲区，固定为CV_8UC3格式
cv::Mat original_img(sFrameInfo.iHeight, sFrameInfo.iWidth, CV_8UC3, g_pRgbBuffer);
// 复制图像数据，确保原始数据不会被后续操作影响
frame = original_img.clone();
// 创建HSV图像用于颜色过滤
cv::Mat hsv_img;
cv::cvtColor(frame, hsv_img, cv::COLOR_BGR2HSV);
// 根据颜色创建原始掩码
if (target_color == "red") {
// Red color range in HSV
cv::Mat mask1, mask2;
cv::inRange(hsv_img, cv::Scalar(0, 43, 49), cv::Scalar(25, 255, 255), mask1);
cv::inRange(hsv_img, cv::Scalar(156, 46, 49), cv::Scalar(180, 255, 255), mask2);
raw_mask = mask1 | mask2;
} else if (target_color == "blue") {
// Blue color range in HSV
cv::inRange(hsv_img, cv::Scalar(83, 200, 44), cv::Scalar(130, 255, 255), raw_mask);
} else {
raw_mask = cv::Mat::zeros(hsv_img.size(), CV_8UC1);
}
// 释放相机缓冲区
CameraReleaseImageBuffer(camera_handle, pbyBuffer);
return true;
}
return false;
}
void MindVisionCamera::release() {
if (is_opened && camera_handle >= 0) {
// 停止采集
if (camera_handle >= 0) {
// 停止采集，不管is_opened状态如何都尝试释放
CameraUnInit(camera_handle);
camera_handle = -1;
is_opened = false;
}
is_opened = false;
}
bool MindVisionCamera::switch_color(const std::string& new_color) {
@@ -201,8 +256,7 @@ bool MindVisionCamera::switch_color(const std::string& new_color) {
if ((lower_color == "red" || lower_color == "blue") && lower_color != target_color) {
target_color = lower_color;
set_cam_params();
return true;
return set_cam_params(); // 返回set_cam_params的结果
}
return false;
}

100
src/MindVisionMain.cpp
View File

@@ -3,6 +3,7 @@
#include <vector>
#include <chrono>
#include <thread>
#include <memory>
#include <opencv2/opencv.hpp>
@@ -22,26 +23,21 @@ void output_control_data(const cv::Point2f* ballistic_point,
TTLCommunicator* ttl_communicator,
const cv::Point2f& img_center,
bool use_ttl) {
// Only send data when TTL is enabled and meets frame interval
if (use_ttl && frame_counter % 5 == 0) {
// Only send data when TTL is enabled, meets frame interval, and has valid target
if (use_ttl && frame_counter % 5 == 0 && ballistic_point != nullptr) {
std::string send_str;
if (ballistic_point != nullptr) {
// Calculate offset (based on actual image center)
int ballistic_offset_x = static_cast<int>(ballistic_point->x - img_center.x);
int ballistic_offset_y = static_cast<int>(ballistic_point->y - img_center.y);
// Calculate offset (based on actual image center)
int ballistic_offset_x = static_cast<int>(ballistic_point->x - img_center.x);
int ballistic_offset_y = static_cast<int>(img_center.y - ballistic_point->y);
// Color simplification mapping
std::string simplified_color = target_color;
if (target_color == "red") simplified_color = "r";
else if (target_color == "blue") simplified_color = "b";
// Color simplification mapping
std::string simplified_color = target_color;
if (target_color == "red") simplified_color = "r";
else if (target_color == "blue") simplified_color = "b";
// Construct send string
send_str = "s " + simplified_color + " " + std::to_string(ballistic_offset_x) + " " + std::to_string(ballistic_offset_y);
} else {
// Format when no detection result
send_str = "s u 0 0";
}
// Construct send string
send_str = "s " + simplified_color + " " + std::to_string(ballistic_offset_x) + " " + std::to_string(ballistic_offset_y);
// Send data
if (ttl_communicator != nullptr) {
@@ -58,10 +54,10 @@ void set_camera_resolution(MindVisionCamera& , int width, int height) {
}
int main(int /*argc*/, char* /*argv*/[]) {
std::string target_color = "blue";
std::string target_color = "red";
int cam_id = 0;
cv::Size default_resolution(640, 480);
bool use_ttl = false; // Set to false to disable TTL communication
bool use_ttl = true; // Set to false to disable TTL communication
// Define optional resolution list (adjust based on camera support)
std::vector<cv::Size> resolutions = {
@@ -83,9 +79,14 @@ int main(int /*argc*/, char* /*argv*/[]) {
// Initialize TTL communication (only when enabled)
TTLCommunicator* ttl = nullptr;
if (use_ttl) {
ttl = new TTLCommunicator(TTL_BAUDRATE);
if (!ttl->connect()) {
std::cout << "Warning: Cannot establish TTL connection, will continue running but not send data" << std::endl;
// On Linux, the port would typically be /dev/ttyUSB0, /dev/ttyACM0, etc.
ttl = new TTLCommunicator("/dev/ttyUSB0", TTL_BAUDRATE);
if (ttl != nullptr) {
if (!ttl->connect()) {
std::cout << "Warning: Cannot establish TTL connection, will continue running but not send data" << std::endl;
}
} else {
std::cout << "Error: Failed to create TTL communicator instance" << std::endl;
}
} else {
std::cout << "TTL communication disabled" << std::endl;
@@ -119,17 +120,23 @@ int main(int /*argc*/, char* /*argv*/[]) {
cv::Mat frame;
try {
while (true) {
if (!camera.read_frame(frame)) {
std::cout << "Cannot read from MindVision camera, exiting!" << std::endl;
// 使用新的颜色过滤方法同时获取图像和原始掩码
cv::Mat raw_mask;
if (!camera.read_frame_with_color_filter(frame, raw_mask, target_color)) {
std::cout << "Cannot read from MindVision camera, exiting!，HERERER" << std::endl;
break;
}
// Get actual image size and calculate center
cv::Point2f img_center(frame.cols / 2.0f, frame.rows / 2.0f);
// Preprocessing
cv::Mat mask, color_only_frame;
preprocessor.process_frame(frame, target_color, mask, color_only_frame);
// 使用OpenCV进行形态学处理
cv::Mat mask;
preprocessor.apply_morphology(raw_mask, mask);
// 生成彩色图像（仅显示目标颜色）
cv::Mat color_only_frame;
frame.copyTo(color_only_frame, raw_mask);
// Armor plate detection
std::vector<LightBar> valid_light_bars;
@@ -142,12 +149,13 @@ int main(int /*argc*/, char* /*argv*/[]) {
current_center = &(armor_plates[0].center);
}
cv::Point2f* predicted_center = nullptr;
// Use smart pointer for safer memory management
std::unique_ptr<cv::Point2f> predicted_center = nullptr;
if (current_center != nullptr) {
// Has detection result: update Kalman filter, reset consecutive prediction count
kalman_tracker.update(*current_center);
predicted_center = new cv::Point2f(kalman_tracker.predict());
predicted_center = std::make_unique<cv::Point2f>(kalman_tracker.predict());
// Get velocity from the Kalman filter
target_speed = cv::Point2f(0.0f, 0.0f); // Kalman filter in OpenCV doesn't directly expose velocity
@@ -158,7 +166,7 @@ int main(int /*argc*/, char* /*argv*/[]) {
if (consecutive_predicts < max_consecutive_predicts) {
cv::Point2f temp_pred = kalman_tracker.predict();
if (temp_pred.x != 0 || temp_pred.y != 0) { // Check if prediction is valid
predicted_center = new cv::Point2f(temp_pred);
predicted_center = std::make_unique<cv::Point2f>(temp_pred);
}
} else {
predicted_center = nullptr;
@@ -169,13 +177,13 @@ int main(int /*argc*/, char* /*argv*/[]) {
// Determine display center
cv::Point2f* display_center = current_center;
if (display_center == nullptr && predicted_center != nullptr) {
display_center = predicted_center;
display_center = predicted_center.get();
}
bool is_predicted = (display_center != nullptr) && (current_center == nullptr);
// Calculate ballistic prediction point
cv::Point2f* ballistic_point = ballistic_predictor.predict_ballistic_point(
predicted_center, img_center, target_speed
predicted_center.get(), img_center, target_speed
);
auto current_time = std::chrono::high_resolution_clock::now();
@@ -210,24 +218,36 @@ int main(int /*argc*/, char* /*argv*/[]) {
std::this_thread::sleep_for(std::chrono::milliseconds(10 - elapsed));
}
// Clean up dynamically allocated memory
if (predicted_center != nullptr) {
delete predicted_center;
predicted_center = nullptr;
}
// Smart pointers automatically handle memory cleanup
}
}
catch (const std::exception& e) {
std::cerr << "Error: " << e.what() << std::endl;
std::cerr << "Error in main loop: " << e.what() << std::endl;
}
catch (...) {
std::cerr << "Unknown error occurred in main loop" << std::endl;
}
// Cleanup
camera.release();
cv::destroyAllWindows();
try {
camera.release();
} catch (...) {
std::cerr << "Error during camera release" << std::endl;
}
try {
cv::destroyAllWindows();
} catch (...) {
std::cerr << "Error during window destruction" << std::endl;
}
// Only close TTL connection when enabled and initialized
if (use_ttl && ttl != nullptr) {
ttl->close();
try {
ttl->close();
} catch (...) {
std::cerr << "Error during TTL close" << std::endl;
}
delete ttl;
ttl = nullptr;
}

123
src/TTLCommunicator.cpp
View File

@@ -1,9 +1,17 @@
#include "TTLCommunicator.h"
#include <iostream>
TTLCommunicator::TTLCommunicator(int baudrate) : baudrate(baudrate), connected(false) {
// Default to a common serial port name, though this will need to be configured for the specific system
port_name = "/dev/ttyUSB0"; // Common for Linux/Ubuntu
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
TTLCommunicator::TTLCommunicator(const std::string& port, int baudrate)
: port_name(port), baudrate(baudrate), connected(false) {
serial_fd = -1;
}
TTLCommunicator::~TTLCommunicator() {
@@ -11,19 +19,16 @@ TTLCommunicator::~TTLCommunicator() {
}
bool TTLCommunicator::connect() {
// Placeholder implementation - in a real implementation, you would open the serial port
std::cout << "Attempting to connect to TTL device on port: " << port_name << " at baud rate: " << baudrate << std::endl;
// For now, simulate a connection attempt
// In real implementation, use system calls to open the serial port
connected = open_serial_port();
if (connected) {
std::cout << "TTL connection established." << std::endl;
} else {
std::cout << "Warning: Failed to establish TTL connection." << std::endl;
}
return connected;
}
@@ -37,23 +42,103 @@ void TTLCommunicator::close() {
bool TTLCommunicator::send_data(const std::string& data) {
if (!connected) {
std::cerr << "Error: Cannot send data, TTL not connected." << std::endl;
return false;
}
// Placeholder for sending data over serial
// In real implementation, write to the serial port
std::cout << "Sending TTL data: " << data << std::endl;
// Simulate successful transmission
return true;
if (serial_fd != -1) {
ssize_t bytes_written = write(serial_fd, data.c_str(), data.length());
if (bytes_written == -1) {
std::cerr << "Error writing to serial port: " << strerror(errno) << std::endl;
return false;
} else {
std::cout << "Sent " << bytes_written << " bytes: " << data << std::endl;
fsync(serial_fd); // Ensure data is sent
return true;
}
}
return false;
}
bool TTLCommunicator::open_serial_port() {
// In a real implementation, this would use system calls like open(), tcsetattr(), etc.
// For now, return true to simulate a successful connection
// Open the serial port
serial_fd = open(port_name.c_str(), O_RDWR | O_NOCTTY);
if (serial_fd == -1) {
std::cerr << "Error opening serial port " << port_name << std::endl;
return false;
}
struct termios tty;
// Get current attributes
if (tcgetattr(serial_fd, &tty) != 0) {
std::cerr << "Error getting serial attributes" << std::endl;
::close(serial_fd);
return false;
}
// Set baud rate
speed_t baud_rate;
switch(baudrate) {
case 9600: baud_rate = B9600; break;
case 19200: baud_rate = B19200; break;
case 38400: baud_rate = B38400; break;
case 57600: baud_rate = B57600; break;
case 115200: baud_rate = B115200; break;
case 230400: baud_rate = B230400; break;
case 460800: baud_rate = B460800; break;
case 921600: baud_rate = B921600; break;
default:
std::cerr << "Unsupported baud rate: " << baudrate << std::endl;
::close(serial_fd);
return false;
}
cfsetispeed(&tty, baud_rate);
cfsetospeed(&tty, baud_rate);
// 8N1: 8 data bits, no parity, 1 stop bit
tty.c_cflag &= ~PARENB; // No parity
tty.c_cflag &= ~CSTOPB; // 1 stop bit
tty.c_cflag &= ~CSIZE; // Clear data bits
tty.c_cflag |= CS8; // 8 data bits
tty.c_cflag &= ~CRTSCTS; // No flow control
tty.c_cflag |= CREAD | CLOCAL; // Enable reading, ignore control lines
// Non-canonical mode
tty.c_lflag &= ~ICANON; // Non-canonical mode
tty.c_lflag &= ~ECHO; // No echo
tty.c_lflag &= ~ECHOE; // No erasure
tty.c_lflag &= ~ISIG; // No signal handling
// No software flow control
tty.c_iflag &= ~(IXON | IXOFF | IXANY);
// No output processing
tty.c_oflag &= ~OPOST;
// Timeouts
tty.c_cc[VMIN] = 0; // Non-blocking read
tty.c_cc[VTIME] = 5; // 0.5 second timeout
// Apply settings
if (tcsetattr(serial_fd, TCSANOW, &tty) != 0) {
std::cerr << "Error setting serial attributes" << std::endl;
::close(serial_fd);
return false;
}
// Flush port
tcflush(serial_fd, TCIOFLUSH);
return true;
}
void TTLCommunicator::close_serial_port() {
// In a real implementation, this would close the file descriptor
if (serial_fd != -1) {
::close(serial_fd);
serial_fd = -1;
}
connected = false;
}

2
src/Visualizer.cpp
View File

@@ -43,7 +43,7 @@ cv::Mat& Visualizer::draw_offset_text(cv::Mat& frame, const cv::Point2f* display
cv::Point2f img_center(frame.cols / 2.0f, frame.rows / 2.0f);
int offset_x = static_cast<int>(display_center->x - img_center.x);
int offset_y = static_cast<int>(display_center->y - img_center.y);
int offset_y = static_cast<int>(img_center.y - display_center->y);
std::stringstream ss;
ss << "Offset: (" << offset_x << ", " << offset_y << ")";

127
src/main.cpp
View File

@@ -3,6 +3,7 @@
#include <vector>
#include <chrono>
#include <thread>
#include <memory>
#include <opencv2/opencv.hpp>
@@ -21,43 +22,41 @@ void output_control_data(const cv::Point2f* ballistic_point,
int frame_counter,
TTLCommunicator* ttl_communicator,
const cv::Point2f& img_center,
bool use_ttl,
bool armor_detected) {
// Only send data when TTL is enabled, meets frame interval, AND armor is detected
if (use_ttl && frame_counter % 5 == 0 && armor_detected) {
// Only send if we have a valid ballistic point to send
if (ballistic_point != nullptr) {
// Calculate offset (based on actual image center)
int ballistic_offset_x = static_cast<int>(ballistic_point->x - img_center.x);
int ballistic_offset_y = static_cast<int>(ballistic_point->y - img_center.y);
bool use_ttl) {
// Only send data when TTL is enabled, meets frame interval, and has valid target
if (use_ttl && frame_counter % 5 == 0 && ballistic_point != nullptr) {
std::string send_str;
// Color simplification mapping
std::string simplified_color = target_color;
if (target_color == "red") simplified_color = "r";
else if (target_color == "blue") simplified_color = "b";
// Calculate offset (based on actual image center)
int ballistic_offset_x = static_cast<int>(ballistic_point->x - img_center.x);
int ballistic_offset_y = static_cast<int>(img_center.y - ballistic_point->y);
// Construct send string
std::string send_str = "s " + simplified_color + " " + std::to_string(ballistic_offset_x) + " " + std::to_string(ballistic_offset_y);
// Color simplification mapping
std::string simplified_color = target_color;
if (target_color == "red") simplified_color = "r";
else if (target_color == "blue") simplified_color = "b";
// Send data
if (ttl_communicator != nullptr) {
ttl_communicator->send_data(send_str);
}
// Construct send string
send_str = "s " + simplified_color + " " + std::to_string(ballistic_offset_x) + " " + std::to_string(ballistic_offset_y);
// Send data
if (ttl_communicator != nullptr) {
ttl_communicator->send_data(send_str);
}
// Note: When ballistic_point is nullptr, no TTL data is sent (fulfilling the requirement)
}
}
void set_camera_resolution(MindVisionCamera, int width, int height) {
void set_camera_resolution(MindVisionCamera& , int width, int height) {
// The resolution is set during camera initialization in MindVision
// We already set it in MindVisionCamera constructor, so this is for logging only
std::cout << "Camera resolution already set to: " << width << "x" << height << " during initialization" << std::endl;
// We need to implement a method in MindVisionCamera to change resolution
// For now, we'll just log the intended change
std::cout << "Setting camera resolution to: " << width << "x" << height << std::endl;
}
int main(int /*argc*/, char* /*argv*/[]) {
std::string target_color = "red";
int cam_id = 0;
cv::Size default_resolution(640, 480); // This will be set by MindVisionCamera constructor
cv::Size default_resolution(640, 480);
bool use_ttl = true; // Set to false to disable TTL communication
// Define optional resolution list (adjust based on camera support)
@@ -80,9 +79,14 @@ int main(int /*argc*/, char* /*argv*/[]) {
// Initialize TTL communication (only when enabled)
TTLCommunicator* ttl = nullptr;
if (use_ttl) {
ttl = new TTLCommunicator(TTL_BAUDRATE);
if (!ttl->connect()) {
std::cout << "Warning: Cannot establish TTL connection, will continue running but not send data" << std::endl;
// On Linux, the port would typically be /dev/ttyUSB0, /dev/ttyACM0, etc.
ttl = new TTLCommunicator("/dev/ttyUSB0", TTL_BAUDRATE);
if (ttl != nullptr) {
if (!ttl->connect()) {
std::cout << "Warning: Cannot establish TTL connection, will continue running but not send data" << std::endl;
}
} else {
std::cout << "Error: Failed to create TTL communicator instance" << std::endl;
}
} else {
std::cout << "TTL communication disabled" << std::endl;
@@ -116,17 +120,23 @@ int main(int /*argc*/, char* /*argv*/[]) {
cv::Mat frame;
try {
while (true) {
//if (!camera.read_frame(frame)) {
// std::cout << "Cannot read from MindVision camera, exiting!" << std::endl;
// break;
//}
// 使用新的颜色过滤方法同时获取图像和原始掩码
cv::Mat raw_mask;
if (!camera.read_frame_with_color_filter(frame, raw_mask, target_color)) {
std::cout << "Cannot read from MindVision camera, exiting!，HERERER" << std::endl;
break;
}
// Get actual image size and calculate center
cv::Point2f img_center(frame.cols / 2.0f, frame.rows / 2.0f);
// Preprocessing
cv::Mat mask, color_only_frame;
preprocessor.process_frame(frame, target_color, mask, color_only_frame);
// 使用OpenCV进行形态学处理
cv::Mat mask;
preprocessor.apply_morphology(raw_mask, mask);
// 生成彩色图像（仅显示目标颜色）
cv::Mat color_only_frame;
frame.copyTo(color_only_frame, raw_mask);
// Armor plate detection
std::vector<LightBar> valid_light_bars;
@@ -139,12 +149,13 @@ int main(int /*argc*/, char* /*argv*/[]) {
current_center = &(armor_plates[0].center);
}
cv::Point2f* predicted_center = nullptr;
// Use smart pointer for safer memory management
std::unique_ptr<cv::Point2f> predicted_center = nullptr;
if (current_center != nullptr) {
// Has detection result: update Kalman filter, reset consecutive prediction count
kalman_tracker.update(*current_center);
predicted_center = new cv::Point2f(kalman_tracker.predict());
predicted_center = std::make_unique<cv::Point2f>(kalman_tracker.predict());
// Get velocity from the Kalman filter
target_speed = cv::Point2f(0.0f, 0.0f); // Kalman filter in OpenCV doesn't directly expose velocity
@@ -155,7 +166,7 @@ int main(int /*argc*/, char* /*argv*/[]) {
if (consecutive_predicts < max_consecutive_predicts) {
cv::Point2f temp_pred = kalman_tracker.predict();
if (temp_pred.x != 0 || temp_pred.y != 0) { // Check if prediction is valid
predicted_center = new cv::Point2f(temp_pred);
predicted_center = std::make_unique<cv::Point2f>(temp_pred);
}
} else {
predicted_center = nullptr;
@@ -166,13 +177,13 @@ int main(int /*argc*/, char* /*argv*/[]) {
// Determine display center
cv::Point2f* display_center = current_center;
if (display_center == nullptr && predicted_center != nullptr) {
display_center = predicted_center;
display_center = predicted_center.get();
}
bool is_predicted = (display_center != nullptr) && (current_center == nullptr);
// Calculate ballistic prediction point
cv::Point2f* ballistic_point = ballistic_predictor.predict_ballistic_point(
predicted_center, img_center, target_speed
predicted_center.get(), img_center, target_speed
);
auto current_time = std::chrono::high_resolution_clock::now();
@@ -186,11 +197,7 @@ int main(int /*argc*/, char* /*argv*/[]) {
visualizer.draw_ballistic_point(frame, ballistic_point);
// Output control data to TTL (passing use_ttl to control whether to send)
// Only send TTL data if actual armor plates were detected (not just predicted)
bool armor_detected = !armor_plates.empty();
// Only send TTL data when we have an actual detection, not just prediction
bool should_send_ttl = armor_detected && (display_center != nullptr);
output_control_data(display_center, target_color, frame_counter, ttl, img_center, use_ttl, should_send_ttl);
output_control_data(display_center, target_color, frame_counter, ttl, img_center, use_ttl);
// Display windows
cv::imshow("Armor Detection", frame);
@@ -208,27 +215,39 @@ int main(int /*argc*/, char* /*argv*/[]) {
auto end_time = std::chrono::high_resolution_clock::now();
auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - current_time).count();
if (elapsed < 10) { // 100 FPS = 10ms per frame
std::this_thread::sleep_for(std::chrono::milliseconds(30 - elapsed));
std::this_thread::sleep_for(std::chrono::milliseconds(10 - elapsed));
}
// Clean up dynamically allocated memory
if (predicted_center != nullptr) {
delete predicted_center;
predicted_center = nullptr;
}
// Smart pointers automatically handle memory cleanup
}
}
catch (const std::exception& e) {
std::cerr << "Error: " << e.what() << std::endl;
std::cerr << "Error in main loop: " << e.what() << std::endl;
}
catch (...) {
std::cerr << "Unknown error occurred in main loop" << std::endl;
}
// Cleanup
camera.release();
cv::destroyAllWindows();
try {
camera.release();
} catch (...) {
std::cerr << "Error during camera release" << std::endl;
}
try {
cv::destroyAllWindows();
} catch (...) {
std::cerr << "Error during window destruction" << std::endl;
}
// Only close TTL connection when enabled and initialized
if (use_ttl && ttl != nullptr) {
ttl->close();
try {
ttl->close();
} catch (...) {
std::cerr << "Error during TTL close" << std::endl;
}
delete ttl;
ttl = nullptr;
}