Subversion Repositories seema-scanner

        Hdlt = Hest_DLT([mScale*Qlocal+mShift, ones(size(Qlocal, 1), 1)]', q(pointMask,:)');

        H = projective2d(Hdlt');

        confI = 1.0;

function H = Hest_DLT(q1, q2)

    % Estimate the homography between a set of point correspondences using the 

    % direct linear transform algorithm.

    %

    % Input:

    %           q1: 3xN matrix of homogenous point coordinates from camera 1. 

    %           q2: 3xN matrix of corresponding points from camera 2.

    % Output:

    %           H: 3x3 matrix. The Fundamental Matrix estimate. 

    %

    % Note that N must be at least 4.

    % See derivation in Aanaes, Lecture Notes on Computer Vision, 2011

    % Normalize points

    [T1,invT1] = normalizationMat(q1);

    q1_tilde = T1*q1;

    T2 = normalizationMat(q2);

    q2_tilde = T2*q2;

    % DLT estimation

    N = size(q1_tilde,2);

    assert(size(q2_tilde,2)==N);

    B = zeros(3*N,9);

    for i=1:N

        q1i = q1_tilde(:,i);

        q2i = q2_tilde(:,i);

        q1_x = [0 -q1i(3) q1i(2); q1i(3) 0 -q1i(1); -q1i(2) q1i(1) 0];

        biT = kron(q2i', q1_x); 

        B(3*(i-1)+1:3*i, :) = biT;

    end

    [U,S,~] = svd(B');

    [~,idx] = min(diag(S));

    h = U(:,idx);

    H_tilde = reshape(h, 3, 3);

    % Unnormalize H

    H = invT1*H_tilde*T2;

    % Arbitrarily chose scale

    H = H * 1/H(3,3);

end

function [T,invT] = normalizationMat(q)

    % Gives a normalization matrix for homogeneous coordinates

    % such that T*q will have zero mean and unit variance.

    % See Aanaes, Computer Vision Lecture Notes 2.8.2

    %

    % q: (M+1)xN matrix of N MD points in homogenous coordinates

    %

    % Extended to also efficiently compute the inverse matrix

    % DTU, 2013, Jakob Wilm

    [M,N] = size(q);

    M = M-1;

    mu = mean(q(1:M,:),2);

    q_bar = q(1:M,:)-repmat(mu,1,N);

    s = mean(sqrt(diag(q_bar'*q_bar)))/sqrt(2);

    T = [eye(M)/s, -mu/s; zeros(1,M) 1];

    invT = [eye(M)*s, mu; zeros(1,M) 1];

end