Homographic transform via SVD/DLT - is it broken or am I doing it wrong?

[tomyan]

I am trying to perform a homographic transform using SVD. Here's a prototype implementation in python showing what I am trying to do that I think gives the correct result:

import numpy as np
from scipy.linalg import svd

# Define the source and target points
source_points = np.array([
    [0.0, 0.0],
    [1.0, 0.0],
    [0.0, 1.0],
    [1.0, 1.0]
])

target_points = np.array([
    [0.0, 0.0],
    [1.0, 0.0],
    [0.0, 1.0],
    [1.0, 1.0]
])

# Construct the A matrix
A = np.zeros((2 * len(source_points), 9))
for i in range(len(source_points)):
    p = source_points[i]
    p_prime = target_points[i]

    row1 = [-p[0], -p[1], -1.0, 0.0, 0.0, 0.0, p_prime[0] * p[0], p_prime[0] * p[1], p_prime[0]]
    row2 = [0.0, 0.0, 0.0, -p[0], -p[1], -1.0, p_prime[1] * p[0], p_prime[1] * p[1], p_prime[1]]

    A[2 * i] = row1
    A[2 * i + 1] = row2

# Perform SVD
U, s, Vt = svd(A)

h = np.reshape(Vt[-1], (3, 3))
print("h:")
print(h)

h /= h[2, 2]
print("Normalized h:")
print(h)

# Test transoforming the source points
transformed_points = np.zeros((len(source_points), 2))
for i in range(len(source_points)):
    p = source_points[i]
    p_prime = np.dot(h, np.append(p, 1))
    p_prime /= p_prime[2]
    transformed_points[i] = p_prime[:2]

print("Transformed points:")
print(transformed_points)

The output shows that the generated transformation matrix is the identity transform, and that the source points get transformed to the target accordingly:

h:
[[ 5.77350269e-01  0.00000000e+00  0.00000000e+00]
 [ 5.55111512e-17  5.77350269e-01  0.00000000e+00]
 [-5.55111512e-17  1.66533454e-16  5.77350269e-01]]
Normalized h:
[[ 1.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 9.61481343e-17  1.00000000e+00  0.00000000e+00]
 [-9.61481343e-17  2.88444403e-16  1.00000000e+00]]
Transformed points:
[[0.00000000e+00 0.00000000e+00]
 [1.00000000e+00 9.61481343e-17]
 [0.00000000e+00 1.00000000e+00]
 [1.00000000e+00 1.00000000e+00]]

I have tried to do the same using nalgegra, but I get very different results:

use nalgebra::{Matrix3, DMatrix, RowDVector, Point2, Point3, SVD};

fn compute_a(source_points: &[Point2<f64>], target_points: &[Point2<f64>]) -> DMatrix<f64> {
    let mut a = DMatrix::<f64>::zeros(2 * source_points.len(), 9);
    for i in 0..source_points.len() {
        let p = &source_points[i];
        let p_prime = &target_points[i];

        let row1 = RowDVector::from_row_slice(&[-p.x, -p.y, -1.0, 0.0, 0.0, 0.0, p_prime.x * p.x, p_prime.x * p.y, p_prime.x]);
        let row2 = RowDVector::from_row_slice(&[0.0, 0.0, 0.0, -p.x, -p.y, -1.0, p_prime.y * p.x, p_prime.y * p.y, p_prime.y]);

        a.set_row(2 * i, &row1);
        a.set_row(2 * i + 1, &row2);
    }
    a
}

#[derive(Debug)]
struct HomographyError;

impl HomographyError {
    fn new(msg: &str) -> Self {
        println!("{}", msg);
        HomographyError
    }
}

fn compute_homography_matrix(source_points: &[Point2<f64>], target_points: &[Point2<f64>]) -> Result<Matrix3<f64>, HomographyError> {
    let a = compute_a(source_points, target_points);

    let svd = SVD::new(a, true, true);
    let v_t = svd.v_t.ok_or_else(|| HomographyError::new("SVD failed to produce V^T."))?;

    let h_vec = v_t.row(v_t.nrows() - 1).iter().cloned().collect::<Vec<_>>();
    let homography_matrix = Matrix3::from_column_slice(&h_vec);

    println!("h = {:?}", homography_matrix);

    let scale = homography_matrix[(2, 2)];
    let normalized_homography_matrix = homography_matrix / scale;

    println!("Normalized h = {:?}", normalized_homography_matrix);

    Ok(normalized_homography_matrix)
}

fn apply_homography(h: &Matrix3<f64>, source_points: &[Point2<f64>]) -> Vec<Point2<f64>> {
    source_points.iter().map(|p| {
        let p_homogeneous = Point3::new(p.x, p.y, 1.0);
        let p_transformed = h * p_homogeneous;
        Point2::new(p_transformed.x / p_transformed.z, p_transformed.y / p_transformed.z)
    }).collect()
}

fn main() {
    let source_points = vec![
        Point2::new(0.0, 0.0),
        Point2::new(1.0, 0.0),
        Point2::new(0.0, 1.0),
        Point2::new(1.0, 1.0),
    ];

    let target_points = vec![
        Point2::new(0.0, 0.0),
        Point2::new(1.0, 0.0),
        Point2::new(0.0, 1.0),
        Point2::new(1.0, 1.0),
    ];

    let hm = compute_homography_matrix(&source_points, &target_points).unwrap();
    let transformed_points = apply_homography(&hm, &source_points);
    print!("Transformed points = {:?}", transformed_points);

}

Output:

h = [[-0.24584745200231492, -0.2741541422939806, 0.16048347717613345], [-0.27415414229398327, -0.24584745200231237, 0.16048347717613462], [-0.4666493874938876, -0.46664938749388307, 0.49169490400462795]]
Normalized h = [[-0.5000000000000019, -0.5575696230754513, 0.32638832712942445], [-0.5575696230754567, -0.4999999999999967, 0.32638832712942684], [-0.9490628918324022, -0.949062891832393, 1.0]]
Transformed points = [[-0.9490628918324022, -0.949062891832393], [-1.0924876691040188, -1.1358909635223533], [-1.1358909635223622, -1.0924876691040064], [-1.2140978091245413, -1.2140978091245294]]

Most likely I am using it incorrectly I guess?