colmap 0.1.2

//! 几何验证模块
//! 
//! 提供特征匹配的几何一致性验证功能

use crate::core::{Feature, ColmapError};
use crate::feature::FeatureMatch;
use nalgebra::{Matrix3, Vector2, Vector3};
use rand::Rng;

/// 几何验证配置
#[derive(Debug, Clone)]
pub struct GeometricConfig {
    /// RANSAC 迭代次数
    pub ransac_iterations: usize,
    /// RANSAC 阈值
    pub ransac_threshold: f64,
    /// 最小内点数量
    pub min_inliers: usize,
    /// 置信度
    pub confidence: f64,
    /// 是否使用归一化坐标
    pub use_normalized_coordinates: bool,
}

impl Default for GeometricConfig {
    fn default() -> Self {
        Self {
            ransac_iterations: 1000,
            ransac_threshold: 1.0,
            min_inliers: 8,
            confidence: 0.99,
            use_normalized_coordinates: true,
        }
    }
}

/// 几何验证结果
#[derive(Debug, Clone)]
pub struct GeometricResult {
    /// 内点匹配
    pub inlier_matches: Vec<FeatureMatch>,
    /// 外点匹配
    pub outlier_matches: Vec<FeatureMatch>,
    /// 估计的几何模型
    pub model: GeometricModel,
    /// 内点比例
    pub inlier_ratio: f64,
    /// 重投影误差
    pub reprojection_error: f64,
}

/// 几何模型类型
#[derive(Debug, Clone)]
pub enum GeometricModel {
    /// 基础矩阵
    FundamentalMatrix(Matrix3<f64>),
    /// 本质矩阵
    EssentialMatrix(Matrix3<f64>),
    /// 单应性矩阵
    Homography(Matrix3<f64>),
    /// 仿射变换
    Affine(Matrix3<f64>),
}

/// 几何验证器
pub struct GeometricVerifier {
    config: GeometricConfig,
}

impl GeometricVerifier {
    /// 创建新的几何验证器
    pub fn new(config: GeometricConfig) -> Self {
        Self { config }
    }
    
    /// 使用基础矩阵验证匹配
    pub fn verify_with_fundamental_matrix(
        &self,
        features1: &[Feature],
        features2: &[Feature],
        matches: &[FeatureMatch],
    ) -> Result<GeometricResult, ColmapError> {
        if matches.len() < self.config.min_inliers {
            return Err(ColmapError::InvalidParameter(
                "Not enough matches for fundamental matrix estimation".to_string()
            ));
        }
        
        // 提取匹配点
        let (points1, points2) = self.extract_matched_points(features1, features2, matches)?;
        
        // 估计基础矩阵
        let (fundamental_matrix, inlier_mask) = self.estimate_fundamental_matrix(&points1, &points2)?;
        
        // 分离内点和外点
        let (inlier_matches, outlier_matches) = self.separate_inliers_outliers(matches, &inlier_mask);
        
        // 计算重投影误差
        let reprojection_error = self.compute_fundamental_reprojection_error(
            &fundamental_matrix, &points1, &points2, &inlier_mask
        )?;
        
        let inlier_ratio = inlier_matches.len() as f64 / matches.len() as f64;
        
        Ok(GeometricResult {
            inlier_matches,
            outlier_matches,
            model: GeometricModel::FundamentalMatrix(fundamental_matrix),
            inlier_ratio,
            reprojection_error,
        })
    }
    
    /// 使用本质矩阵验证匹配
    pub fn verify_with_essential_matrix(
        &self,
        features1: &[Feature],
        features2: &[Feature],
        matches: &[FeatureMatch],
        camera_matrix: &Matrix3<f64>,
    ) -> Result<GeometricResult, ColmapError> {
        if matches.len() < self.config.min_inliers {
            return Err(ColmapError::InvalidParameter(
                "Not enough matches for essential matrix estimation".to_string()
            ));
        }
        
        // 提取匹配点并归一化
        let (points1, points2) = self.extract_matched_points(features1, features2, matches)?;
        let (norm_points1, norm_points2) = self.normalize_points(&points1, &points2, camera_matrix)?;
        
        // 估计本质矩阵
        let (essential_matrix, inlier_mask) = self.estimate_essential_matrix(&norm_points1, &norm_points2)?;
        
        // 分离内点和外点
        let (inlier_matches, outlier_matches) = self.separate_inliers_outliers(matches, &inlier_mask);
        
        // 计算重投影误差
        let reprojection_error = self.compute_essential_reprojection_error(
            &essential_matrix, &norm_points1, &norm_points2, &inlier_mask
        )?;
        
        let inlier_ratio = inlier_matches.len() as f64 / matches.len() as f64;
        
        Ok(GeometricResult {
            inlier_matches,
            outlier_matches,
            model: GeometricModel::EssentialMatrix(essential_matrix),
            inlier_ratio,
            reprojection_error,
        })
    }
    
    /// 使用单应性矩阵验证匹配
    pub fn verify_with_homography(
        &self,
        features1: &[Feature],
        features2: &[Feature],
        matches: &[FeatureMatch],
    ) -> Result<GeometricResult, ColmapError> {
        if matches.len() < 4 {
            return Err(ColmapError::InvalidParameter(
                "Not enough matches for homography estimation".to_string()
            ));
        }
        
        // 提取匹配点
        let (points1, points2) = self.extract_matched_points(features1, features2, matches)?;
        
        // 估计单应性矩阵
        let (homography, inlier_mask) = self.estimate_homography(&points1, &points2)?;
        
        // 分离内点和外点
        let (inlier_matches, outlier_matches) = self.separate_inliers_outliers(matches, &inlier_mask);
        
        // 计算重投影误差
        let reprojection_error = self.compute_homography_reprojection_error(
            &homography, &points1, &points2, &inlier_mask
        )?;
        
        let inlier_ratio = inlier_matches.len() as f64 / matches.len() as f64;
        
        Ok(GeometricResult {
            inlier_matches,
            outlier_matches,
            model: GeometricModel::Homography(homography),
            inlier_ratio,
            reprojection_error,
        })
    }
    
    /// 自动选择最佳几何模型
    pub fn verify_auto(
        &self,
        features1: &[Feature],
        features2: &[Feature],
        matches: &[FeatureMatch],
        camera_matrix: Option<&Matrix3<f64>>,
    ) -> Result<GeometricResult, ColmapError> {
        let mut best_result: Option<GeometricResult> = None;
        let mut best_score = 0.0;
        
        // 尝试单应性矩阵
        if let Ok(homography_result) = self.verify_with_homography(features1, features2, matches) {
            let score = self.compute_model_score(&homography_result);
            if score > best_score {
                best_score = score;
                best_result = Some(homography_result);
            }
        }
        
        // 尝试基础矩阵
        if let Ok(fundamental_result) = self.verify_with_fundamental_matrix(features1, features2, matches) {
            let score = self.compute_model_score(&fundamental_result);
            if score > best_score {
                best_score = score;
                best_result = Some(fundamental_result);
            }
        }
        
        // 如果有相机矩阵，尝试本质矩阵
        if let Some(camera_matrix) = camera_matrix
            && let Ok(essential_result) = self.verify_with_essential_matrix(
                features1, features2, matches, camera_matrix
            ) {
                let score = self.compute_model_score(&essential_result);
                if score > best_score {
                    best_result = Some(essential_result);
                }
            }
        
        best_result.ok_or_else(|| ColmapError::FeatureMatching(
            "Failed to find valid geometric model".to_string()
        ))
    }
    
    /// 提取匹配点坐标
    fn extract_matched_points(
        &self,
        features1: &[Feature],
        features2: &[Feature],
        matches: &[FeatureMatch],
    ) -> Result<(Vec<Vector2<f64>>, Vec<Vector2<f64>>), ColmapError> {
        let mut points1 = Vec::with_capacity(matches.len());
        let mut points2 = Vec::with_capacity(matches.len());
        
        for match_result in matches {
            if match_result.query_idx >= features1.len() || match_result.train_idx >= features2.len() {
                return Err(ColmapError::InvalidParameter(
                    "Match index out of bounds".to_string()
                ));
            }
            
            let f1 = &features1[match_result.query_idx];
            let f2 = &features2[match_result.train_idx];
            
            points1.push(Vector2::new(f1.point.coords.x, f1.point.coords.y));
            points2.push(Vector2::new(f2.point.coords.x, f2.point.coords.y));
        }
        
        Ok((points1, points2))
    }
    
    /// 估计基础矩阵
    fn estimate_fundamental_matrix(
        &self,
        points1: &[Vector2<f64>],
        points2: &[Vector2<f64>],
    ) -> Result<(Matrix3<f64>, Vec<bool>), ColmapError> {
        if points1.len() < 8 {
            return Err(ColmapError::InvalidParameter(
                "Need at least 8 points for fundamental matrix estimation".to_string()
            ));
        }
        
        let mut best_matrix = Matrix3::zeros();
        let mut best_inliers = Vec::new();
        let mut best_count = 0;
        
        let mut rng = rand::thread_rng();
        
        for _ in 0..self.config.ransac_iterations {
            // 随机选择8个点
            let mut indices = Vec::new();
            while indices.len() < 8 {
                let idx = rng.gen_range(0..points1.len());
                if !indices.contains(&idx) {
                    indices.push(idx);
                }
            }
            
            // 使用8点算法估计基础矩阵
            if let Ok(matrix) = self.estimate_fundamental_8point(&indices, points1, points2) {
                // 计算内点
                let inliers = self.compute_fundamental_inliers(&matrix, points1, points2);
                let inlier_count = inliers.iter().filter(|&&x| x).count();
                
                if inlier_count > best_count {
                    best_count = inlier_count;
                    best_matrix = matrix;
                    best_inliers = inliers;
                }
            }
        }
        
        if best_count < self.config.min_inliers {
            return Err(ColmapError::FeatureMatching(
                "Not enough inliers found".to_string()
            ));
        }
        
        Ok((best_matrix, best_inliers))
    }
    
    /// 估计本质矩阵
    fn estimate_essential_matrix(
        &self,
        points1: &[Vector2<f64>],
        points2: &[Vector2<f64>],
    ) -> Result<(Matrix3<f64>, Vec<bool>), ColmapError> {
        if points1.len() < 5 {
            return Err(ColmapError::InvalidParameter(
                "Need at least 5 points for essential matrix estimation".to_string()
            ));
        }
        
        let mut best_matrix = Matrix3::zeros();
        let mut best_inliers = Vec::new();
        let mut best_count = 0;
        
        let mut rng = rand::thread_rng();
        
        for _ in 0..self.config.ransac_iterations {
            // 随机选择5个点
            let mut indices = Vec::new();
            while indices.len() < 5 {
                let idx = rng.gen_range(0..points1.len());
                if !indices.contains(&idx) {
                    indices.push(idx);
                }
            }
            
            // 使用5点算法估计本质矩阵
            if let Ok(matrix) = self.estimate_essential_5point(&indices, points1, points2) {
                // 计算内点
                let inliers = self.compute_essential_inliers(&matrix, points1, points2);
                let inlier_count = inliers.iter().filter(|&&x| x).count();
                
                if inlier_count > best_count {
                    best_count = inlier_count;
                    best_matrix = matrix;
                    best_inliers = inliers;
                }
            }
        }
        
        if best_count < self.config.min_inliers {
            return Err(ColmapError::FeatureMatching(
                "Not enough inliers found".to_string()
            ));
        }
        
        Ok((best_matrix, best_inliers))
    }
    
    /// 估计单应性矩阵
    fn estimate_homography(
        &self,
        points1: &[Vector2<f64>],
        points2: &[Vector2<f64>],
    ) -> Result<(Matrix3<f64>, Vec<bool>), ColmapError> {
        if points1.len() < 4 {
            return Err(ColmapError::InvalidParameter(
                "Need at least 4 points for homography estimation".to_string()
            ));
        }
        
        let mut best_matrix = Matrix3::zeros();
        let mut best_inliers = Vec::new();
        let mut best_count = 0;
        
        let mut rng = rand::thread_rng();
        
        for _ in 0..self.config.ransac_iterations {
            // 随机选择4个点
            let mut indices = Vec::new();
            while indices.len() < 4 {
                let idx = rng.gen_range(0..points1.len());
                if !indices.contains(&idx) {
                    indices.push(idx);
                }
            }
            
            // 使用4点算法估计单应性矩阵
            if let Ok(matrix) = self.estimate_homography_4point(&indices, points1, points2) {
                // 计算内点
                let inliers = self.compute_homography_inliers(&matrix, points1, points2);
                let inlier_count = inliers.iter().filter(|&&x| x).count();
                
                if inlier_count > best_count {
                    best_count = inlier_count;
                    best_matrix = matrix;
                    best_inliers = inliers;
                }
            }
        }
        
        if best_count < 4 {
            return Err(ColmapError::FeatureMatching(
                "Not enough inliers found".to_string()
            ));
        }
        
        Ok((best_matrix, best_inliers))
    }
    
    /// 归一化点坐标
    fn normalize_points(
        &self,
        points1: &[Vector2<f64>],
        points2: &[Vector2<f64>],
        camera_matrix: &Matrix3<f64>,
    ) -> Result<(Vec<Vector2<f64>>, Vec<Vector2<f64>>), ColmapError> {
        let inv_k = camera_matrix.try_inverse()
            .ok_or_else(|| ColmapError::InvalidParameter("Camera matrix is not invertible".to_string()))?;
        
        let mut norm_points1 = Vec::with_capacity(points1.len());
        let mut norm_points2 = Vec::with_capacity(points2.len());
        
        for point in points1 {
            let homogeneous = Vector3::new(point.x, point.y, 1.0);
            let normalized = inv_k * homogeneous;
            norm_points1.push(Vector2::new(normalized.x / normalized.z, normalized.y / normalized.z));
        }
        
        for point in points2 {
            let homogeneous = Vector3::new(point.x, point.y, 1.0);
            let normalized = inv_k * homogeneous;
            norm_points2.push(Vector2::new(normalized.x / normalized.z, normalized.y / normalized.z));
        }
        
        Ok((norm_points1, norm_points2))
    }
    
    /// 分离内点和外点匹配
    fn separate_inliers_outliers(
        &self,
        matches: &[FeatureMatch],
        inlier_mask: &[bool],
    ) -> (Vec<FeatureMatch>, Vec<FeatureMatch>) {
        let mut inliers = Vec::new();
        let mut outliers = Vec::new();
        
        for (i, &is_inlier) in inlier_mask.iter().enumerate() {
            if i < matches.len() {
                if is_inlier {
                    inliers.push(matches[i].clone());
                } else {
                    outliers.push(matches[i].clone());
                }
            }
        }
        
        (inliers, outliers)
    }
    
    /// 计算基础矩阵重投影误差
    fn compute_fundamental_reprojection_error(
        &self,
        fundamental_matrix: &Matrix3<f64>,
        points1: &[Vector2<f64>],
        points2: &[Vector2<f64>],
        inlier_mask: &[bool],
    ) -> Result<f64, ColmapError> {
        let mut total_error = 0.0;
        let mut count = 0;
        
        for (i, &is_inlier) in inlier_mask.iter().enumerate() {
            if is_inlier && i < points1.len() && i < points2.len() {
                let p1 = Vector3::new(points1[i].x, points1[i].y, 1.0);
                let p2 = Vector3::new(points2[i].x, points2[i].y, 1.0);
                
                // 计算极线
                let line = fundamental_matrix * p1;
                
                // 点到极线的距离
                let distance = (line.dot(&p2)).abs() / (line.x * line.x + line.y * line.y).sqrt();
                total_error += distance;
                count += 1;
            }
        }
        
        Ok(if count > 0 { total_error / count as f64 } else { 0.0 })
    }
    
    /// 计算本质矩阵重投影误差
    fn compute_essential_reprojection_error(
        &self,
        essential_matrix: &Matrix3<f64>,
        points1: &[Vector2<f64>],
        points2: &[Vector2<f64>],
        inlier_mask: &[bool],
    ) -> Result<f64, ColmapError> {
        // 本质矩阵的重投影误差计算与基础矩阵类似
        self.compute_fundamental_reprojection_error(essential_matrix, points1, points2, inlier_mask)
    }
    
    /// 计算单应性矩阵重投影误差
    fn compute_homography_reprojection_error(
        &self,
        homography: &Matrix3<f64>,
        points1: &[Vector2<f64>],
        points2: &[Vector2<f64>],
        inlier_mask: &[bool],
    ) -> Result<f64, ColmapError> {
        let mut total_error = 0.0;
        let mut count = 0;
        
        for (i, &is_inlier) in inlier_mask.iter().enumerate() {
            if is_inlier && i < points1.len() && i < points2.len() {
                let p1 = Vector3::new(points1[i].x, points1[i].y, 1.0);
                let projected = homography * p1;
                let projected_2d = Vector2::new(
                    projected.x / projected.z,
                    projected.y / projected.z,
                );
                
                let error = (projected_2d - points2[i]).norm();
                total_error += error;
                count += 1;
            }
        }
        
        Ok(if count > 0 { total_error / count as f64 } else { 0.0 })
    }
    
    /// 计算模型质量分数
    fn compute_model_score(&self, result: &GeometricResult) -> f64 {
        // 综合考虑内点比例和重投影误差
        let inlier_score = result.inlier_ratio;
        let error_score = 1.0 / (1.0 + result.reprojection_error);
        
        inlier_score * 0.7 + error_score * 0.3
    }
    
    /// 使用8点算法估计基础矩阵
    fn estimate_fundamental_8point(
        &self,
        indices: &[usize],
        points1: &[Vector2<f64>],
        points2: &[Vector2<f64>],
    ) -> Result<Matrix3<f64>, ColmapError> {
        if indices.len() != 8 {
            return Err(ColmapError::InvalidParameter(
                "Need exactly 8 points for fundamental matrix estimation".to_string()
            ));
        }
        
        // 构建约束矩阵 A
        let mut a = nalgebra::DMatrix::zeros(8, 9);
        
        for (i, &idx) in indices.iter().enumerate() {
            let p1 = &points1[idx];
            let p2 = &points2[idx];
            
            a[(i, 0)] = p2.x * p1.x;
            a[(i, 1)] = p2.x * p1.y;
            a[(i, 2)] = p2.x;
            a[(i, 3)] = p2.y * p1.x;
            a[(i, 4)] = p2.y * p1.y;
            a[(i, 5)] = p2.y;
            a[(i, 6)] = p1.x;
            a[(i, 7)] = p1.y;
            a[(i, 8)] = 1.0;
        }
        
        // SVD 分解求解
        let svd = a.svd(true, true);
        if let Some(v_t) = svd.v_t {
            let f_vec = v_t.row(8);
            let f = Matrix3::new(
                f_vec[0], f_vec[1], f_vec[2],
                f_vec[3], f_vec[4], f_vec[5],
                f_vec[6], f_vec[7], f_vec[8],
            );
            
            // 强制秩为2
            let svd_f = f.svd(true, true);
            let mut s = svd_f.singular_values;
            s[2] = 0.0;
            
            if let (Some(u), Some(v_t)) = (svd_f.u, svd_f.v_t) {
                let s_mat = Matrix3::new(
                    s[0], 0.0, 0.0,
                    0.0, s[1], 0.0,
                    0.0, 0.0, 0.0,
                );
                Ok(u * s_mat * v_t)
            } else {
                Err(ColmapError::FeatureMatching(
                    "SVD decomposition failed".to_string()
                ))
            }
        } else {
            Err(ColmapError::FeatureMatching(
                "SVD decomposition failed".to_string()
            ))
        }
    }
    
    /// 使用5点算法估计本质矩阵
    fn estimate_essential_5point(
        &self,
        indices: &[usize],
        points1: &[Vector2<f64>],
        points2: &[Vector2<f64>],
    ) -> Result<Matrix3<f64>, ColmapError> {
        if indices.len() != 5 {
            return Err(ColmapError::InvalidParameter(
                "Need exactly 5 points for essential matrix estimation".to_string()
            ));
        }
        
        // 简化实现：使用8点算法的前5个点
        let mut a = nalgebra::DMatrix::zeros(5, 9);
        
        for (i, &idx) in indices.iter().enumerate() {
            let p1 = &points1[idx];
            let p2 = &points2[idx];
            
            a[(i, 0)] = p2.x * p1.x;
            a[(i, 1)] = p2.x * p1.y;
            a[(i, 2)] = p2.x;
            a[(i, 3)] = p2.y * p1.x;
            a[(i, 4)] = p2.y * p1.y;
            a[(i, 5)] = p2.y;
            a[(i, 6)] = p1.x;
            a[(i, 7)] = p1.y;
            a[(i, 8)] = 1.0;
        }
        
        // SVD 分解求解
        let svd = a.svd(true, true);
        if let Some(v_t) = svd.v_t {
            let f_vec = v_t.row(8);
            let e = Matrix3::new(
                f_vec[0], f_vec[1], f_vec[2],
                f_vec[3], f_vec[4], f_vec[5],
                f_vec[6], f_vec[7], f_vec[8],
            );
            
            // 强制本质矩阵约束：两个相等的奇异值和一个零奇异值
            let svd_e = e.svd(true, true);
            let s = svd_e.singular_values;
            let sigma = (s[0] + s[1]) / 2.0;
            
            if let (Some(u), Some(v_t)) = (svd_e.u, svd_e.v_t) {
                let s_mat = Matrix3::new(
                    sigma, 0.0, 0.0,
                    0.0, sigma, 0.0,
                    0.0, 0.0, 0.0,
                );
                Ok(u * s_mat * v_t)
            } else {
                Err(ColmapError::FeatureMatching(
                    "SVD decomposition failed".to_string()
                ))
            }
        } else {
            Err(ColmapError::FeatureMatching(
                "SVD decomposition failed".to_string()
            ))
        }
    }
    
    /// 使用4点算法估计单应性矩阵
    fn estimate_homography_4point(
        &self,
        indices: &[usize],
        points1: &[Vector2<f64>],
        points2: &[Vector2<f64>],
    ) -> Result<Matrix3<f64>, ColmapError> {
        if indices.len() != 4 {
            return Err(ColmapError::InvalidParameter(
                "Need exactly 4 points for homography estimation".to_string()
            ));
        }
        
        // 构建约束矩阵 A
        let mut a = nalgebra::DMatrix::zeros(8, 9);
        
        for (i, &idx) in indices.iter().enumerate() {
            let p1 = &points1[idx];
            let p2 = &points2[idx];
            
            let row1 = 2 * i;
            let row2 = 2 * i + 1;
            
            // 第一行约束
            a[(row1, 0)] = p1.x;
            a[(row1, 1)] = p1.y;
            a[(row1, 2)] = 1.0;
            a[(row1, 6)] = -p2.x * p1.x;
            a[(row1, 7)] = -p2.x * p1.y;
            a[(row1, 8)] = -p2.x;
            
            // 第二行约束
            a[(row2, 3)] = p1.x;
            a[(row2, 4)] = p1.y;
            a[(row2, 5)] = 1.0;
            a[(row2, 6)] = -p2.y * p1.x;
            a[(row2, 7)] = -p2.y * p1.y;
            a[(row2, 8)] = -p2.y;
        }
        
        // SVD 分解求解
        let svd = a.svd(true, true);
        if let Some(v_t) = svd.v_t {
            let h_vec = v_t.row(8);
            let h = Matrix3::new(
                h_vec[0], h_vec[1], h_vec[2],
                h_vec[3], h_vec[4], h_vec[5],
                h_vec[6], h_vec[7], h_vec[8],
            );
            Ok(h)
        } else {
            Err(ColmapError::FeatureMatching(
                "SVD decomposition failed".to_string()
            ))
        }
    }
    
    /// 计算基础矩阵的内点
    fn compute_fundamental_inliers(
        &self,
        fundamental_matrix: &Matrix3<f64>,
        points1: &[Vector2<f64>],
        points2: &[Vector2<f64>],
    ) -> Vec<bool> {
        let mut inliers = Vec::new();
        
        for (p1, p2) in points1.iter().zip(points2.iter()) {
            let p1_h = Vector3::new(p1.x, p1.y, 1.0);
            let p2_h = Vector3::new(p2.x, p2.y, 1.0);
            
            // 计算极线
            let line = fundamental_matrix * p1_h;
            
            // 点到极线的距离
            let distance = (line.dot(&p2_h)).abs() / (line.x * line.x + line.y * line.y).sqrt();
            inliers.push(distance < self.config.ransac_threshold);
        }
        
        inliers
    }
    
    /// 计算本质矩阵的内点
    fn compute_essential_inliers(
        &self,
        essential_matrix: &Matrix3<f64>,
        points1: &[Vector2<f64>],
        points2: &[Vector2<f64>],
    ) -> Vec<bool> {
        // 本质矩阵的内点计算与基础矩阵类似
        self.compute_fundamental_inliers(essential_matrix, points1, points2)
    }
    
    /// 计算单应性矩阵的内点
    fn compute_homography_inliers(
        &self,
        homography: &Matrix3<f64>,
        points1: &[Vector2<f64>],
        points2: &[Vector2<f64>],
    ) -> Vec<bool> {
        let mut inliers = Vec::new();
        
        for (p1, p2) in points1.iter().zip(points2.iter()) {
            let p1_h = Vector3::new(p1.x, p1.y, 1.0);
            let projected = homography * p1_h;
            let projected_2d = Vector2::new(
                projected.x / projected.z,
                projected.y / projected.z,
            );
            
            let error = (projected_2d - p2).norm();
            inliers.push(error < self.config.ransac_threshold);
        }
        
        inliers
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::core::Feature;
    
    fn create_test_features() -> (Vec<Feature>, Vec<Feature>) {
        use nalgebra::Point2;
        let features1 = vec![
            Feature::new(Point2::new(0.0, 0.0), vec![0; 32], 1.0, 0.0, 1.0, 0),
            Feature::new(Point2::new(100.0, 0.0), vec![1; 32], 1.0, 0.0, 1.0, 0),
            Feature::new(Point2::new(100.0, 100.0), vec![2; 32], 1.0, 0.0, 1.0, 0),
            Feature::new(Point2::new(0.0, 100.0), vec![3; 32], 1.0, 0.0, 1.0, 0),
        ];
        
        let features2 = vec![
            Feature::new(Point2::new(10.0, 10.0), vec![4; 32], 1.0, 0.0, 1.0, 0),
            Feature::new(Point2::new(110.0, 10.0), vec![5; 32], 1.0, 0.0, 1.0, 0),
            Feature::new(Point2::new(110.0, 110.0), vec![6; 32], 1.0, 0.0, 1.0, 0),
            Feature::new(Point2::new(10.0, 110.0), vec![7; 32], 1.0, 0.0, 1.0, 0),
        ];
        
        (features1, features2)
    }
    
    fn create_test_matches() -> Vec<FeatureMatch> {
        vec![
            FeatureMatch::new(0, 0, 0.1),
            FeatureMatch::new(1, 1, 0.1),
            FeatureMatch::new(2, 2, 0.1),
            FeatureMatch::new(3, 3, 0.1),
        ]
    }
    
    #[test]
    fn test_geometric_config_default() {
        let config = GeometricConfig::default();
        assert_eq!(config.ransac_iterations, 1000);
        assert_eq!(config.ransac_threshold, 1.0);
        assert_eq!(config.min_inliers, 8);
        assert_eq!(config.confidence, 0.99);
        assert!(config.use_normalized_coordinates);
    }
    
    #[test]
    fn test_geometric_verifier_creation() {
        let config = GeometricConfig::default();
        let verifier = GeometricVerifier::new(config);
        assert_eq!(verifier.config.ransac_iterations, 1000);
    }
    
    #[test]
    fn test_extract_matched_points() {
        let config = GeometricConfig::default();
        let verifier = GeometricVerifier::new(config);
        
        let (features1, features2) = create_test_features();
        let matches = create_test_matches();
        
        let result = verifier.extract_matched_points(&features1, &features2, &matches);
        assert!(result.is_ok());
        
        let (points1, points2) = result.unwrap();
        assert_eq!(points1.len(), 4);
        assert_eq!(points2.len(), 4);
        assert_eq!(points1[0], Vector2::new(0.0, 0.0));
        assert_eq!(points2[0], Vector2::new(10.0, 10.0));
    }
    
    #[test]
    fn test_separate_inliers_outliers() {
        let config = GeometricConfig::default();
        let verifier = GeometricVerifier::new(config);
        
        let matches = create_test_matches();
        let inlier_mask = vec![true, false, true, false];
        
        let (inliers, outliers) = verifier.separate_inliers_outliers(&matches, &inlier_mask);
        assert_eq!(inliers.len(), 2);
        assert_eq!(outliers.len(), 2);
        assert_eq!(inliers[0].query_idx, 0);
        assert_eq!(inliers[1].query_idx, 2);
        assert_eq!(outliers[0].query_idx, 1);
        assert_eq!(outliers[1].query_idx, 3);
    }
    
    #[test]
    fn test_normalize_points() {
        let config = GeometricConfig::default();
        let verifier = GeometricVerifier::new(config);
        
        let points1 = vec![Vector2::new(320.0, 240.0)];
        let points2 = vec![Vector2::new(330.0, 250.0)];
        
        // 简单的相机矩阵
        let camera_matrix = Matrix3::new(
            500.0, 0.0, 320.0,
            0.0, 500.0, 240.0,
            0.0, 0.0, 1.0,
        );
        
        let result = verifier.normalize_points(&points1, &points2, &camera_matrix);
        assert!(result.is_ok());
        
        let (norm_points1, norm_points2) = result.unwrap();
        assert_eq!(norm_points1.len(), 1);
        assert_eq!(norm_points2.len(), 1);
        
        // 归一化后的点应该接近原点
        assert!((norm_points1[0].x - 0.0).abs() < 0.1);
        assert!((norm_points1[0].y - 0.0).abs() < 0.1);
    }
}