参考上海交通大学代码进行修改

Catalyst-MDVS2/inc/ArmorDetector.h

@@ -0,0 +1,65 @@
+#ifndef ARMORDETECTOR_H
+#define ARMORDETECTOR_H
+
+#include <opencv2/opencv.hpp>
+#include <vector>
+#include <string>
+#include "config.h"
+
+struct LightBar {
+    cv::Point2f center;
+    cv::Size2f size;
+    float angle;          // in degrees
+    float angle_rad;      // in radians
+    float area;
+    std::vector<cv::Point2f> center_line;  // [end1, end2]
+    float center_line_length;
+    std::vector<cv::Point2f> box;
+};
+
+struct ArmorPlate {
+    std::string color;
+    cv::Point2f center;
+    double confidence;
+    std::pair<LightBar, LightBar> pair;
+    std::vector<cv::Point2f> corners_2d;  // Can be computed later for 3D pose
+
+    // Function to get bounding rectangle of the armor plate
+    cv::Rect2d getBoundingRect() const {
+        if (corners_2d.size() >= 4) {
+            cv::Point2f min_pt = corners_2d[0];
+            cv::Point2f max_pt = corners_2d[0];
+            for (const auto& pt : corners_2d) {
+                min_pt.x = std::min(min_pt.x, pt.x);
+                min_pt.y = std::min(min_pt.y, pt.y);
+                max_pt.x = std::max(max_pt.x, pt.x);
+                max_pt.y = std::max(max_pt.y, pt.y);
+            }
+            return cv::Rect2d(min_pt.x, min_pt.y, max_pt.x - min_pt.x, max_pt.y - min_pt.y);
+        }
+        // Fallback: use center and a fixed size
+        return cv::Rect2d(center.x - 30, center.y - 15, 60, 30);
+    }
+};
+
+class ArmorDetector {
+public:
+    ArmorDetector();
+    
+    // Main detection function
+    void detect(const cv::Mat& mask, const std::string& target_color, 
+                std::vector<LightBar>& valid_light_bars, 
+                std::vector<ArmorPlate>& armor_plates);
+
+private:
+    float angle_threshold_rad;
+    
+    // Helper methods
+    std::vector<LightBar> extract_initial_light_bars(const cv::Mat& mask);
+    std::vector<std::vector<cv::Point2f>> merge_nearby_light_bars(const std::vector<LightBar>& initial_light_bars);
+    std::vector<LightBar> filter_valid_light_bars(const std::vector<std::vector<cv::Point2f>>& processed_boxes);
+    std::vector<ArmorPlate> pair_light_bars_to_armor(const std::vector<LightBar>& light_bars, const std::string& target_color);
+    double calculate_iou(const cv::Rect& b1, const cv::Rect& b2);
+};
+
+#endif // ARMORDETECTOR_H

Catalyst-MDVS2/inc/BallisticPredictor.h

@@ -0,0 +1,29 @@
+#ifndef BALLISTICPREDICTOR_H
+#define BALLISTICPREDICTOR_H
+
+#include <opencv2/opencv.hpp>
+
+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,
+                                        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);
+};
+
+#endif // BALLISTICPREDICTOR_H

Catalyst-MDVS2/inc/BallisticPredictor.h:Zone.Identifier

@@ -0,0 +1,2 @@
+[ZoneTransfer]
+ZoneId=3

Catalyst-MDVS2/inc/Camera.h

@@ -0,0 +1,25 @@
+#ifndef CAMERA_H
+#define CAMERA_H
+
+#include <opencv2/opencv.hpp>
+#include <string>
+
+class Camera {
+public:
+    cv::VideoCapture cap;
+    bool is_opened;
+    std::string target_color;
+    
+    Camera(int cam_id = 0, const std::string& target_color = "red");
+    ~Camera();
+    
+    void set_cam_params();
+    bool read_frame(cv::Mat& frame);
+    void release();
+    bool switch_color(const std::string& target_color);
+
+private:
+    void set_camera_parameters();
+};
+
+#endif // CAMERA_H

Catalyst-MDVS2/inc/Camera.h:Zone.Identifier

@@ -0,0 +1,2 @@
+[ZoneTransfer]
+ZoneId=3

Catalyst-MDVS2/inc/ImagePreprocessor.h

@@ -0,0 +1,19 @@
+#ifndef IMAGEPREPROCESSOR_H
+#define IMAGEPREPROCESSOR_H
+
+#include <opencv2/opencv.hpp>
+#include <string>
+
+class ImagePreprocessor {
+public:
+    ImagePreprocessor();
+    ~ImagePreprocessor();
+
+    // 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

Catalyst-MDVS2/inc/ImagePreprocessor.h:Zone.Identifier

@@ -0,0 +1,2 @@
+[ZoneTransfer]
+ZoneId=3

Catalyst-MDVS2/inc/KalmanFilter.h

@@ -0,0 +1,38 @@
+#ifndef KALMANFILTER_H
+#define KALMANFILTER_H
+
+#include <opencv2/opencv.hpp>
+
+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:
+    cv::KalmanFilter kf;
+    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

Catalyst-MDVS2/inc/KalmanFilter.h:Zone.Identifier

@@ -0,0 +1,2 @@
+[ZoneTransfer]
+ZoneId=3

Catalyst-MDVS2/inc/MindVisionCamera.h

@@ -0,0 +1,43 @@
+#ifndef MINDVISION_CAMERA_H
+#define MINDVISION_CAMERA_H
+
+#include <opencv2/opencv.hpp>
+#include <string>
+#include <memory>
+
+// MindVision SDK 头文件 - 可能需要根据实际SDK文件调整
+extern "C" {
+    #include "CameraApi.h"  // 这是MindVision SDK的典型头文件
+}
+
+class MindVisionCamera {
+public:
+    CameraHandle camera_handle;     // MindVision SDk中的相机句柄
+    bool is_opened;
+    std::string target_color;
+    int width;
+    int height;
+    int fps;
+    unsigned char* g_pRgbBuffer; // 处理后数据缓存区
+    tSdkCameraCapbility capability;  // 相机能力信息
+    tSdkImageResolution image_resolution;  // 分辨率信息
+
+    MindVisionCamera(int cam_id = 0, const std::string& target_color = "red");
+    ~MindVisionCamera();
+
+    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);
+
+    int get_width() const { return width; }
+    int get_height() const { return height; }
+    bool set_resolution(int width, int height);
+
+private:
+    void set_camera_parameters();
+    bool initialize_camera(int cam_id);
+};
+
+#endif // MINDVISION_CAMERA_H

Catalyst-MDVS2/inc/MindVisionCamera.h:Zone.Identifier

@@ -0,0 +1,2 @@
+[ZoneTransfer]
+ZoneId=3

Catalyst-MDVS2/inc/TTLCommunicator.h

@@ -0,0 +1,27 @@
+#ifndef TTLCOMMUNICATOR_H
+#define TTLCOMMUNICATOR_H
+
+#include <string>
+
+class TTLCommunicator {
+public:
+    TTLCommunicator(const std::string& port_name = "/dev/ttyUSB0", int baudrate = 115200);
+    ~TTLCommunicator();
+
+    bool connect();
+    void close();
+    bool send_data(const std::string& data);
+    bool is_connected() const { return connected; }
+
+private:
+    std::string port_name;
+    int baudrate;
+    bool connected;
+
+    int serial_fd;
+
+    bool open_serial_port();
+    void close_serial_port();
+};
+
+#endif // TTLCOMMUNICATOR_H

Catalyst-MDVS2/inc/TTLCommunicator.h:Zone.Identifier

@@ -0,0 +1,2 @@
+[ZoneTransfer]
+ZoneId=3

Catalyst-MDVS2/inc/Visualizer.h

@@ -0,0 +1,30 @@
+#ifndef VISUALIZER_H
+#define VISUALIZER_H
+
+#include <opencv2/opencv.hpp>
+#include <vector>
+#include <string>
+#include "ArmorDetector.h"
+
+class Visualizer {
+public:
+    Visualizer();
+    ~Visualizer();
+    
+    // Draw light bars on frame
+    cv::Mat& draw_light_bars(cv::Mat& frame, const std::vector<LightBar>& light_bars, const std::string& target_color);
+    
+    // Draw armor plate on frame
+    cv::Mat& draw_armor_plate(cv::Mat& frame, const ArmorPlate& armor_plate);
+    
+    // Draw offset text
+    cv::Mat& draw_offset_text(cv::Mat& frame, const cv::Point2f* display_center, const std::string& target_color, bool is_predicted = false);
+    
+    // Draw crosshair at center
+    cv::Mat& draw_crosshair(cv::Mat& frame, const cv::Point2f& center, const cv::Scalar& color = cv::Scalar(0, 255, 0), int size = 10);
+    
+    // Draw ballistic point
+    cv::Mat& draw_ballistic_point(cv::Mat& frame, const cv::Point2f* ballistic_point);
+};
+
+#endif // VISUALIZER_H

Catalyst-MDVS2/inc/Visualizer.h:Zone.Identifier

@@ -0,0 +1,2 @@
+[ZoneTransfer]
+ZoneId=3

Catalyst-MDVS2/inc/config.h

@@ -0,0 +1,45 @@
+#ifndef CONFIG_H
+#define CONFIG_H
+
+#include <opencv2/opencv.hpp>
+
+// Camera intrinsic parameters (need actual calibration)
+const cv::Mat CAMERA_MATRIX = (cv::Mat_<float>(3, 3) << 
+    600, 0, 320,
+    0, 600, 240,
+    0, 0, 1);
+
+const cv::Mat DIST_COEFFS = cv::Mat::zeros(5, 1, CV_32F); // Distortion coefficients (set to 0 for no distortion)
+
+// Armor dimensions (in mm)
+const float ARMOR_WIDTH = 135.0f;
+const float ARMOR_HEIGHT = 125.0f;
+const float ARMOR_THICKNESS = 20.0f;
+
+const cv::Mat ARMOR_3D_POINTS = (cv::Mat_<float>(4, 3) <<
+    -ARMOR_WIDTH / 2, -ARMOR_HEIGHT / 2, 0,
+    ARMOR_WIDTH / 2, -ARMOR_HEIGHT / 2, 0,
+    ARMOR_WIDTH / 2, ARMOR_HEIGHT / 2, 0,
+    -ARMOR_WIDTH / 2, ARMOR_HEIGHT / 2, 0);
+
+// Detection thresholds
+const float HORIZONTAL_ANGLE_THRESHOLD = 25.0f; // Light bar horizontal angle threshold (degrees)
+const float HORIZONTAL_ANGLE_THRESHOLD_RAD = HORIZONTAL_ANGLE_THRESHOLD * CV_PI / 180.0f;
+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 = 4.0f;
+const float ARMOR_LENGTH_DIFF_RATIO = 0.5f;      // Armor light bar length difference ratio
+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
+const float KF_MEASUREMENT_NOISE = 0.5f;    // Measurement noise covariance
+
+// Focal length in pixels (from camera calibration)
+const double FOCAL_PIXAL = 600.0;  // This should match your actual camera calibration
+
+// TTL communication settings
+const int TTL_BAUDRATE = 115200;
+
+#endif // CONFIG_H