colmap 0.1.2

//! 相机姿态和变换相关的数据结构
//!
//! 这个模块定义了相机姿态、变换矩阵和相关的几何运算。

use nalgebra::{Matrix3, Matrix4, Vector3, Translation3, Isometry3, UnitQuaternion};
use serde::{Deserialize, Serialize};
use crate::core::Result;

/// 相机姿态（已在 camera.rs 中定义，这里提供额外的工具函数）
pub use crate::core::camera::CameraPose;

/// 姿态估计结果
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct PoseEstimationResult {
    /// 估计的姿态
    pub pose: CameraPose,
    /// 内点数量
    pub num_inliers: usize,
    /// 总匹配数量
    pub num_matches: usize,
    /// 内点比例
    pub inlier_ratio: f64,
    /// 重投影误差
    pub reprojection_error: f64,
}

impl PoseEstimationResult {
    /// 创建新的姿态估计结果
    pub fn new(
        pose: CameraPose,
        num_inliers: usize,
        num_matches: usize,
        reprojection_error: f64,
    ) -> Self {
        let inlier_ratio = if num_matches > 0 {
            num_inliers as f64 / num_matches as f64
        } else {
            0.0
        };
        
        Self {
            pose,
            num_inliers,
            num_matches,
            inlier_ratio,
            reprojection_error,
        }
    }
    
    /// 检查姿态估计是否可靠
    pub fn is_reliable(&self, min_inliers: usize, min_inlier_ratio: f64) -> bool {
        self.num_inliers >= min_inliers && self.inlier_ratio >= min_inlier_ratio
    }
}

/// 相对姿态
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct RelativePose {
    /// 相对旋转
    pub rotation: UnitQuaternion<f64>,
    /// 相对平移（单位向量）
    pub translation: Vector3<f64>,
    /// 尺度（如果已知）
    pub scale: Option<f64>,
}

impl RelativePose {
    /// 创建新的相对姿态
    pub fn new(rotation: UnitQuaternion<f64>, translation: Vector3<f64>) -> Self {
        Self {
            rotation,
            translation: translation.normalize(),
            scale: None,
        }
    }
    
    /// 设置尺度
    pub fn with_scale(mut self, scale: f64) -> Self {
        self.scale = Some(scale);
        self
    }
    
    /// 转换为变换矩阵
    pub fn to_transform(&self) -> Isometry3<f64> {
        let translation = if let Some(scale) = self.scale {
            self.translation * scale
        } else {
            self.translation
        };
        
        Isometry3::from_parts(
            Translation3::from(translation),
            self.rotation,
        )
    }
    
    /// 从两个绝对姿态计算相对姿态
    pub fn from_absolute_poses(pose1: &CameraPose, pose2: &CameraPose) -> Self {
        let relative_transform = pose1.matrix().try_inverse().unwrap() * pose2.matrix();
        
        // 从变换矩阵中提取旋转和平移
        let rotation_matrix = relative_transform.fixed_view::<3, 3>(0, 0);
        let translation_vec = relative_transform.fixed_view::<3, 1>(0, 3);
        
        let rotation = UnitQuaternion::from_matrix(&rotation_matrix.into_owned());
        let translation = Vector3::new(translation_vec[0], translation_vec[1], translation_vec[2]);
        let scale = translation.norm();
        
        Self {
            rotation,
            translation: translation.normalize(),
            scale: Some(scale),
        }
    }
}

/// 姿态工具函数
pub mod utils {
    use super::*;
    
    /// 从旋转矩阵和平移向量创建姿态
    pub fn pose_from_rt(rotation: &Matrix3<f64>, translation: &Vector3<f64>) -> Result<CameraPose> {
        let rotation_quat = UnitQuaternion::from_matrix_eps(
            rotation,
            1e-6,
            10,
            UnitQuaternion::identity(),
        );
        
        Ok(CameraPose::new(rotation_quat, *translation))
    }
    
    /// 从变换矩阵创建姿态
    pub fn pose_from_matrix(matrix: &Matrix4<f64>) -> Result<CameraPose> {
        let rotation = matrix.fixed_view::<3, 3>(0, 0).into_owned();
        let translation = matrix.fixed_view::<3, 1>(0, 3).into_owned();
        
        pose_from_rt(&rotation, &translation)
    }
    
    /// 计算两个姿态之间的角度差（弧度）
    pub fn angle_between_poses(pose1: &CameraPose, pose2: &CameraPose) -> f64 {
        let relative_rotation = pose1.rotation.inverse() * pose2.rotation;
        relative_rotation.angle()
    }
    
    /// 计算两个姿态之间的平移距离
    pub fn distance_between_poses(pose1: &CameraPose, pose2: &CameraPose) -> f64 {
        (pose1.translation - pose2.translation).norm()
    }
    
    /// 插值两个姿态
    pub fn interpolate_poses(pose1: &CameraPose, pose2: &CameraPose, t: f64) -> CameraPose {
        let t = t.clamp(0.0, 1.0);
        
        let rotation = pose1.rotation.slerp(&pose2.rotation, t);
        let translation = pose1.translation.lerp(&pose2.translation, t);
        
        CameraPose::new(rotation, translation)
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use nalgebra::{Vector3, UnitQuaternion};
    
    #[test]
    fn test_pose_estimation_result() {
        let pose = CameraPose::identity();
        let result = PoseEstimationResult::new(pose, 50, 100, 1.5);
        
        assert_eq!(result.num_inliers, 50);
        assert_eq!(result.num_matches, 100);
        assert!((result.inlier_ratio - 0.5).abs() < 1e-6);
        assert!(result.is_reliable(30, 0.3));
        assert!(!result.is_reliable(60, 0.3));
    }
    
    #[test]
    fn test_relative_pose() {
        let rotation = UnitQuaternion::identity();
        let translation = Vector3::new(1.0, 0.0, 0.0);
        
        let rel_pose = RelativePose::new(rotation, translation).with_scale(2.0);
        assert_eq!(rel_pose.scale, Some(2.0));
        
        let transform = rel_pose.to_transform();
        assert!((transform.translation.vector.norm() - 2.0).abs() < 1e-6);
    }
    
    #[test]
    fn test_pose_utils() {
        let pose1 = CameraPose::identity();
        let pose2 = CameraPose::new(
            UnitQuaternion::identity(),
            Vector3::new(1.0, 0.0, 0.0),
        );
        
        let distance = utils::distance_between_poses(&pose1, &pose2);
        assert!((distance - 1.0).abs() < 1e-6);
        
        let interpolated = utils::interpolate_poses(&pose1, &pose2, 0.5);
        assert!((interpolated.translation.x - 0.5).abs() < 1e-6);
    }
}