3D目标检测评价指标

# 伪代码：使用 open3d 或 scipy.spatial
def compute_3d_iou(box1, box2):
    # box: (center, size, rotation_matrix)
    corners1 = get_rotated_bbox_corners(box1)
    corners2 = get_rotated_bbox_corners(box2)
    # 计算交集凸多面体
    intersection_poly = convex_intersection(corners1, corners2)
    if intersection_poly is None: return 0.0
    intersection_vol = convex_polyhedron_volume(intersection_poly)
    vol1 = box1.l * box1.w * box1.h
    vol2 = box2.l * box2.w * box2.h
    iou = intersection_vol / (vol1 + vol2 - intersection_vol)
    return iou

#include <vector>
#include <array>
#include <cmath>
#include <algorithm>
#include <Eigen/Dense>

struct Box3D {
    float cx, cy, cz;      // 中心点
    float w, l, h;         // 长宽高
    float theta;           // 偏航角
};

// 获取3D框的8个顶点
std::vector<Eigen::Vector3f> get3DBoxCorners(const Box3D& box) {
    std::vector<Eigen::Vector3f> corners(8);
    
    // 局部坐标系下的8个顶点
    float w2 = box.w / 2, l2 = box.l / 2, h2 = box.h / 2;
    std::array<Eigen::Vector3f, 8> local_corners = {
        Eigen::Vector3f(-w2, -l2, -h2), Eigen::Vector3f(-w2, -l2, h2),
        Eigen::Vector3f(-w2,  l2, -h2), Eigen::Vector3f(-w2,  l2, h2),
        Eigen::Vector3f( w2, -l2, -h2), Eigen::Vector3f( w2, -l2, h2),
        Eigen::Vector3f( w2,  l2, -h2), Eigen::Vector3f( w2,  l2, h2)
    };
    
    // 旋转矩阵 (绕z轴)
    float cos_t = std::cos(box.theta);
    float sin_t = std::sin(box.theta);
    Eigen::Matrix3f R;
    R << cos_t, -sin_t, 0,
         sin_t,  cos_t, 0,
         0,      0,     1;
    
    for (int i = 0; i < 8; ++i) {
        corners[i] = R * local_corners[i] + Eigen::Vector3f(box.cx, box.cy, box.cz);
    }
    
    return corners;
}

// 多边形裁剪 (Sutherland-Hodgman)
std::vector<Eigen::Vector2f> sutherlandHodgman(
    const std::vector<Eigen::Vector2f>& subjectPolygon,
    const std::vector<Eigen::Vector2f>& clipPolygon) {
    
    std::vector<Eigen::Vector2f> outputList = subjectPolygon;
    
    for (size_t edge = 0; edge < clipPolygon.size(); ++edge) {
        std::vector<Eigen::Vector2f> inputList = outputList;
        outputList.clear();
        
        if (inputList.empty()) break;
        
        Eigen::Vector2f A = clipPolygon[edge];
        Eigen::Vector2f B = clipPolygon[(edge + 1) % clipPolygon.size()];
        
        for (size_t i = 0; i < inputList.size(); ++i) {
            Eigen::Vector2f P = inputList[i];
            Eigen::Vector2f Q = inputList[(i + 1) % inputList.size()];
            
            // 计算点在边的哪一侧
            float cross1 = (B.x() - A.x()) * (P.y() - A.y()) - 
                          (B.y() - A.y()) * (P.x() - A.x());
            float cross2 = (B.x() - A.x()) * (Q.y() - A.y()) - 
                          (B.y() - A.y()) * (Q.x() - A.x());
            
            bool insideP = cross1 >= 0;
            bool insideQ = cross2 >= 0;
            
            if (insideP && insideQ) {
                // 都在内部，保留Q
                outputList.push_back(Q);
            } else if (insideP && !insideQ) {
                // P在内，Q在外，保留交点
                // 计算线段PQ与边AB的交点
                float t = cross1 / (cross1 - cross2);
                Eigen::Vector2f intersection = P + t * (Q - P);
                outputList.push_back(intersection);
            } else if (!insideP && insideQ) {
                // P在外，Q在内，保留交点和Q
                float t = cross1 / (cross1 - cross2);
                Eigen::Vector2f intersection = P + t * (Q - P);
                outputList.push_back(intersection);
                outputList.push_back(Q);
            }
            // 都在外部，不保留
        }
    }
    
    return outputList;
}

// 计算多边形面积 (Shoelace公式)
float polygonArea(const std::vector<Eigen::Vector2f>& poly) {
    if (poly.size() < 3) return 0.0f;
    
    float area = 0.0f;
    for (size_t i = 0; i < poly.size(); ++i) {
        size_t j = (i + 1) % poly.size();
        area += poly[i].x() * poly[j].y();
        area -= poly[j].x() * poly[i].y();
    }
    return std::abs(area) / 2.0f;
}

// 计算BEV平面两个旋转矩形的IoU
float computeBEVIoURotated(const Box3D& box1, const Box3D& box2) {
    // 获取BEV corners (只取x,y，取底部4个点: 0,2,6,4)
    auto corners1_3d = get3DBoxCorners(box1);
    auto corners2_3d = get3DBoxCorners(box2);
    
    std::vector<Eigen::Vector2f> poly1, poly2;
    std::array<int, 4> indices = {0, 2, 6, 4};
    
    for (int idx : indices) {
        poly1.push_back(corners1_3d[idx].head<2>());
        poly2.push_back(corners2_3d[idx].head<2>());
    }
    
    // 使用Sutherland-Hodgman计算交集
    std::vector<Eigen::Vector2f> intersection = sutherlandHodgman(poly1, poly2);
    if (intersection.empty()) {
        intersection = sutherlandHodgman(poly2, poly1);
    }
    
    float area1 = polygonArea(poly1);
    float area2 = polygonArea(poly2);
    float interArea = polygonArea(intersection);
    
    float unionArea = area1 + area2 - interArea;
    return (unionArea > 0) ? (interArea / unionArea) : 0.0f;
}

// 完整3D IoU计算
float compute3DIoU(const Box3D& box1, const Box3D& box2) {
    // 高度交集
    float z1_min = box1.cz - box1.h / 2;
    float z1_max = box1.cz + box1.h / 2;
    float z2_min = box2.cz - box2.h / 2;
    float z2_max = box2.cz + box2.h / 2;
    
    float z_overlap = std::min(z1_max, z2_max) - std::max(z1_min, z2_min);
    if (z_overlap <= 0) return 0.0f;
    
    // BEV交集
    float bev_iou = computeBEVIoURotated(box1, box2);
    float area1 = box1.w * box1.l;
    float area2 = box2.w * box2.l;
    float bev_overlap = bev_iou * (area1 + area2) / (1.0f + bev_iou);
    
    // 精确计算应使用多边形交集面积
    // 这里简化处理
    
    float intersection_3d = z_overlap * bev_overlap;
    float vol1 = area1 * box1.h;
    float vol2 = area2 * box2.h;
    float union_3d = vol1 + vol2 - intersection_3d;
    
    return (union_3d > 0) ? (intersection_3d / union_3d) : 0.0f;
}

// 简化版：BEV 2D IoU + 高度检查（忽略旋转细节）
float approx3DIoU(const Box3D& box1, const Box3D& box2) {
    // 轴对齐近似
    float x1_min = box1.cx - box1.w / 2;
    float x1_max = box1.cx + box1.w / 2;
    float y1_min = box1.cy - box1.l / 2;
    float y1_max = box1.cy + box1.l / 2;
    float z1_min = box1.cz - box1.h / 2;
    float z1_max = box1.cz + box1.h / 2;
    
    float x2_min = box2.cx - box2.w / 2;
    float x2_max = box2.cx + box2.w / 2;
    float y2_min = box2.cy - box2.l / 2;
    float y2_max = box2.cy + box2.l / 2;
    float z2_min = box2.cz - box2.h / 2;
    float z2_max = box2.cz + box2.h / 2;
    
    float x_overlap = std::max(0.0f, std::min(x1_max, x2_max) - std::max(x1_min, x2_min));
    float y_overlap = std::max(0.0f, std::min(y1_max, y2_max) - std::max(y1_min, y2_min));
    float z_overlap = std::max(0.0f, std::min(z1_max, z2_max) - std::max(z1_min, z2_min));
    
    float intersection = x_overlap * y_overlap * z_overlap;
    float vol1 = box1.w * box1.l * box1.h;
    float vol2 = box2.w * box2.l * box2.h;
    float uni = vol1 + vol2 - intersection;
    
    return (uni > 0) ? (intersection / uni) : 0.0f;
}

#include <vector>
#include <algorithm>
#include <numeric>

struct Detection {
    Box3D box;
    float score;
    int class_id;
    bool is_tp = false;
};

struct GroundTruth {
    Box3D box;
    int class_id;
    bool matched = false;
};

class APEvaluator {
public:
    float computeAP(std::vector<Detection>& predictions,
                    std::vector<GroundTruth>& ground_truths,
                    float iou_threshold) {
        // 按置信度降序排列
        std::sort(predictions.begin(), predictions.end(),
                  [](const Detection& a, const Detection& b) {
                      return a.score > b.score;
                  });
        
        std::vector<bool> tp(predictions.size(), false);
        std::vector<bool> fp(predictions.size(), false);
        
        // 重置GT匹配状态
        for (auto& gt : ground_truths) {
            gt.matched = false;
        }
        
        // 贪心匹配
        for (size_t i = 0; i < predictions.size(); ++i) {
            const auto& pred = predictions[i];
            
            float max_iou = 0.0f;
            size_t max_gt_idx = -1;
            
            // 找同类别、未匹配、IoU最大的GT
            for (size_t j = 0; j < ground_truths.size(); ++j) {
                auto& gt = ground_truths[j];
                if (gt.matched || gt.class_id != pred.class_id) continue;
                
                float iou = compute3DIoU(pred.box, gt.box);
                if (iou > max_iou) {
                    max_iou = iou;
                    max_gt_idx = j;
                }
            }
            
            if (max_iou >= iou_threshold && max_gt_idx != -1) {
                tp[i] = true;
                ground_truths[max_gt_idx].matched = true;
            } else {
                fp[i] = true;
            }
        }
        
        // 计算PR曲线
        return interpolateAP(tp, fp, ground_truths.size());
    }
    
private:
    float interpolateAP(const std::vector<bool>& tp,
                        const std::vector<bool>& fp,
                        size_t num_gt) {
        std::vector<float> precisions;
        std::vector<float> recalls;
        
        int tp_sum = 0, fp_sum = 0;
        for (size_t i = 0; i < tp.size(); ++i) {
            if (tp[i]) tp_sum++;
            if (fp[i]) fp_sum++;
            
            float precision = static_cast<float>(tp_sum) / (tp_sum + fp_sum);
            float recall = static_cast<float>(tp_sum) / num_gt;
            
            precisions.push_back(precision);
            recalls.push_back(recall);
        }
        
        // 101点插值 (COCO风格)
        std::vector<float> recall_thresholds;
        for (int i = 0; i <= 100; ++i) {
            recall_thresholds.push_back(i / 100.0f);
        }
        
        float ap = 0.0f;
        for (float r_t : recall_thresholds) {
            float max_p = 0.0f;
            for (size_t i = 0; i < recalls.size(); ++i) {
                if (recalls[i] >= r_t) {
                    max_p = std::max(max_p, precisions[i]);
                }
            }
            ap += max_p;
        }
        
        return ap / recall_thresholds.size();
    }
};

// Car: AP@0.7 (3D IoU >= 0.7才算匹配)
// Pedestrian: AP@0.5  
// Cyclist: AP@0.5

// 且分难度：
// Easy: 近处，完整可见
// Moderate: 中等距离，部分遮挡
// Hard: 远处，严重遮挡

struct KITTIDifficulty {
    float min_height;
    float max_occlusion;
    float max_truncation;
    float min_depth;
};

std::map<std::string, KITTIDifficulty> KITTI_CONFIG = {
    {"Easy",   {40.0f, 0, 0.15f, 0.0f}},
    {"Moderate", {25.0f, 1, 0.30f, 0.0f}},
    {"Hard",   {25.0f, 2, 0.50f, 0.0f}}
};

#include <Eigen/Dense>

struct NuScenesBox {
    Box3D box;
    Eigen::Vector2f velocity;  // (vx, vy)
    std::vector<int> attributes;
};

struct TPMetrics {
    float ate;  // Average Translation Error
    float ase;  // Average Scale Error
    float aoe;  // Average Orientation Error
    float ave;  // Average Velocity Error
    float aae;  // Average Attribute Error
};

class NuScenesEvaluator {
public:
    // ATE（平移误差）
    float computeATE(const NuScenesBox& gt, const NuScenesBox& pred) {
        Eigen::Vector3f gt_center(gt.box.cx, gt.box.cy, gt.box.cz);
        Eigen::Vector3f pred_center(pred.box.cx, pred.box.cy, pred.box.cz);
        return (gt_center - pred_center).norm();
    }
    
    // ASE（尺度误差）
    float computeASE(const NuScenesBox& gt, const NuScenesBox& pred) {
        // 计算尺寸维度的IoU（假设中心对齐）
        float w_i = std::min(gt.box.w, pred.box.w);
        float l_i = std::min(gt.box.l, pred.box.l);
        float h_i = std::min(gt.box.h, pred.box.h);
        
        float w_u = std::max(gt.box.w, pred.box.w);
        float l_u = std::max(gt.box.l, pred.box.l);
        float h_u = std::max(gt.box.h, pred.box.h);
        
        float iou_3d = (w_i * l_i * h_i) / (w_u * l_u * h_u);
        return 1.0f - iou_3d;
    }
    
    // AOE（朝向误差）
    float computeAOE(const NuScenesBox& gt, const NuScenesBox& pred, 
                     bool is_vehicle = true) {
        float diff = std::abs(gt.box.theta - pred.box.theta);
        
        // 归一化到 [0, π]
        while (diff > M_PI) diff -= 2 * M_PI;
        while (diff < -M_PI) diff += 2 * M_PI;
        diff = std::abs(diff);
        
        // 对于车辆，考虑对称性（0和π等价）
        if (is_vehicle) {
            diff = std::min(diff, static_cast<float>(M_PI) - diff);
        }
        
        return diff;
    }
    
    // AVE（速度误差）
    float computeAVE(const NuScenesBox& gt, const NuScenesBox& pred) {
        return (gt.velocity - pred.velocity).norm();
    }
    
    // AAE（属性误差）
    float computeAAE(const NuScenesBox& gt, const NuScenesBox& pred) {
        if (gt.attributes.empty()) return 0.0f;
        
        int correct = 0;
        size_t min_len = std::min(gt.attributes.size(), pred.attributes.size());
        for (size_t i = 0; i < min_len; ++i) {
            if (gt.attributes[i] == pred.attributes[i]) correct++;
        }
        return 1.0f - static_cast<float>(correct) / gt.attributes.size();
    }
    
    // 计算单个类别的TP指标
    TPMetrics computeTPMetrics(
        const std::vector<NuScenesBox>& gts,
        const std::vector<NuScenesBox>& preds,
        float distance_threshold = 2.0f) {
        
        // 基于BEV中心距离匹配（非IoU）
        std::vector<bool> matched(gts.size(), false);
        std::vector<std::pair<size_t, size_t>> matches;
        
        for (size_t i = 0; i < preds.size(); ++i) {
            float min_dist = distance_threshold;
            size_t best_gt = -1;
            
            for (size_t j = 0; j < gts.size(); ++j) {
                if (matched[j]) continue;
                
                float dist = std::hypot(
                    preds[i].box.cx - gts[j].box.cx,
                    preds[i].box.cy - gts[j].box.cy
                );
                
                if (dist < min_dist) {
                    min_dist = dist;
                    best_gt = j;
                }
            }
            
            if (best_gt != -1) {
                matched[best_gt] = true;
                matches.push_back({best_gt, i});
            }
        }
        
        // 计算各指标
        TPMetrics metrics = {0, 0, 0, 0, 0};
        if (matches.empty()) return metrics;
        
        for (const auto& [gt_idx, pred_idx] : matches) {
            metrics.ate += computeATE(gts[gt_idx], preds[pred_idx]);
            metrics.ase += computeASE(gts[gt_idx], preds[pred_idx]);
            metrics.aoe += computeAOE(gts[gt_idx], preds[pred_idx]);
            metrics.ave += computeAVE(gts[gt_idx], preds[pred_idx]);
            metrics.aae += computeAAE(gts[gt_idx], preds[pred_idx]);
        }
        
        float n = matches.size();
        metrics.ate /= n;
        metrics.ase /= n;
        metrics.aoe /= n;
        metrics.ave /= n;
        metrics.aae /= n;
        
        return metrics;
    }
    
    // 计算NDS
    float computeNDS(float mAP, const TPMetrics& tp) {
        // 归一化各误差
        float norm_ate = std::min(1.0f, tp.ate / 2.0f);      // 2m阈值
        float norm_ase = std::min(1.0f, tp.ase);
        float norm_aoe = std::min(1.0f, tp.aoe / static_cast<float>(M_PI));
        float norm_ave = std::min(1.0f, tp.ave / 0.5f);      // 0.5m/s阈值
        float norm_aae = std::min(1.0f, tp.aae);
        
        float nds = 0.5f * mAP + 
                    0.1f * (1 - norm_ate) +
                    0.1f * (1 - norm_ase) +
                    0.1f * (1 - norm_aoe) +
                    0.1f * (1 - norm_ave) +
                    0.1f * (1 - norm_aae);
        
        return nds;
    }
};

#include <vector>
#include <string>
#include <map>
#include <algorithm>
#include <numeric>
#include <Eigen/Dense>

class DetectionEvaluator {
public:
    DetectionEvaluator(const std::string& dataset = "kitti") 
        : dataset_(dataset) {
        if (dataset == "kitti") {
            iou_thresholds_ = {0.7f};  // Car
            // 对于Pedestrian/Cyclist使用0.5
        } else if (dataset == "nuscenes") {
            iou_thresholds_ = {};  // NuScenes使用距离匹配
        } else if (dataset == "coco") {
            for (float t = 0.5f; t <= 0.95f; t += 0.05f) {
                iou_thresholds_.push_back(t);
            }
        }
    }
    
    struct EvaluationResult {
        float mAP = 0.0f;
        float NDS = 0.0f;  // 仅NuScenes
        std::map<std::string, float> class_AP;
        std::map<std::string, TPMetrics> class_TP;
    };
    
    EvaluationResult evaluate(
        const std::map<std::string, std::vector<Detection>>& predictions,
        const std::map<std::string, std::vector<GroundTruth>>& ground_truths) {
        
        EvaluationResult result;
        std::vector<float> all_aps;
        
        for (const auto& [class_name, preds] : predictions) {
            const auto& gts = ground_truths.at(class_name);
            
            if (dataset_ == "nuscenes") {
                // NuScenes评估
                float ap = computeNuScenesAP(preds, gts);
                result.class_AP[class_name] = ap;
                all_aps.push_back(ap);
                
                // 计算TP指标
                NuScenesEvaluator nusc_eval;
                auto tp = nusc_eval.computeTPMetrics(
                    convertToNuScenesBoxes(gts),
                    convertToNuScenesBoxes(preds)
                );
                result.class_TP[class_name] = tp;
                
            } else {
                // KITTI/COCO风格：多IoU阈值平均
                std::vector<float> aps;
                for (float thresh : iou_thresholds_) {
                    aps.push_back(computeAP(preds, gts, thresh));
                }
                float mean_ap = std::accumulate(aps.begin(), aps.end(), 0.0f) / aps.size();
                result.class_AP[class_name] = mean_ap;
                all_aps.push_back(mean_ap);
            }
        }
        
        result.mAP = std::accumulate(all_aps.begin(), all_aps.end(), 0.0f) 
                     / all_aps.size();
        
        // 计算NDS（仅NuScenes）
        if (dataset_ == "nuscenes") {
            NuScenesEvaluator nusc_eval;
            TPMetrics avg_tp = averageTPMetrics(result.class_TP);
            result.NDS = nusc_eval.computeNDS(result.mAP, avg_tp);
        }
        
        return result;
    }
    
private:
    std::string dataset_;
    std::vector<float> iou_thresholds_;
    APEvaluator ap_evaluator_;
    NuScenesEvaluator nusc_evaluator_;
    
    float computeAP(const std::vector<Detection>& preds,
                    const std::vector<GroundTruth>& gts,
                    float iou_thresh) {
        return ap_evaluator_.computeAP(
            const_cast<std::vector<Detection>&>(preds),
            const_cast<std::vector<GroundTruth>&>(gts),
            iou_thresh
        );
    }
    
    float computeNuScenesAP(const std::vector<Detection>& preds,
                            const std::vector<GroundTruth>& gts) {
        // NuScenes使用距离匹配而非IoU
        // 实现类似但匹配逻辑不同
        return 0.0f;  // 简化
    }
    
    std::vector<NuScenesBox> convertToNuScenesBoxes(
        const std::vector<GroundTruth>& gts) {
        // 转换函数
        return {};
    }
    
    TPMetrics averageTPMetrics(const std::map<std::string, TPMetrics>& tp_map) {
        TPMetrics avg = {0, 0, 0, 0, 0};
        if (tp_map.empty()) return avg;
        
        for (const auto& [name, tp] : tp_map) {
            avg.ate += tp.ate;
            avg.ase += tp.ase;
            avg.aoe += tp.aoe;
            avg.ave += tp.ave;
            avg.aae += tp.aae;
        }
        
        float n = tp_map.size();
        avg.ate /= n; avg.ase /= n; avg.aoe /= n; avg.ave /= n; avg.aae /= n;
        return avg;
    }
};