colmap 0.1.2

//! 三角化算法实现

use crate::core::{
    Camera, CameraPose, Point2, Point3, Vector3, Result, ColmapError
};
use nalgebra::{Matrix4, SVD};

/// 三角化方法
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum TriangulationMethod {
    /// 线性三角化（DLT）
    Linear,
    /// 迭代三角化
    Iterative,
    /// 中点法
    Midpoint,
}

/// 三角化配置
#[derive(Debug, Clone)]
pub struct TriangulationConfig {
    /// 三角化方法
    pub method: TriangulationMethod,
    /// 最小三角化角度（弧度）
    pub min_angle: f64,
    /// 最大重投影误差
    pub max_reprojection_error: f64,
    /// 迭代法的最大迭代次数
    pub max_iterations: usize,
    /// 收敛阈值
    pub convergence_threshold: f64,
}

impl Default for TriangulationConfig {
    fn default() -> Self {
        Self {
            method: TriangulationMethod::Linear,
            min_angle: 1.0_f64.to_radians(),
            max_reprojection_error: 4.0,
            max_iterations: 100,
            convergence_threshold: 1e-6,
        }
    }
}

/// 三角化结果
#[derive(Debug, Clone)]
pub struct TriangulationResult {
    /// 三角化的3D点
    pub point: Point3,
    /// 是否成功
    pub success: bool,
    /// 重投影误差
    pub reprojection_error: f64,
    /// 三角化角度
    pub triangulation_angle: f64,
}

/// 三角化器
#[derive(Debug)]
pub struct Triangulator {
    config: TriangulationConfig,
}

impl Triangulator {
    /// 创建新的三角化器
    pub fn new(config: TriangulationConfig) -> Self {
        Self { config }
    }

    /// 使用默认配置创建三角化器
    pub fn default() -> Self {
        Self::new(TriangulationConfig::default())
    }

    /// 三角化两个视图中的对应点
    pub fn triangulate_two_view(
        &self,
        camera1: &Camera,
        pose1: &CameraPose,
        point1: &Point2,
        camera2: &Camera,
        pose2: &CameraPose,
        point2: &Point2,
    ) -> Result<TriangulationResult> {
        // 检查三角化角度
        let angle = self.compute_triangulation_angle(
            camera1, pose1, point1,
            camera2, pose2, point2,
        )?;
        
        if angle < self.config.min_angle {
            return Ok(TriangulationResult {
                point: Point3::new(0.0, 0.0, 0.0),
                success: false,
                reprojection_error: f64::INFINITY,
                triangulation_angle: angle,
            });
        }

        // 根据配置选择三角化方法
        let point = match self.config.method {
            TriangulationMethod::Linear => {
                self.triangulate_linear(camera1, pose1, point1, camera2, pose2, point2)?
            },
            TriangulationMethod::Iterative => {
                self.triangulate_iterative(camera1, pose1, point1, camera2, pose2, point2)?
            },
            TriangulationMethod::Midpoint => {
                self.triangulate_midpoint(camera1, pose1, point1, camera2, pose2, point2)?
            },
        };

        // 计算重投影误差
        let error1 = self.compute_reprojection_error(camera1, pose1, &point, point1)?;
        let error2 = self.compute_reprojection_error(camera2, pose2, &point, point2)?;
        let reprojection_error = (error1 + error2) / 2.0;

        let success = reprojection_error < self.config.max_reprojection_error;

        Ok(TriangulationResult {
            point,
            success,
            reprojection_error,
            triangulation_angle: angle,
        })
    }

    /// 多视图三角化
    pub fn triangulate_multi_view(
        &self,
        observations: &[(Camera, CameraPose, Point2)],
    ) -> Result<TriangulationResult> {
        if observations.len() < 2 {
            return Err(ColmapError::Triangulation(
                "至少需要两个观测点进行三角化".to_string()
            ));
        }

        // 使用前两个观测进行初始三角化
        let (camera1, pose1, point1) = &observations[0];
        let (camera2, pose2, point2) = &observations[1];
        
        let mut result = self.triangulate_two_view(
            camera1, pose1, point1,
            camera2, pose2, point2,
        )?;

        if !result.success {
            return Ok(result);
        }

        // 如果有更多观测，使用迭代优化
        if observations.len() > 2 {
            result.point = self.refine_point_multi_view(&result.point, observations)?;
            
            // 重新计算重投影误差
            let mut total_error = 0.0;
            for (camera, pose, point) in observations {
                let error = self.compute_reprojection_error(camera, pose, &result.point, point)?;
                total_error += error;
            }
            result.reprojection_error = total_error / observations.len() as f64;
            result.success = result.reprojection_error < self.config.max_reprojection_error;
        }

        Ok(result)
    }

    /// 线性三角化（DLT方法）
    fn triangulate_linear(
        &self,
        camera1: &Camera,
        pose1: &CameraPose,
        point1: &Point2,
        camera2: &Camera,
        pose2: &CameraPose,
        point2: &Point2,
    ) -> Result<Point3> {
        // 构建投影矩阵
        let p1 = self.build_projection_matrix(camera1, pose1)?;
        let p2 = self.build_projection_matrix(camera2, pose2)?;

        // 归一化图像坐标
        let norm1 = camera1.image_to_normalized(point1)?;
        let norm2 = camera2.image_to_normalized(point2)?;

        // 构建线性方程组 AX = 0
        let mut a = Matrix4::zeros();
        
        // 第一个相机的约束
        a.set_row(0, &(norm1.x * p1.row(2) - p1.row(0)));
        a.set_row(1, &(norm1.y * p1.row(2) - p1.row(1)));
        
        // 第二个相机的约束
        a.set_row(2, &(norm2.x * p2.row(2) - p2.row(0)));
        a.set_row(3, &(norm2.y * p2.row(2) - p2.row(1)));

        // 使用SVD求解
        let svd = SVD::new(a, true, true);
        let v = svd.v_t.ok_or_else(|| ColmapError::Triangulation(
            "SVD分解失败".to_string()
        ))?;
        
        let solution = v.row(3);
        
        if solution[3].abs() < 1e-10 {
            return Err(ColmapError::Triangulation(
                "齐次坐标的w分量接近零".to_string()
            ));
        }

        Ok(Point3::new(
            solution[0] / solution[3],
            solution[1] / solution[3],
            solution[2] / solution[3],
        ))
    }

    /// 迭代三角化
    fn triangulate_iterative(
        &self,
        camera1: &Camera,
        pose1: &CameraPose,
        point1: &Point2,
        camera2: &Camera,
        pose2: &CameraPose,
        point2: &Point2,
    ) -> Result<Point3> {
        // 先用线性方法获得初始估计
        let mut point = self.triangulate_linear(camera1, pose1, point1, camera2, pose2, point2)?;

        // 迭代优化
        for _ in 0..self.config.max_iterations {
            let old_point = point;
            
            // 计算雅可比矩阵和残差
            let (jacobian, residual) = self.compute_jacobian_and_residual(
                &point,
                &[(camera1.clone(), pose1.clone(), *point1),
                  (camera2.clone(), pose2.clone(), *point2)],
            )?;

            // 求解线性系统
            let jtj = jacobian.transpose() * &jacobian;
            let jtr = jacobian.transpose() * residual;
            
            let svd = SVD::new(jtj, true, true);
            let delta = svd.solve(&jtr, 1e-10).map_err(|_| ColmapError::Triangulation(
                "无法求解线性系统".to_string()
            ))?;

            // 更新点坐标
            point.coords += delta;

            // 检查收敛
            if (point - old_point).norm() < self.config.convergence_threshold {
                break;
            }
        }

        Ok(point)
    }

    /// 中点法三角化
    fn triangulate_midpoint(
        &self,
        camera1: &Camera,
        pose1: &CameraPose,
        point1: &Point2,
        camera2: &Camera,
        pose2: &CameraPose,
        point2: &Point2,
    ) -> Result<Point3> {
        // 获取相机中心和射线方向
        let center1 = pose1.translation;
        let center2 = pose2.translation;
        
        let ray1 = self.compute_ray_direction(camera1, pose1, point1)?;
        let ray2 = self.compute_ray_direction(camera2, pose2, point2)?;

        // 计算两条射线的最近点
        let center1_point = Point3::new(center1.x, center1.y, center1.z);
        let center2_point = Point3::new(center2.x, center2.y, center2.z);
        let (t1, t2) = self.compute_closest_points_on_rays(&center1_point, &ray1, &center2_point, &ray2)?;
        
        let point1_on_ray = center1 + t1 * ray1;
        let point2_on_ray = center2 + t2 * ray2;
        
        // 返回中点
        let midpoint = (point1_on_ray + point2_on_ray) / 2.0;
        Ok(Point3::new(midpoint.x, midpoint.y, midpoint.z))
    }

    /// 计算三角化角度
    fn compute_triangulation_angle(
        &self,
        camera1: &Camera,
        pose1: &CameraPose,
        point1: &Point2,
        camera2: &Camera,
        pose2: &CameraPose,
        point2: &Point2,
    ) -> Result<f64> {
        let _center1 = pose1.translation;
        let _center2 = pose2.translation;
        
        let ray1 = self.compute_ray_direction(camera1, pose1, point1)?;
        let ray2 = self.compute_ray_direction(camera2, pose2, point2)?;
        
        let cos_angle = ray1.dot(&ray2) / (ray1.norm() * ray2.norm());
        Ok(cos_angle.acos())
    }

    /// 计算射线方向
    fn compute_ray_direction(
        &self,
        camera: &Camera,
        pose: &CameraPose,
        point: &Point2,
    ) -> Result<Vector3> {
        let normalized = camera.image_to_normalized(point)?;
        let ray_camera = Vector3::new(normalized.x, normalized.y, 1.0).normalize();
        Ok(pose.rotation * ray_camera)
    }

    /// 计算两条射线上的最近点参数
    fn compute_closest_points_on_rays(
        &self,
        origin1: &Point3,
        dir1: &Vector3,
        origin2: &Point3,
        dir2: &Vector3,
    ) -> Result<(f64, f64)> {
        let w = origin1 - origin2;
        let a = dir1.dot(dir1);
        let b = dir1.dot(dir2);
        let c = dir2.dot(dir2);
        let d = dir1.dot(&w);
        let e = dir2.dot(&w);
        
        let denom = a * c - b * b;
        if denom.abs() < 1e-10 {
            return Err(ColmapError::Triangulation(
                "射线平行，无法三角化".to_string()
            ));
        }
        
        let t1 = (b * e - c * d) / denom;
        let t2 = (a * e - b * d) / denom;
        
        Ok((t1, t2))
    }

    /// 构建投影矩阵
    fn build_projection_matrix(&self, camera: &Camera, pose: &CameraPose) -> Result<nalgebra::Matrix3x4<f64>> {
        let k = camera.calibration_matrix();
        let rt = pose.to_matrix3x4();
        Ok(k * rt)
    }

    /// 计算重投影误差
    fn compute_reprojection_error(
        &self,
        camera: &Camera,
        pose: &CameraPose,
        point3d: &Point3,
        observed: &Point2,
    ) -> Result<f64> {
        // 需要先设置相机姿态
        let mut temp_camera = camera.clone();
        temp_camera.set_pose(pose.clone());
        let projected = temp_camera.world_to_image(point3d)?;
        let dx = projected.x - observed.x;
        let dy = projected.y - observed.y;
        Ok((dx * dx + dy * dy).sqrt())
    }

    /// 多视图点优化
    fn refine_point_multi_view(
        &self,
        initial_point: &Point3,
        observations: &[(Camera, CameraPose, Point2)],
    ) -> Result<Point3> {
        let mut point = *initial_point;
        
        for _ in 0..self.config.max_iterations {
            let old_point = point;
            
            let (jacobian, residual) = self.compute_jacobian_and_residual(&point, observations)?;
            
            let jtj = jacobian.transpose() * &jacobian;
            let jtr = jacobian.transpose() * residual;
            
            let svd = SVD::new(jtj, true, true);
            let delta = svd.solve(&jtr, 1e-10).map_err(|_| ColmapError::Triangulation(
                "无法求解线性系统".to_string()
            ))?;

            point.coords += delta;

            if (point - old_point).norm() < self.config.convergence_threshold {
                break;
            }
        }
        
        Ok(point)
    }

    /// 计算雅可比矩阵和残差
    fn compute_jacobian_and_residual(
        &self,
        point: &Point3,
        observations: &[(Camera, CameraPose, Point2)],
    ) -> Result<(nalgebra::DMatrix<f64>, nalgebra::DVector<f64>)> {
        let num_observations = observations.len();
        let mut jacobian = nalgebra::DMatrix::zeros(2 * num_observations, 3);
        let mut residual = nalgebra::DVector::zeros(2 * num_observations);
        
        for (i, (camera, pose, observed)) in observations.iter().enumerate() {
            // 需要先设置相机姿态
            let mut temp_camera = camera.clone();
            temp_camera.set_pose(pose.clone());
            let projected = temp_camera.world_to_image(point)?;
            
            // 残差
            residual[2 * i] = projected.x - observed.x;
            residual[2 * i + 1] = projected.y - observed.y;
            
            // 雅可比矩阵（简化实现）
            let eps = 1e-6;
            for j in 0..3 {
                let mut point_plus = *point;
                point_plus[j] += eps;
                // 需要先设置相机姿态
                let mut temp_camera = camera.clone();
                temp_camera.set_pose(pose.clone());
                let projected_plus = temp_camera.world_to_image(&point_plus)?;
                
                jacobian[(2 * i, j)] = (projected_plus.x - projected.x) / eps;
                jacobian[(2 * i + 1, j)] = (projected_plus.y - projected.y) / eps;
            }
        }
        
        Ok((jacobian, residual))
    }
}

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

    #[test]
    fn test_triangulator_creation() {
        let triangulator = Triangulator::default();
        assert_eq!(triangulator.config.method, TriangulationMethod::Linear);
    }

    #[test]
    fn test_triangulation_config() {
        let config = TriangulationConfig {
            method: TriangulationMethod::Iterative,
            min_angle: 2.0_f64.to_radians(),
            max_reprojection_error: 2.0,
            max_iterations: 50,
            convergence_threshold: 1e-8,
        };
        
        let triangulator = Triangulator::new(config);
        assert_eq!(triangulator.config.method, TriangulationMethod::Iterative);
        assert_eq!(triangulator.config.max_iterations, 50);
    }
}