欢迎来到南叶の小窝

1
cv::Point2f p1(100, 200);
2
cv::Point2f p2(400, 500);
3

4
double distance = cv::norm(p1 - p2);

1
// 两个灯条的中心点
2
cv::Point2f center1 = bar1.rect.center;
3
cv::Point2f center2 = bar2.rect.center;
4

5
// 计算距离-
6
float distance = cv::norm(center1 - center2);
7

8
// 判断距离是否合理（参数随便给的）
9
float avgLength = (bar1.rect.size.height + bar2.rect.size.height) / 2;
10
if (distance < avgLength * 1.5 || distance > avgLength * 4.0) {
11
    // 距离不合理，不是装甲板
12
    return false;
13
}

1
float angle1 = bar1.rect.angle;
2
float angle2 = bar2.rect.angle;
3

4
// 计算角度差的绝对值
5
float angleDiff = std::abs(angle1 - angle2);
6

7
// 判断是否平行
8
if (angleDiff > 10.0) {
9
    // 角度差太大，不平行
10
    return false;
11
}

1
// 问题：OpenCV的angle范围是-90°到0°
2
// 如果一个灯条angle=-5°，另一个是-85°
3
// 直接相减：|-5 - (-85)| = 80°  实际上它们可能是平行的！
4

5
// 解决方案：归一化角度
6
float normalizeAngle(float angle) {
7
    // 将angle转换到0-90范围
8
    angle = std::abs(angle);
9
    if (angle > 90) angle = 180 - angle;
10
    return angle;
11
}
12

13
float angle1 = normalizeAngle(bar1.rect.angle);
14
float angle2 = normalizeAngle(bar2.rect.angle);
15
float angleDiff = std::abs(angle1 - angle2);

1
float length1 = bar1.rect.size.height;  // 假设已确保height是长边
2
float length2 = bar2.rect.size.height;
3

4
// 计算长度比（大/小）
5
float lengthRatio = std::max(length1, length2) / std::min(length1, length2);
6

7
// 判断
8
if (lengthRatio > 2.0) {
9
    // 长度相差超过2倍，不是装甲板
10
    return false;
11
}

1
float centerY1 = bar1.rect.center.y;
2
float centerY2 = bar2.rect.center.y;
3

4
// 计算y坐标差的绝对值
5
float heightDiff = std::abs(centerY1 - centerY2);
6

7
// 用平均长度作为参考
8
float avgLength = (bar1.rect.size.height + bar2.rect.size.height) / 2;
9

10
// 判断高度差是否合理
11
if (heightDiff > avgLength * 0.5) {
12
    // 高度差太大，不在同一水平线
13
    return false;
14
}

1
// 归一化角度到0-90度范围
2
float normalizeAngle(float angle) {
3
    angle = std::abs(angle);
4
    if (angle > 90) {
5
        angle = 180 - angle;
6
    }
7
    return angle;
8
}

1
bool isValidArmorPair(const LightBar& bar1, const LightBar& bar2) {
2
    // ============ 判据1：距离检查 ============
3
    float distance = cv::norm(bar1.center - bar2.center);
4
    float avgLength = (bar1.length + bar2.length) / 2;
5

6
    // 距离应该在1.5~4倍平均长度之间
7
    if (distance < avgLength * 1.5) {
8
        std::cout << " 距离太近: " << distance
9
                  << " < " << avgLength * 1.5 << std::endl;
10
        return false;
11
    }
12
    if (distance > avgLength * 4.0) {
13
        std::cout << " 距离太远: " << distance
14
                  << " > " << avgLength * 4.0 << std::endl;
15
        return false;
16
    }
17

18
    // ============ 判据2：角度检查 ============
19
    float angle1 = normalizeAngle(bar1.angle);
20
    float angle2 = normalizeAngle(bar2.angle);
21
    float angleDiff = std::abs(angle1 - angle2);
22

23
    // 角度差应小于10度
24
    if (angleDiff > 10.0) {
25
        std::cout << " 角度差太大: " << angleDiff << "°" << std::endl;
26
        return false;
27
    }
28

29
    // ============ 判据3：长度比检查 ============
30
    float maxLength = std::max(bar1.length, bar2.length);
31
    float minLength = std::min(bar1.length, bar2.length);
32
    float lengthRatio = maxLength / minLength;
33

34
    // 长度比应小于2.0
35
    if (lengthRatio > 2.0) {
36
        std::cout << " 长度比过大: " << lengthRatio << std::endl;
37
        return false;
38
    }
39

40
    // ============ 判据4：高度差检查 ============
41
    float heightDiff = std::abs(bar1.center.y - bar2.center.y);
42

43
    // 高度差应小于平均长度的50%
44
    if (heightDiff > avgLength * 0.5) {
45
        std::cout << " 高度差太大: " << heightDiff
46
                  << " > " << avgLength * 0.5 << std::endl;
47
        return false;
48
    }
49

50
    // ============ 通过所有判据 ============
51
    std::cout << "[通过] 距离:" << distance
52
              << " 角度差:" << angleDiff
53
              << " 长度比:" << lengthRatio
54
              << " 高度差:" << heightDiff << std::endl;
55
    return true;
56
}

1
void drawArmorPlate(cv::Mat& image, const ArmorPlate& armor, int index) {
2
    // 1. 绘制左侧灯条（绿色）
3
    cv::Point2f vertices1[4];
4
    armor.leftBar.rect.points(vertices1);
5
    for (int i = 0; i < 4; i++) {
6
        cv::line(image, vertices1[i], vertices1[(i+1)%4],
7
                 cv::Scalar(0, 255, 0), 2);
8
    }
9

10
    // 2. 绘制右侧灯条（绿色）
11
    cv::Point2f vertices2[4];
12
    armor.rightBar.rect.points(vertices2);
13
    for (int i = 0; i < 4; i++) {
14
        cv::line(image, vertices2[i], vertices2[(i+1)%4],
15
                 cv::Scalar(0, 255, 0), 2);
16
    }
17

18
    // 3. 绘制连接线（黄色）
19
    cv::line(image, armor.leftBar.center, armor.rightBar.center,
20
             cv::Scalar(0, 255, 255), 2);
21

22
    // 4. 绘制装甲板外框（蓝色）
23
    cv::Point2f topLeft = vertices1[0];
24
    cv::Point2f topRight = vertices2[1];
25
    cv::Point2f bottomRight = vertices2[2];
26
    cv::Point2f bottomLeft = vertices1[3];
27

28
    cv::line(image, topLeft, topRight, cv::Scalar(255, 0, 0), 2);
29
    cv::line(image, topRight, bottomRight, cv::Scalar(255, 0, 0), 2);
30
    cv::line(image, bottomRight, bottomLeft, cv::Scalar(255, 0, 0), 2);
31
    cv::line(image, bottomLeft, topLeft, cv::Scalar(255, 0, 0), 2);
32

33
    // 5. 绘制中心点（红色）
34
    cv::circle(image, armor.center, 5, cv::Scalar(0, 0, 255), -1);
35

36
    // 6. 显示装甲板信息
37
    std::string info = "Armor#" + std::to_string(index);
38
    cv::putText(image, info,
39
                cv::Point(armor.center.x - 30, armor.center.y - 15),
40
                cv::FONT_HERSHEY_SIMPLEX, 0.6,
41
                cv::Scalar(255, 255, 255), 2);
42

43
    // 7. 显示宽度信息
44
    std::string sizeInfo = "W:" + std::to_string((int)armor.width);
45
    cv::putText(image, sizeInfo,
46
                cv::Point(armor.center.x - 25, armor.center.y + 5),
47
                cv::FONT_HERSHEY_SIMPLEX, 0.4,
48
                cv::Scalar(255, 255, 255), 1);
49
}

1
void drawAllArmorPlates(cv::Mat& image, const std::vector<ArmorPlate>& armorPlates) {
2
    for (size_t i = 0; i < armorPlates.size(); i++) {
3
        drawArmorPlate(image, armorPlates[i], i);
4
    }
5

6
    // 在左上角显示统计信息
7
    std::string stats = "Armors: " + std::to_string(armorPlates.size());
8
    cv::putText(image, stats, cv::Point(10, 30),
9
                cv::FONT_HERSHEY_SIMPLEX, 1.0,
10
                cv::Scalar(0, 255, 255), 2);
11
}

1
// 快速预判（避免不必要的计算）
2
bool quickReject(const LightBar& bar1, const LightBar& bar2) {
3
    // 如果x坐标差太小，肯定不是装甲板（两灯条重叠）
4
    float xDiff = std::abs(bar1.center.x - bar2.center.x);
5
    if (xDiff < 10) return true;
6

7
    // 如果x坐标差太大，距离肯定超标
8
    float maxLength = std::max(bar1.length, bar2.length);
9
    if (xDiff > maxLength * 5) return true;
10

11
    return false;
12
}
13

14
std::vector<ArmorPlate> findArmorPlatesOptimized(const std::vector<LightBar>& lightBars) {
15
    std::vector<ArmorPlate> armorPlates;
16

17
    for (size_t i = 0; i < lightBars.size(); i++) {
18
        for (size_t j = i + 1; j < lightBars.size(); j++) {
19
            // 快速预判
20
            if (quickReject(lightBars[i], lightBars[j])) {
21
                continue;
22
            }
23

24
            // 详细判断
25
            if (isValidArmorPair(lightBars[i], lightBars[j])) {
26
                if (lightBars[i].center.x < lightBars[j].center.x) {
27
                    armorPlates.emplace_back(lightBars[i], lightBars[j]);
28
                } else {
29
                    armorPlates.emplace_back(lightBars[j], lightBars[i]);
30
                }
31
            }
32
        }
33
    }
34

35
    return armorPlates;
36
}

1
std::vector<ArmorPlate> findArmorPlatesSorted(std::vector<LightBar> lightBars) {
2
    // 按x坐标排序
3
    std::sort(lightBars.begin(), lightBars.end(),
4
              [](const LightBar& a, const LightBar& b) {
5
                  return a.center.x < b.center.x;
6
              });
7

8
    std::vector<ArmorPlate> armorPlates;
9

10
    for (size_t i = 0; i < lightBars.size(); i++) {
11
        for (size_t j = i + 1; j < lightBars.size(); j++) {
12
            // 如果j灯条的x坐标已经太远，后面的更远，直接break
13
            float xDiff = lightBars[j].center.x - lightBars[i].center.x;
14
            if (xDiff > lightBars[i].length * 5) {
15
                break;
16
            }
17

18
            if (isValidArmorPair(lightBars[i], lightBars[j])) {
19
                armorPlates.emplace_back(lightBars[i], lightBars[j]);
20
            }
21
        }
22
    }
23

24
    return armorPlates;
25
}

1
// 计算两灯条连线与水平线的夹角
2
float calculateTiltAngle(const LightBar& bar1, const LightBar& bar2) {
3
    float dx = bar2.center.x - bar1.center.x;
4
    float dy = bar2.center.y - bar1.center.y;
5
    return std::atan2(dy, dx) * 180.0 / CV_PI;
6
}
7

8
// 改进的高度差判据
9
bool isValidArmorPair_Advanced(const LightBar& bar1, const LightBar& bar2) {
10
    // ... 前面的判据 ...
11

12
    // 计算倾斜角度
13
    float tiltAngle = std::abs(calculateTiltAngle(bar1, bar2));
14

15
    // 如果倾斜角度大，放宽高度差要求
16
    float heightThreshold = avgLength * 0.5;
17
    if (tiltAngle > 15.0) {
18
        heightThreshold = avgLength * 0.8;  // 放宽到0.8倍
19
    }
20

21
    if (heightDiff > heightThreshold) {
22
        return false;
23
    }
24

25
    return true;
26
}

1
enum ArmorType {
2
    SMALL_ARMOR,  // 小装甲板
3
    LARGE_ARMOR   // 大装甲板
4
};
5

6
struct ArmorPlate {
7
    LightBar leftBar;
8
    LightBar rightBar;
9
    cv::Point2f center;
10
    float width;
11
    float height;
12
    ArmorType type;  // 新增：装甲板类型
13

14
    ArmorPlate(const LightBar& left, const LightBar& right)
15
        : leftBar(left), rightBar(right) {
16
        center.x = (left.center.x + right.center.x) / 2;
17
        center.y = (left.center.y + right.center.y) / 2;
18
        width = cv::norm(left.center - right.center);
19
        height = (left.length + right.length) / 2;
20

21
        // 根据宽高比判断类型
22
        float aspectRatio = width / height;
23
        if (aspectRatio > 3.0) {
24
            type = LARGE_ARMOR;  // 宽高比大，大装甲板
25
        } else {
26
            type = SMALL_ARMOR;  // 宽高比小，小装甲板
27
        }
28
    }
29

30
    std::string getTypeName() const {
31
        return (type == SMALL_ARMOR) ? "Small" : "Large";
32
    }
33
};
34

35
// 绘制时显示类型
36
void drawArmorPlate(cv::Mat& image, const ArmorPlate& armor, int index) {
37
    // ... 原来的绘制代码 ...
38

39
    // 显示类型
40
    std::string typeInfo = armor.getTypeName();
41
    cv::Scalar color = (armor.type == SMALL_ARMOR) ?
42
                       cv::Scalar(0, 255, 0) : cv::Scalar(255, 0, 255);
43
    cv::putText(image, typeInfo,
44
                cv::Point(armor.center.x - 30, armor.center.y + 20),
45
                cv::FONT_HERSHEY_SIMPLEX, 0.5, color, 2);
46
}

1
struct ArmorPlate {
2
    LightBar leftBar;
3
    LightBar rightBar;
4
    cv::Point2f center;
5
    float width;
6
    float height;
7
    float confidence;  // 置信度 0-100
8

9
    ArmorPlate(const LightBar& left, const LightBar& right)
10
        : leftBar(left), rightBar(right) {
11
        center.x = (left.center.x + right.center.x) / 2;
12
        center.y = (left.center.y + right.center.y) / 2;
13
        width = cv::norm(left.center - right.center);
14
        height = (left.length + right.length) / 2;
15

16
        // 计算置信度
17
        confidence = calculateConfidence(left, right);
18
    }
19

20
private:
21
    float calculateConfidence(const LightBar& bar1, const LightBar& bar2) {
22
        float score = 100.0;
23

24
        // 1. 角度差越小越好（满分20）
25
        float angle1 = normalizeAngle(bar1.angle);
26
        float angle2 = normalizeAngle(bar2.angle);
27
        float angleDiff = std::abs(angle1 - angle2);
28
        float angleScore = std::max(0.0f, 20.0f - angleDiff * 2.0f);
29

30
        // 2. 长度比越接近1越好（满分20）
31
        float lengthRatio = std::max(bar1.length, bar2.length) /
32
                           std::min(bar1.length, bar2.length);
33
        float lengthScore = std::max(0.0f, 20.0f - (lengthRatio - 1.0f) * 10.0f);
34

35
        // 3. 高度差越小越好（满分20）
36
        float avgLength = (bar1.length + bar2.length) / 2;
37
        float heightDiff = std::abs(bar1.center.y - bar2.center.y);
38
        float heightScore = std::max(0.0f, 20.0f - heightDiff / avgLength * 40.0f);
39

40
        // 4. 距离合理性（满分20）
41
        float distance = cv::norm(bar1.center - bar2.center);
42
        float idealDist = avgLength * 2.5;  // 理想距离
43
        float distDiff = std::abs(distance - idealDist);
44
        float distScore = std::max(0.0f, 20.0f - distDiff / avgLength * 10.0f);
45

46
        // 5. 面积大小（满分20）- 面积越大越好
47
        float area = width * height;
48
        float areaScore = std::min(20.0f, area / 10000.0f * 20.0f);
49

50
        score = angleScore + lengthScore + heightScore + distScore + areaScore;
51
        return std::min(100.0f, score);
52
    }
53
};
54

55
// 选择最佳装甲板
56
ArmorPlate selectBestArmor(const std::vector<ArmorPlate>& armorPlates) {
57
    if (armorPlates.empty()) {
58
        throw std::runtime_error("没有装甲板可选择！");
59
    }
60

61
    // 找出置信度最高的装甲板
62
    auto bestArmor = std::max_element(armorPlates.begin(), armorPlates.end(),
63
        [](const ArmorPlate& a, const ArmorPlate& b) {
64
            return a.confidence < b.confidence;
65
        });
66

67
    std::cout << "最佳装甲板置信度: " << bestArmor->confidence << std::endl;
68
    return *bestArmor;
69
}
70

71
// 绘制时显示置信度
72
void drawArmorPlateWithConfidence(cv::Mat& image, const ArmorPlate& armor, int index) {
73
    // ... 原来的绘制代码 ...
74

75
    // 显示置信度
76
    std::string confInfo = "Conf:" + std::to_string((int)armor.confidence);
77
    cv::putText(image, confInfo,
78
                cv::Point(armor.center.x - 30, armor.center.y + 20),
79
                cv::FONT_HERSHEY_SIMPLEX, 0.5,
80
                cv::Scalar(255, 255, 0), 1);
81
}

1
bool isValidArmorPair_Dynamic(const LightBar& bar1, const LightBar& bar2,
2
                               float cameraDistance) {
3
    float avgLength = (bar1.length + bar2.length) / 2;
4

5
    // 根据相机距离调整参数
6
    float distanceMin, distanceMax, angleDiffMax;
7

8
    if (cameraDistance < 3.0) {  // 近距离（3米内）
9
        distanceMin = avgLength * 2.0;
10
        distanceMax = avgLength * 3.5;
11
        angleDiffMax = 5.0;
12
    } else {  // 远距离（3米外）
13
        distanceMin = avgLength * 1.5;
14
        distanceMax = avgLength * 4.5;
15
        angleDiffMax = 12.0;
16
    }
17

18
    float distance = cv::norm(bar1.center - bar2.center);
19
    if (distance < distanceMin || distance > distanceMax) {
20
        return false;
21
    }
22

23
    // ... 其他判据 ...
24

25
    return true;
26
}

1
bool isValidArmorPair_Robust(const LightBar& bar1, const LightBar& bar2) {
2
    // 检查输入有效性
3
    if (bar1.length <= 0 || bar2.length <= 0) {
4
        std::cerr << "警告：灯条长度无效！" << std::endl;
5
        return false;
6
    }
7

8
    if (bar1.center == bar2.center) {
9
        std::cerr << "警告：两灯条中心重合！" << std::endl;
10
        return false;
11
    }
12

13
    // 避免除零错误
14
    float minLength = std::min(bar1.length, bar2.length);
15
    if (minLength < 1e-6) {
16
        return false;
17
    }
18

19
    // 正常判断流程...
20

21
    return true;
22
}

1
class ArmorTracker {
2
private:
3
    std::vector<ArmorPlate> historyArmors;  // 历史帧的装甲板
4
    const int HISTORY_SIZE = 5;  // 保持最近5帧
5

6
public:
7
    // 添加新帧的装甲板
8
    void addFrame(const std::vector<ArmorPlate>& armors) {
9
        if (historyArmors.size() >= HISTORY_SIZE) {
10
            historyArmors.erase(historyArmors.begin());
11
        }
12
        historyArmors.insert(historyArmors.end(), armors.begin(), armors.end());
13
    }
14

15
    // 获取稳定的装甲板（连续出现多帧）
16
    std::vector<ArmorPlate> getStableArmors() {
17
        // 统计每个位置出现的次数
18
        std::vector<ArmorPlate> stableArmors;
19

20
        // 实际应该实现位置聚类和计数
21
        // 这里简化处理
22

23
        return stableArmors;
24
    }
25
};

1
// 全局参数
2
int g_distanceMin = 15;  // 1.5 * 10
3
int g_distanceMax = 40;  // 4.0 * 10
4
int g_angleDiffMax = 10;
5
int g_lengthRatioMax = 20;  // 2.0 * 10
6
int g_heightDiffMax = 5;  // 0.5 * 10
7

8
void createParamWindow() {
9
    cv::namedWindow("参数调整");
10

11
    cv::createTrackbar("距离最小(x0.1)", "参数调整", &g_distanceMin, 50);
12
    cv::createTrackbar("距离最大(x0.1)", "参数调整", &g_distanceMax, 100);
13
    cv::createTrackbar("角度差最大", "参数调整", &g_angleDiffMax, 30);
14
    cv::createTrackbar("长度比最大(x0.1)", "参数调整", &g_lengthRatioMax, 50);
15
    cv::createTrackbar("高度差最大(x0.1)", "参数调整", &g_heightDiffMax, 20);
16
}
17

18
bool isValidArmorPair_Tunable(const LightBar& bar1, const LightBar& bar2) {
19
    float distance = cv::norm(bar1.center - bar2.center);
20
    float avgLength = (bar1.length + bar2.length) / 2;
21

22
    // 使用全局参数
23
    float distanceMin = g_distanceMin / 10.0f * avgLength;
24
    float distanceMax = g_distanceMax / 10.0f * avgLength;
25

26
    if (distance < distanceMin || distance > distanceMax) {
27
        return false;
28
    }
29

30
    // ... 其他判据使用全局参数 ...
31

32
    return true;
33
}