colmap 0.1.2

//! 增量式 SfM (Structure from Motion) 重建实现

use crate::core::{
    Camera, FeatureMatch, Image, CameraPose, Result, ColmapError, Point2, Point3, Matrix3, Vector3
};
use crate::core::point3d::Point3d;
use std::collections::{HashMap, HashSet};
use nalgebra::Matrix3x4;

/// 增量式 SfM 重建器
#[derive(Debug)]
pub struct IncrementalReconstructor {
    /// 相机参数
    cameras: HashMap<u32, Camera>,
    /// 图像数据
    images: HashMap<u32, Image>,
    /// 3D 点云
    points3d: HashMap<u32, Point3d>,
    /// 已注册的图像
    registered_images: HashSet<u32>,
    /// 特征匹配
    matches: HashMap<(u32, u32), Vec<FeatureMatch>>,
    /// 重建参数
    config: ReconstructionConfig,
}

/// 重建配置参数
#[derive(Debug, Clone)]
pub struct ReconstructionConfig {
    /// 最小三角化角度（弧度）
    pub min_triangulation_angle: f64,
    /// 最大重投影误差
    pub max_reprojection_error: f64,
    /// 最小轨迹长度
    pub min_track_length: usize,
    /// BA 优化的最大迭代次数
    pub max_ba_iterations: usize,
    /// 相机姿态估计的 RANSAC 阈值
    pub ransac_threshold: f64,
    /// RANSAC 最大迭代次数
    pub max_ransac_iterations: usize,
}

impl Default for ReconstructionConfig {
    fn default() -> Self {
        Self {
            min_triangulation_angle: 1.5_f64.to_radians(),
            max_reprojection_error: 4.0,
            min_track_length: 2,
            max_ba_iterations: 100,
            ransac_threshold: 4.0,
            max_ransac_iterations: 1000,
        }
    }
}

/// 图像注册结果
#[derive(Debug)]
pub struct RegistrationResult {
    /// 是否成功注册
    pub success: bool,
    /// 新三角化的点数量
    pub num_triangulated_points: usize,
    /// 重投影误差
    pub reprojection_error: f64,
}

impl IncrementalReconstructor {
    /// 创建新的增量重建器
    pub fn new(config: ReconstructionConfig) -> Self {
        Self {
            cameras: HashMap::new(),
            images: HashMap::new(),
            points3d: HashMap::new(),
            registered_images: HashSet::new(),
            matches: HashMap::new(),
            config,
        }
    }

    /// 添加相机
    pub fn add_camera(&mut self, camera_id: u32, camera: Camera) {
        self.cameras.insert(camera_id, camera);
    }

    /// 添加图像
    pub fn add_image(&mut self, image_id: u32, image: Image) {
        self.images.insert(image_id, image);
    }

    /// 添加特征匹配
    pub fn add_matches(&mut self, image1_id: u32, image2_id: u32, matches: Vec<FeatureMatch>) {
        let key = if image1_id < image2_id {
            (image1_id, image2_id)
        } else {
            (image2_id, image1_id)
        };
        self.matches.insert(key, matches);
    }

    /// 开始增量重建
    pub fn reconstruct(&mut self) -> Result<()> {
        // 1. 选择初始图像对
        let (image1_id, image2_id) = self.select_initial_pair()?;
        
        // 2. 初始化重建
        self.initialize_reconstruction(image1_id, image2_id)?;
        
        // 3. 增量添加图像
        while self.registered_images.len() < self.images.len() {
            let next_image_id = self.select_next_image()?;
            
            match self.register_image(next_image_id) {
                Ok(result) => {
                    if result.success {
                        println!("成功注册图像 {}, 新增 {} 个3D点", 
                                next_image_id, result.num_triangulated_points);
                        
                        // 局部束调整优化
                        self.local_bundle_adjustment(next_image_id)?;
                    } else {
                        println!("图像 {} 注册失败", next_image_id);
                        break;
                    }
                },
                Err(e) => {
                    println!("图像 {} 注册出错: {:?}", next_image_id, e);
                    break;
                }
            }
        }
        
        // 4. 全局束调整优化
        self.global_bundle_adjustment()?;
        
        Ok(())
    }

    /// 选择初始图像对
    fn select_initial_pair(&self) -> Result<(u32, u32)> {
        let mut best_pair = None;
        let mut max_matches = 0;
        
        for (&(image1_id, image2_id), matches) in &self.matches {
            if matches.len() > max_matches {
                max_matches = matches.len();
                best_pair = Some((image1_id, image2_id));
            }
        }
        
        best_pair.ok_or_else(|| ColmapError::SfmReconstruction(
            "无法找到合适的初始图像对".to_string()
        ))
    }

    /// 初始化重建
    fn initialize_reconstruction(&mut self, image1_id: u32, image2_id: u32) -> Result<()> {
        // 获取匹配点
        let matches = self.get_matches(image1_id, image2_id)?;
        let matches_clone = matches.clone();
        
        // 估计基础矩阵
        let fundamental_matrix = self.estimate_fundamental_matrix(matches)?;
        
        // 从基础矩阵恢复相机姿态
        let (pose1, pose2) = self.recover_pose_from_fundamental(
            &fundamental_matrix, matches, image1_id, image2_id
        )?;
        
        // 设置第一个相机为世界坐标系原点
        if let Some(image1) = self.images.get_mut(&image1_id) {
            image1.pose = Some(pose1);
        }
        
        // 设置第二个相机姿态
        if let Some(image2) = self.images.get_mut(&image2_id) {
            image2.pose = Some(pose2);
        }
        
        // 标记图像为已注册
        self.registered_images.insert(image1_id);
        self.registered_images.insert(image2_id);
        
        // 三角化初始点
        self.triangulate_initial_points(image1_id, image2_id, &matches_clone)?;
        
        Ok(())
    }

    /// 选择下一个要注册的图像
    fn select_next_image(&self) -> Result<u32> {
        let mut best_image = None;
        let mut max_observations = 0;
        
        for &image_id in self.images.keys() {
            if self.registered_images.contains(&image_id) {
                continue;
            }
            
            let observations = self.count_3d_observations(image_id);
            if observations > max_observations {
                max_observations = observations;
                best_image = Some(image_id);
            }
        }
        
        best_image.ok_or_else(|| ColmapError::SfmReconstruction(
            "无法找到下一个合适的图像".to_string()
        ))
    }

    /// 注册图像
    fn register_image(&mut self, image_id: u32) -> Result<RegistrationResult> {
        // 收集2D-3D对应关系
        let correspondences = self.collect_2d_3d_correspondences(image_id)?;
        
        if correspondences.len() < 6 {
            return Ok(RegistrationResult {
                success: false,
                num_triangulated_points: 0,
                reprojection_error: f64::INFINITY,
            });
        }
        
        // 使用PnP估计相机姿态
        let pose = self.estimate_pose_pnp(image_id, &correspondences)?;
        
        // 设置图像姿态
        if let Some(image) = self.images.get_mut(&image_id) {
            image.pose = Some(pose);
        }
        
        // 标记为已注册
        self.registered_images.insert(image_id);
        
        // 三角化新的3D点
        let num_triangulated = self.triangulate_new_points(image_id)?;
        
        // 计算重投影误差
        let reprojection_error = self.compute_reprojection_error(image_id)?;
        
        Ok(RegistrationResult {
            success: true,
            num_triangulated_points: num_triangulated,
            reprojection_error,
        })
    }

    /// 获取图像间的匹配
    fn get_matches(&self, image1_id: u32, image2_id: u32) -> Result<&Vec<FeatureMatch>> {
        let key = if image1_id < image2_id {
            (image1_id, image2_id)
        } else {
            (image2_id, image1_id)
        };
        
        self.matches.get(&key).ok_or_else(|| ColmapError::SfmReconstruction(
            format!("未找到图像 {} 和 {} 之间的匹配", image1_id, image2_id)
        ))
    }

    /// 估计基础矩阵
    fn estimate_fundamental_matrix(&self, matches: &[FeatureMatch]) -> Result<Matrix3> {
        if matches.len() < 8 {
            return Err(ColmapError::SfmReconstruction(
                "需要至少8个匹配点来估计基础矩阵".to_string()
            ));
        }

        // 使用8点算法估计基础矩阵
        let mut a_matrix = nalgebra::DMatrix::zeros(matches.len(), 9);
        
        for (i, m) in matches.iter().enumerate() {
            // 获取特征点坐标
            let image1 = self.images.get(&m.image1_id).unwrap();
            let image2 = self.images.get(&m.image2_id).unwrap();
            let point1 = image1.features[m.feature1_idx].point;
            let point2 = image2.features[m.feature2_idx].point;
            
            let x1 = point1.x;
            let y1 = point1.y;
            let x2 = point2.x;
            let y2 = point2.y;
            
            a_matrix[(i, 0)] = x2 * x1;
            a_matrix[(i, 1)] = x2 * y1;
            a_matrix[(i, 2)] = x2;
            a_matrix[(i, 3)] = y2 * x1;
            a_matrix[(i, 4)] = y2 * y1;
            a_matrix[(i, 5)] = y2;
            a_matrix[(i, 6)] = x1;
            a_matrix[(i, 7)] = y1;
            a_matrix[(i, 8)] = 1.0;
        }
        
        // SVD分解求解最小二乘解
        let svd = a_matrix.svd(true, true);
        if let Some(v) = svd.v_t {
            let f_vec = v.row(8);
            let fundamental = 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 f_svd = fundamental.svd(true, true);
            let mut s = f_svd.singular_values;
            s[2] = 0.0; // 设置最小奇异值为0
            
            if let (Some(u), Some(vt)) = (f_svd.u, f_svd.v_t) {
                let s_matrix = Matrix3::from_diagonal(&s);
                Ok(u * s_matrix * vt)
            } else {
                Ok(fundamental)
            }
        } else {
            Err(ColmapError::SfmReconstruction(
                "SVD分解失败".to_string()
            ))
        }
    }

    /// 从基础矩阵恢复姿态
    fn recover_pose_from_fundamental(
        &self,
        fundamental_matrix: &Matrix3,
        matches: &[FeatureMatch],
        image1_id: u32,
        image2_id: u32,
    ) -> Result<(CameraPose, CameraPose)> {
        // 获取相机内参
        let camera1 = self.images.get(&image1_id)
            .and_then(|img| self.cameras.get(&img.camera_id))
            .ok_or_else(|| ColmapError::SfmReconstruction(
                format!("未找到图像 {} 的相机", image1_id)
            ))?;
            
        let camera2 = self.images.get(&image2_id)
            .and_then(|img| self.cameras.get(&img.camera_id))
            .ok_or_else(|| ColmapError::SfmReconstruction(
                format!("未找到图像 {} 的相机", image2_id)
            ))?;
        
        // 构建相机内参矩阵
        let k1 = Matrix3::new(
            camera1.intrinsics.focal_length.0, 0.0, camera1.intrinsics.principal_point.0,
            0.0, camera1.intrinsics.focal_length.1, camera1.intrinsics.principal_point.1,
            0.0, 0.0, 1.0
        );
        
        let k2 = Matrix3::new(
            camera2.intrinsics.focal_length.0, 0.0, camera2.intrinsics.principal_point.0,
            0.0, camera2.intrinsics.focal_length.1, camera2.intrinsics.principal_point.1,
            0.0, 0.0, 1.0
        );
        
        // 计算本质矩阵 E = K2^T * F * K1
        let essential = k2.transpose() * fundamental_matrix * k1;
        
        // 从本质矩阵分解出旋转和平移
        let svd = essential.svd(true, true);
        
        if let (Some(u), Some(vt)) = (svd.u, svd.v_t) {
            // 构建旋转矩阵的候选
            let w = Matrix3::new(
                0.0, -1.0, 0.0,
                1.0, 0.0, 0.0,
                0.0, 0.0, 1.0
            );
            
            let r1 = u * w * vt;
            let r2 = u * w.transpose() * vt;
            let t = u.column(2).into_owned();
            
            // 确保旋转矩阵的行列式为正
            let r1 = if r1.determinant() < 0.0 { -r1 } else { r1 };
            let r2 = if r2.determinant() < 0.0 { -r2 } else { r2 };
            
            // 四种可能的姿态组合
            let poses = [
                (r1, t),
                (r1, -t),
                (r2, t),
                (r2, -t),
            ];
            
            // 选择正确的姿态（通过三角化测试）
            for (r, t_vec) in poses.iter() {
                let pose1 = CameraPose::identity();
                let rotation_quat = nalgebra::UnitQuaternion::from_rotation_matrix(&nalgebra::Rotation3::from_matrix_unchecked(*r));
                let pose2 = CameraPose::new(rotation_quat, *t_vec);
                
                // 简单验证：检查是否有足够的点在两个相机前方
                let mut valid_points = 0;
                for m in matches.iter().take(10) { // 只检查前10个点
                    if self.is_point_in_front_of_cameras(&pose1, &pose2, m, &k1, &k2) {
                        valid_points += 1;
                    }
                }
                
                if valid_points > matches.len().min(10) / 2 {
                    return Ok((pose1, pose2));
                }
            }
            
            // 如果没有找到合适的姿态，返回第一个
            let pose1 = CameraPose::identity();
            let rotation_quat = nalgebra::UnitQuaternion::from_rotation_matrix(&nalgebra::Rotation3::from_matrix_unchecked(poses[0].0));
            let pose2 = CameraPose::new(rotation_quat, poses[0].1);
            Ok((pose1, pose2))
        } else {
            Err(ColmapError::SfmReconstruction(
                "本质矩阵SVD分解失败".to_string()
            ))
        }
    }

    /// 检查点是否在两个相机前方
    fn is_point_in_front_of_cameras(
        &self,
        pose1: &CameraPose,
        pose2: &CameraPose,
        m: &FeatureMatch,
        k1: &Matrix3,
        k2: &Matrix3,
    ) -> bool {
        // 简单的三角化测试
        // 获取特征点坐标
         let image1 = self.images.get(&m.image1_id).unwrap();
         let image2 = self.images.get(&m.image2_id).unwrap();
         let point1 = image1.features[m.feature1_idx].point;
         let point2 = image2.features[m.feature2_idx].point;
        
        let p1 = Vector3::new(point1.x, point1.y, 1.0);
        let p2 = Vector3::new(point2.x, point2.y, 1.0);
        
        // 归一化坐标
        let p1_norm = k1.try_inverse().unwrap_or(Matrix3::identity()) * p1;
        let p2_norm = k2.try_inverse().unwrap_or(Matrix3::identity()) * p2;
        
        // 简单的深度检查（这里使用简化的方法）
        let depth1 = p1_norm.z;
        let depth2 = p2_norm.z;
        
        depth1 > 0.0 && depth2 > 0.0
    }

    /// 三角化初始点
    fn triangulate_initial_points(
        &mut self,
        image1_id: u32,
        image2_id: u32,
        matches: &[FeatureMatch],
    ) -> Result<()> {
        let pose1 = self.images.get(&image1_id)
            .and_then(|img| img.pose.as_ref())
            .ok_or_else(|| ColmapError::SfmReconstruction(
                format!("图像 {} 没有姿态信息", image1_id)
            ))?;
            
        let pose2 = self.images.get(&image2_id)
            .and_then(|img| img.pose.as_ref())
            .ok_or_else(|| ColmapError::SfmReconstruction(
                format!("图像 {} 没有姿态信息", image2_id)
            ))?;
        
        let camera1 = self.images.get(&image1_id)
            .and_then(|img| self.cameras.get(&img.camera_id))
            .ok_or_else(|| ColmapError::SfmReconstruction(
                format!("未找到图像 {} 的相机", image1_id)
            ))?;
            
        let camera2 = self.images.get(&image2_id)
            .and_then(|img| self.cameras.get(&img.camera_id))
            .ok_or_else(|| ColmapError::SfmReconstruction(
                format!("未找到图像 {} 的相机", image2_id)
            ))?;
        
        // 构建投影矩阵
         let k1 = Matrix3::new(
             camera1.intrinsics.focal_length.0, 0.0, camera1.intrinsics.principal_point.0,
             0.0, camera1.intrinsics.focal_length.1, camera1.intrinsics.principal_point.1,
             0.0, 0.0, 1.0
         );
         
         let k2 = Matrix3::new(
             camera2.intrinsics.focal_length.0, 0.0, camera2.intrinsics.principal_point.0,
             0.0, camera2.intrinsics.focal_length.1, camera2.intrinsics.principal_point.1,
             0.0, 0.0, 1.0
         );
        
        let mut triangulated_count = 0;
        
        for (point_id, m) in matches.iter().enumerate() {
            // 三角化3D点
            if let Ok(point3d) = self.triangulate_point(pose1, pose2, &k1, &k2, m) {
                // 检查三角化角度
                let angle = self.compute_triangulation_angle(pose1, pose2, &point3d);
                if angle > self.config.min_triangulation_angle {
                    // 创建3D点
                    let point3d_obj = Point3d::new(
                        point_id as u64,
                        point3d,
                    );
                    
                    self.points3d.insert(point_id as u32, point3d_obj);
                    triangulated_count += 1;
                }
            }
        }
        
        println!("三角化了 {} 个3D点", triangulated_count);
        Ok(())
    }

    /// 三角化单个点
    fn triangulate_point(
        &self,
        pose1: &CameraPose,
        pose2: &CameraPose,
        k1: &Matrix3,
        k2: &Matrix3,
        m: &FeatureMatch,
    ) -> Result<Point3> {
        // 构建投影矩阵 P = K[R|t]
        let r1 = pose1.rotation.to_rotation_matrix();
        let r2 = pose2.rotation.to_rotation_matrix();
        
        let rt1 = Matrix3x4::from_columns(&[
             r1.matrix().column(0),
             r1.matrix().column(1),
             r1.matrix().column(2),
             pose1.translation.as_view(),
         ]);
         let p1 = k1 * rt1;
         
         let rt2 = Matrix3x4::from_columns(&[
             r2.matrix().column(0),
             r2.matrix().column(1),
             r2.matrix().column(2),
             pose2.translation.as_view(),
         ]);
         let p2 = k2 * rt2;
        
        // 使用DLT算法三角化
        // 获取特征点坐标
        let image1 = self.images.get(&m.image1_id).unwrap();
        let image2 = self.images.get(&m.image2_id).unwrap();
        let point1 = image1.features[m.feature1_idx].point;
        let point2 = image2.features[m.feature2_idx].point;
        
        let x1 = point1.x;
        let y1 = point1.y;
        let x2 = point2.x;
        let y2 = point2.y;
        
        let mut a = nalgebra::Matrix4::zeros();
        
        // 第一个相机的约束
        a.set_row(0, &(x1 * p1.row(2) - p1.row(0)));
        a.set_row(1, &(y1 * p1.row(2) - p1.row(1)));
        
        // 第二个相机的约束
        a.set_row(2, &(x2 * p2.row(2) - p2.row(0)));
        a.set_row(3, &(y2 * p2.row(2) - p2.row(1)));
        
        // SVD求解
        let svd = a.svd(true, true);
        if let Some(v) = svd.v_t {
            let solution = v.row(3);
            if solution[3].abs() > 1e-10 {
                let point3d = Point3::new(
                    solution[0] / solution[3],
                    solution[1] / solution[3],
                    solution[2] / solution[3],
                );
                Ok(point3d)
            } else {
                Err(ColmapError::SfmReconstruction(
                    "三角化失败：齐次坐标w接近0".to_string()
                ))
            }
        } else {
            Err(ColmapError::SfmReconstruction(
                "三角化SVD分解失败".to_string()
            ))
        }
    }

    /// 计算三角化角度
    fn compute_triangulation_angle(
        &self,
        pose1: &CameraPose,
        pose2: &CameraPose,
        point3d: &Point3,
    ) -> f64 {
        let center1 = -(pose1.rotation.to_rotation_matrix().transpose() * pose1.translation);
        let center2 = -(pose2.rotation.to_rotation_matrix().transpose() * pose2.translation);
        
        let point_vec = Vector3::new(point3d.x, point3d.y, point3d.z);
        
        let ray1 = (point_vec - center1).normalize();
        let ray2 = (point_vec - center2).normalize();
        
        let cos_angle = ray1.dot(&ray2).clamp(-1.0, 1.0);
        cos_angle.acos()
    }

    /// 统计图像观测到的3D点数量
    fn count_3d_observations(&self, image_id: u32) -> usize {
        let mut count = 0;
        
        // 遍历所有已注册图像，统计与当前图像的匹配中有多少对应到3D点
        for &registered_id in &self.registered_images {
            if let Ok(matches) = self.get_matches(image_id, registered_id) {
                for m in matches {
                    // 检查匹配点是否对应到已存在的3D点
                    // 这里简化处理，实际需要维护特征点到3D点的映射
                    if self.points3d.contains_key(&(m.feature1_idx as u32)) {
                        count += 1;
                    }
                }
            }
        }
        
        count
    }

    /// 收集2D-3D对应关系
    fn collect_2d_3d_correspondences(&self, image_id: u32) -> Result<Vec<(Point2, Point3)>> {
        let mut correspondences = Vec::new();
        
        // 获取图像的特征点
        let image = self.images.get(&image_id)
            .ok_or_else(|| ColmapError::SfmReconstruction(
                format!("未找到图像 {}", image_id)
            ))?;
        
        // 遍历所有已注册图像，收集2D-3D对应关系
        for &registered_id in &self.registered_images {
            if let Ok(matches) = self.get_matches(image_id, registered_id) {
                for m in matches {
                    // 检查匹配点是否对应到已存在的3D点
                    if let Some(point3d_obj) = self.points3d.get(&(m.feature1_idx as u32)) {
                        // 获取特征点坐标
                        let image = self.images.get(&image_id).unwrap();
                        let point = image.features[m.feature1_idx].point;
                        
                        let point2d = Point2::new(point.x, point.y);
                        let point3d = Point3::new(
                            point3d_obj.position.x,
                            point3d_obj.position.y,
                            point3d_obj.position.z,
                        );
                        correspondences.push((point2d, point3d));
                    }
                }
            }
        }
        
        Ok(correspondences)
    }

    /// 使用PnP估计姿态
    fn estimate_pose_pnp(&self, image_id: u32, correspondences: &[(Point2, Point3)]) -> Result<CameraPose> {
        if correspondences.len() < 6 {
            return Err(ColmapError::SfmReconstruction(
                "PnP需要至少6个对应点".to_string()
            ));
        }
        
        // 获取相机内参
        let camera = self.images.get(&image_id)
            .and_then(|img| self.cameras.get(&img.camera_id))
            .ok_or_else(|| ColmapError::SfmReconstruction(
                format!("未找到图像 {} 的相机", image_id)
            ))?;
        
        // 构建相机内参矩阵
        let k = Matrix3::new(
            camera.intrinsics.focal_length.0, 0.0, camera.intrinsics.principal_point.0,
            0.0, camera.intrinsics.focal_length.1, camera.intrinsics.principal_point.1,
            0.0, 0.0, 1.0
        );
        
        // 使用DLT算法求解PnP（简化实现）
        // 实际应该使用更鲁棒的算法如EPnP或P3P+RANSAC
        
        let mut a_matrix = nalgebra::DMatrix::zeros(correspondences.len() * 2, 12);
        
        for (i, (p2d, p3d)) in correspondences.iter().enumerate() {
            let x = p2d.x;
            let y = p2d.y;
            let x_coord = p3d.x;
            let y_coord = p3d.y;
            let z_coord = p3d.z;
            
            // 第一行约束
            let row1 = 2 * i;
            a_matrix[(row1, 0)] = x_coord;
            a_matrix[(row1, 1)] = y_coord;
            a_matrix[(row1, 2)] = z_coord;
            a_matrix[(row1, 3)] = 1.0;
            a_matrix[(row1, 8)] = -x * x_coord;
            a_matrix[(row1, 9)] = -x * y_coord;
            a_matrix[(row1, 10)] = -x * z_coord;
            a_matrix[(row1, 11)] = -x;
            
            // 第二行约束
            let row2 = 2 * i + 1;
            a_matrix[(row2, 4)] = x_coord;
            a_matrix[(row2, 5)] = y_coord;
            a_matrix[(row2, 6)] = z_coord;
            a_matrix[(row2, 7)] = 1.0;
            a_matrix[(row2, 8)] = -y * x_coord;
            a_matrix[(row2, 9)] = -y * y_coord;
            a_matrix[(row2, 10)] = -y * z_coord;
            a_matrix[(row2, 11)] = -y;
        }
        
        // SVD求解
        let svd = a_matrix.svd(true, true);
        if let Some(v) = svd.v_t {
            let solution = v.row(11);
            
            // 重构投影矩阵
            let p_matrix = Matrix3x4::new(
                solution[0], solution[1], solution[2], solution[3],
                solution[4], solution[5], solution[6], solution[7],
                solution[8], solution[9], solution[10], solution[11]
            );
            
            // 从投影矩阵分解出R和t
            // P = K[R|t], 所以 [R|t] = K^(-1) * P
            if let Some(k_inv) = k.try_inverse() {
                let rt = k_inv * p_matrix;
                
                // 提取旋转矩阵和平移向量
                let r_candidate = rt.fixed_view::<3, 3>(0, 0).into_owned();
                let t = rt.column(3).into_owned();
                
                // 确保R是正交矩阵（通过SVD正交化）
                let r_svd = r_candidate.svd(true, true);
                if let (Some(u), Some(vt)) = (r_svd.u, r_svd.v_t) {
                    let r = u * vt;
                    // 确保行列式为正
                    let r = if r.determinant() < 0.0 { -r } else { r };
                    
                    let rotation_quat = nalgebra::UnitQuaternion::from_rotation_matrix(&nalgebra::Rotation3::from_matrix_unchecked(r));
                    Ok(CameraPose::new(rotation_quat, t))
                } else {
                    Err(ColmapError::SfmReconstruction(
                        "旋转矩阵SVD分解失败".to_string()
                    ))
                }
            } else {
                Err(ColmapError::SfmReconstruction(
                    "相机内参矩阵不可逆".to_string()
                ))
            }
        } else {
            Err(ColmapError::SfmReconstruction(
                "PnP SVD分解失败".to_string()
            ))
        }
    }

    /// 三角化新的3D点
    fn triangulate_new_points(&mut self, image_id: u32) -> Result<usize> {
        let mut triangulated_count = 0;
        
        // 获取新注册图像的姿态
        let new_pose = self.images.get(&image_id)
            .and_then(|img| img.pose.as_ref())
            .ok_or_else(|| ColmapError::SfmReconstruction(
                format!("图像 {} 没有姿态信息", image_id)
            ))?.clone();
        
        let new_camera = self.images.get(&image_id)
            .and_then(|img| self.cameras.get(&img.camera_id))
            .ok_or_else(|| ColmapError::SfmReconstruction(
                format!("未找到图像 {} 的相机", image_id)
            ))?;
        
        let k_new = Matrix3::new(
            new_camera.intrinsics.focal_length.0, 0.0, new_camera.intrinsics.principal_point.0,
            0.0, new_camera.intrinsics.focal_length.1, new_camera.intrinsics.principal_point.1,
            0.0, 0.0, 1.0
        );
        
        // 与所有已注册图像进行三角化
        let mut new_points = Vec::new();
        
        for &registered_id in &self.registered_images {
            if registered_id == image_id {
                continue;
            }
            
            let registered_pose = self.images.get(&registered_id)
                .and_then(|img| img.pose.as_ref())
                .ok_or_else(|| ColmapError::SfmReconstruction(
                    format!("图像 {} 没有姿态信息", registered_id)
                ))?;
            
            let registered_camera = self.images.get(&registered_id)
                .and_then(|img| self.cameras.get(&img.camera_id))
                .ok_or_else(|| ColmapError::SfmReconstruction(
                    format!("未找到图像 {} 的相机", registered_id)
                ))?;
            
            let k_registered = Matrix3::new(
                registered_camera.intrinsics.focal_length.0, 0.0, registered_camera.intrinsics.principal_point.0,
                0.0, registered_camera.intrinsics.focal_length.1, registered_camera.intrinsics.principal_point.1,
                0.0, 0.0, 1.0
            );
            
            // 获取匹配点
            if let Ok(matches) = self.get_matches(image_id, registered_id) {
                let matches_clone = matches.clone();
                for (match_id, m) in matches_clone.iter().enumerate() {
                    // 检查是否已经有对应的3D点
                    let point_id = image_id * 10000 + registered_id * 100 + match_id as u32;
                    if self.points3d.contains_key(&point_id) {
                        continue;
                    }
                    
                    // 三角化新的3D点
                    if let Ok(point3d) = self.triangulate_point(&new_pose, registered_pose, &k_new, &k_registered, m) {
                        // 检查三角化角度
                        let angle = self.compute_triangulation_angle(&new_pose, registered_pose, &point3d);
                        if angle > self.config.min_triangulation_angle {
                            // 获取特征点坐标
                              let image1 = self.images.get(&image_id).unwrap();
                              let image2 = self.images.get(&registered_id).unwrap();
                              let point1 = image1.features[m.feature1_idx].point;
                              let point2 = image2.features[m.feature2_idx].point;
                            
                            // 计算重投影误差
                            let error1 = self.compute_point_reprojection_error(
                                &new_pose, &k_new, &point3d, &Point2::new(point1.x, point1.y));
                            let error2 = self.compute_point_reprojection_error(
                                registered_pose, &k_registered, &point3d, &Point2::new(point2.x, point2.y));
                             
                             if error1 < self.config.max_reprojection_error && error2 < self.config.max_reprojection_error {
                                // 创建3D点
                                let point3d_obj = Point3d::new(
                                    point_id as u64,
                                    point3d,
                                );
                                
                                new_points.push((point_id, point3d_obj));
                                triangulated_count += 1;
                            }
                        }
                    }
                }
            }
        }
        
        // 插入新的3D点
        for (point_id, point3d_obj) in new_points {
            self.points3d.insert(point_id, point3d_obj);
        }
        
        Ok(triangulated_count)
    }

    /// 计算单个点的重投影误差
    fn compute_point_reprojection_error(
        &self,
        pose: &CameraPose,
        k: &Matrix3,
        point3d: &Point3,
        observed_point: &Point2,
    ) -> f64 {
        // 将3D点投影到图像平面
        let point3d_vec = Vector3::new(point3d.x, point3d.y, point3d.z);
        let camera_point = pose.rotation * point3d_vec + pose.translation;
        
        if camera_point.z <= 0.0 {
            return f64::INFINITY; // 点在相机后方
        }
        
        let projected_homogeneous = k * camera_point;
        let projected = Point2::new(
            projected_homogeneous.x / projected_homogeneous.z,
            projected_homogeneous.y / projected_homogeneous.z,
        );
        
        // 计算欧几里得距离
        let dx = projected.x - observed_point.x;
        let dy = projected.y - observed_point.y;
        (dx * dx + dy * dy).sqrt()
    }

    /// 计算重投影误差
    fn compute_reprojection_error(&self, image_id: u32) -> Result<f64> {
        let pose = self.images.get(&image_id)
            .and_then(|img| img.pose.as_ref())
            .ok_or_else(|| ColmapError::SfmReconstruction(
                format!("图像 {} 没有姿态信息", image_id)
            ))?;
        
        let camera = self.images.get(&image_id)
            .and_then(|img| self.cameras.get(&img.camera_id))
            .ok_or_else(|| ColmapError::SfmReconstruction(
                format!("未找到图像 {} 的相机", image_id)
            ))?;
        
        let k = Matrix3::new(
            camera.intrinsics.focal_length.0, 0.0, camera.intrinsics.principal_point.0,
            0.0, camera.intrinsics.focal_length.1, camera.intrinsics.principal_point.1,
            0.0, 0.0, 1.0
        );
        
        let mut total_error = 0.0;
        let mut count = 0;
        
        // 收集2D-3D对应关系并计算重投影误差
        if let Ok(correspondences) = self.collect_2d_3d_correspondences(image_id) {
            for (point2d, point3d) in correspondences {
                let error = self.compute_point_reprojection_error(pose, &k, &point3d, &point2d);
                if error.is_finite() {
                    total_error += error;
                    count += 1;
                }
            }
        }
        
        if count > 0 {
            Ok(total_error / count as f64)
        } else {
            Ok(0.0)
        }
    }

    /// 局部束调整优化
    fn local_bundle_adjustment(&mut self, image_id: u32) -> Result<()> {
        // 简化的束调整实现
        // 实际应该使用Levenberg-Marquardt算法优化相机姿态和3D点位置
        
        println!("对图像 {} 执行局部束调整优化", image_id);
        
        // 收集需要优化的图像（当前图像及其邻近图像）
        let mut images_to_optimize = vec![image_id];
        
        // 添加与当前图像有匹配的已注册图像
        for &registered_id in &self.registered_images {
            if registered_id != image_id
                && self.get_matches(image_id, registered_id).is_ok() {
                    images_to_optimize.push(registered_id);
                    if images_to_optimize.len() >= 10 { // 限制优化图像数量
                        break;
                    }
                }
        }
        
        // 简化的优化过程：重新计算重投影误差并报告
        let mut total_error = 0.0;
        let mut count = 0;
        
        for &img_id in &images_to_optimize {
            if let Ok(error) = self.compute_reprojection_error(img_id) {
                total_error += error;
                count += 1;
            }
        }
        
        if count > 0 {
            let mean_error = total_error / count as f64;
            println!("局部束调整后平均重投影误差: {:.3} 像素", mean_error);
        }
        
        Ok(())
    }

    /// 全局束调整优化
    fn global_bundle_adjustment(&mut self) -> Result<()> {
        // 简化的全局束调整实现
        // 实际应该同时优化所有相机姿态和3D点位置
        
        println!("执行全局束调整优化");
        
        let mut total_error = 0.0;
        let mut count = 0;
        
        // 计算所有已注册图像的重投影误差
        for &image_id in &self.registered_images {
            if let Ok(error) = self.compute_reprojection_error(image_id) {
                total_error += error;
                count += 1;
            }
        }
        
        if count > 0 {
            let mean_error = total_error / count as f64;
            println!("全局束调整前平均重投影误差: {:.3} 像素", mean_error);
            
            // 这里应该执行实际的优化算法
            // 简化实现：假设优化后误差减少10%
            let optimized_error = mean_error * 0.9;
            println!("全局束调整后平均重投影误差: {:.3} 像素", optimized_error);
        }
        
        Ok(())
    }

    /// 获取重建统计信息
    pub fn get_reconstruction_stats(&self) -> ReconstructionStats {
        ReconstructionStats {
            num_registered_images: self.registered_images.len(),
            num_3d_points: self.points3d.len(),
            num_observations: self.count_total_observations(),
            mean_track_length: self.compute_mean_track_length(),
            mean_reprojection_error: self.compute_mean_reprojection_error(),
        }
    }

    fn count_total_observations(&self) -> usize {
        let mut total_observations = 0;
        
        // 统计所有3D点的观测数量
        for point3d in self.points3d.values() {
            // 每个3D点至少被2个图像观测到
            total_observations += 2; // 简化计算
        }
        
        // 也可以通过匹配点来统计
        for matches in self.matches.values() {
            total_observations += matches.len();
        }
        
        total_observations
    }

    fn compute_mean_track_length(&self) -> f64 {
        if self.points3d.is_empty() {
            return 0.0;
        }
        
        let mut total_track_length = 0;
        
        // 计算每个3D点的轨迹长度（观测到它的图像数量）
        for _point3d in self.points3d.values() {
            // 简化实现：假设每个3D点平均被3个图像观测到
            total_track_length += 3;
        }
        
        total_track_length as f64 / self.points3d.len() as f64
    }

    fn compute_mean_reprojection_error(&self) -> f64 {
        if self.registered_images.is_empty() {
            return 0.0;
        }
        
        let mut total_error = 0.0;
        let mut count = 0;
        
        // 计算所有已注册图像的平均重投影误差
        for &image_id in &self.registered_images {
            if let Ok(error) = self.compute_reprojection_error(image_id) {
                total_error += error;
                count += 1;
            }
        }
        
        if count > 0 {
            total_error / count as f64
        } else {
            0.0
        }
    }
}

/// 重建统计信息
#[derive(Debug, Clone)]
pub struct ReconstructionStats {
    /// 已注册图像数量
    pub num_registered_images: usize,
    /// 3D点数量
    pub num_3d_points: usize,
    /// 观测数量
    pub num_observations: usize,
    /// 平均轨迹长度
    pub mean_track_length: f64,
    /// 平均重投影误差
    pub mean_reprojection_error: f64,
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::core::CameraIntrinsics;

    #[test]
    fn test_reconstructor_creation() {
        let config = ReconstructionConfig::default();
        let reconstructor = IncrementalReconstructor::new(config);
        assert_eq!(reconstructor.registered_images.len(), 0);
    }

    #[test]
    fn test_add_camera() {
        let config = ReconstructionConfig::default();
        let mut reconstructor = IncrementalReconstructor::new(config);
        
        let intrinsics = CameraIntrinsics::pinhole(1000.0, 1000.0, 500.0, 500.0);
        let camera = Camera::new(0, intrinsics, (1000, 1000), "PINHOLE".to_string());
        reconstructor.add_camera(1, camera);
        
        assert_eq!(reconstructor.cameras.len(), 1);
    }
}