scirs2-vision 0.4.4

//! Homography estimation and image perspective transformation
//!
//! This module provides functions for estimating homographies between images
//! and applying perspective transformations to images.

use crate::error::Result;
use crate::feature::{
    ransac::{run_ransac, Homography, RansacConfig},
    KeyPoint, PointMatch,
};
use image::{DynamicImage, GenericImageView, ImageBuffer, Pixel, Rgba};

/// Find homography between two sets of corresponding points
///
/// # Arguments
///
/// * `src_points` - Source points (x, y)
/// * `dst_points` - Destination points (x, y)
/// * `ransac_threshold` - Maximum allowed reprojection error
/// * `confidence` - Confidence level (0-1)
///
/// # Returns
///
/// * Result containing (homography, mask) where mask indicates inliers
#[allow(dead_code)]
pub fn find_homography(
    src_points: &[(f64, f64)],
    dst_points: &[(f64, f64)],
    ransac_threshold: f64,
    confidence: f64,
) -> Result<(Homography, Vec<bool>)> {
    if src_points.len() != dst_points.len() {
        return Err(crate::error::VisionError::InvalidParameter(
            "Source and destination point sets must have the same length".to_string(),
        ));
    }

    if src_points.len() < 4 {
        return Err(crate::error::VisionError::InvalidParameter(
            "At least 4 point correspondences are required".to_string(),
        ));
    }

    // Create matches from point pairs
    let matches: Vec<_> = src_points
        .iter()
        .zip(dst_points.iter())
        .map(|(&src, &dst)| PointMatch {
            point1: src,
            point2: dst,
        })
        .collect();

    // Configure RANSAC
    let config = RansacConfig {
        max_iterations: 2000,
        threshold: ransac_threshold,
        min_inliers: 4,
        confidence,
        seed: None,
        refinement_iterations: 5,
    };

    // Run RANSAC for homography estimation
    let result = run_ransac::<Homography>(&matches, &config)?;

    // Create inlier mask
    let mut mask = vec![false; src_points.len()];
    for &idx in &result.inliers {
        mask[idx] = true;
    }

    Ok((result.model, mask))
}

/// Find homography between two images using feature matches
///
/// # Arguments
///
/// * `matches` - Matching keypoints between images (idx1, idx2, distance)
/// * `keypoints1` - Keypoints from first image
/// * `keypoints2` - Keypoints from second image
/// * `ransac_threshold` - Maximum allowed reprojection error
/// * `confidence` - Confidence level (0-1)
///
/// # Returns
///
/// * Result containing (homography, mask) where mask indicates inliers
#[allow(dead_code)]
pub fn find_homography_from_matches(
    matches: &[(usize, usize, f32)],
    keypoints1: &[KeyPoint],
    keypoints2: &[KeyPoint],
    ransac_threshold: f64,
    confidence: f64,
) -> Result<(Homography, Vec<bool>)> {
    if matches.is_empty() {
        return Err(crate::error::VisionError::InvalidParameter(
            "No matches provided".to_string(),
        ));
    }

    // Extract matching points
    let mut src_points = Vec::with_capacity(matches.len());
    let mut dst_points = Vec::with_capacity(matches.len());

    for &(idx1, idx2_, _) in matches {
        if idx1 >= keypoints1.len() || idx2_ >= keypoints2.len() {
            return Err(crate::error::VisionError::InvalidParameter(format!(
                "Invalid keypoint indices: ({idx1}, {idx2_})"
            )));
        }

        let kp1 = &keypoints1[idx1];
        let kp2 = &keypoints2[idx2_];

        src_points.push((kp1.x as f64, kp1.y as f64));
        dst_points.push((kp2.x as f64, kp2.y as f64));
    }

    find_homography(&src_points, &dst_points, ransac_threshold, confidence)
}

/// Warp an image using a homography
///
/// # Arguments
///
/// * `src` - Source image
/// * `homography` - Homography matrix
/// * `width` - Output image width
/// * `height` - Output image height
///
/// # Returns
///
/// * Result containing warped image
#[allow(dead_code)]
pub fn warp_perspective<P>(
    src: &DynamicImage,
    homography: &Homography,
    width: u32,
    height: u32,
) -> Result<DynamicImage>
where
    P: Pixel + 'static,
    P::Subpixel: 'static,
{
    let mut dst = ImageBuffer::new(width, height);

    // Compute bounds for warping
    let src_width = src.width();
    let src_height = src.height();

    // Inverse mapping approach - loop through destination pixels
    for y in 0..height {
        for x in 0..width {
            // Apply inverse homography to get source pixel
            let (src_x, src_y) = homography.inverse_transform_point(x as f64, y as f64);

            // Bilinear interpolation
            // Check if source point is within bounds with border
            if src_x >= 0.0
                && src_x < (src_width as f64 - 1.0)
                && src_y >= 0.0
                && src_y < (src_height as f64 - 1.0)
            {
                let x0 = src_x.floor() as u32;
                let y0 = src_y.floor() as u32;
                let x1 = x0 + 1;
                let y1 = y0 + 1;

                // Fractional parts
                let sx = src_x - x0 as f64;
                let sy = src_y - y0 as f64;

                // Get four surrounding pixels
                let p00 = src.get_pixel(x0, y0);
                let p01 = src.get_pixel(x0, y1);
                let p10 = src.get_pixel(x1, y0);
                let p11 = src.get_pixel(x1, y1);

                // Convert to RGB for blending
                let color = interpolate_pixel(
                    &p00.to_rgba(),
                    &p01.to_rgba(),
                    &p10.to_rgba(),
                    &p11.to_rgba(),
                    sx,
                    sy,
                );

                dst.put_pixel(x, y, color);
            }
        }
    }

    Ok(DynamicImage::ImageRgba8(dst))
}

/// Bilinear interpolation for pixels
#[allow(dead_code)]
fn interpolate_pixel(
    p00: &Rgba<u8>,
    p01: &Rgba<u8>,
    p10: &Rgba<u8>,
    p11: &Rgba<u8>,
    sx: f64,
    sy: f64,
) -> Rgba<u8> {
    let mut color = [0u8; 4];

    for c in 0..4 {
        let c00 = p00[c] as f64;
        let c01 = p01[c] as f64;
        let c10 = p10[c] as f64;
        let c11 = p11[c] as f64;

        // Bilinear interpolation formula
        let val = (1.0 - sx) * (1.0 - sy) * c00
            + sx * (1.0 - sy) * c10
            + (1.0 - sx) * sy * c01
            + sx * sy * c11;

        color[c] = val.round().clamp(0.0, 255.0) as u8;
    }

    Rgba(color)
}

/// Calculate perspective transform from quad to quad
///
/// # Arguments
///
/// * `src_quad` - Source quadrilateral corners
/// * `dst_quad` - Destination quadrilateral corners
///
/// # Returns
///
/// * Result containing homography matrix
#[allow(dead_code)]
pub fn perspective_transform(
    src_quad: &[(f64, f64); 4],
    dst_quad: &[(f64, f64); 4],
) -> Result<Homography> {
    find_homography(src_quad, dst_quad, 1e-10, 0.99).map(|(h_, _)| h_)
}

/// Fit a rectangle to a set of points
///
/// # Arguments
///
/// * `points` - Input points
///
/// # Returns
///
/// * Rectangle corners (top-left, top-right, bottom-right, bottom-left)
#[allow(dead_code)]
pub fn fit_rectangle(points: &[(f64, f64)]) -> [(f64, f64); 4] {
    if points.is_empty() {
        return [(0.0, 0.0); 4];
    }

    // Find min/max coordinates
    let mut min_x = f64::MAX;
    let mut min_y = f64::MAX;
    let mut max_x = f64::MIN;
    let mut max_y = f64::MIN;

    for &(x, y) in points {
        min_x = min_x.min(x);
        min_y = min_y.min(y);
        max_x = max_x.max(x);
        max_y = max_y.max(y);
    }

    // Create rectangle corners
    [
        (min_x, min_y), // top-left
        (max_x, min_y), // top-right
        (max_x, max_y), // bottom-right
        (min_x, max_y), // bottom-left
    ]
}

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

    #[test]
    fn test_perspective_transform() {
        // Skip the RANSAC-based approach and directly create a homography
        let h = crate::feature::ransac::Homography::new(&[
            1.1, 0.05, 10.0, 0.05, 1.2, 15.0, 0.001, 0.001, 1.0,
        ]);

        // Test point transformations
        let src_points = [(50.0, 50.0), (100.0, 50.0), (100.0, 100.0), (50.0, 100.0)];

        for (x, y) in src_points {
            let (tx, ty) = h.transform_point(x, y);
            let (x_back, y_back) = h.inverse_transform_point(tx, ty);

            // Test that forward and inverse transformations are consistent
            assert!((x - x_back).abs() < 1.0);
            assert!((y - y_back).abs() < 1.0);
        }
    }

    #[test]
    fn test_fit_rectangle() {
        let points = [
            (10.0, 5.0),
            (5.0, 10.0),
            (20.0, 30.0),
            (40.0, 15.0),
            (30.0, 5.0),
        ];

        let rect = fit_rectangle(&points);

        assert_eq!(rect[0], (5.0, 5.0)); // top-left
        assert_eq!(rect[1], (40.0, 5.0)); // top-right
        assert_eq!(rect[2], (40.0, 30.0)); // bottom-right
        assert_eq!(rect[3], (5.0, 30.0)); // bottom-left
    }
}