horn.m

This is a MATLAB implementation of Shape from Motion.

function [Z,p,q]=horn(lambda,image,mu,adjust,ps,qs,iter)
src_norm = sqrt(ps^2 + qs^2 + 1.0);
image=double(image);
[szy,szx] = size(image);
height=szy;
width=szx;
adjDuration = 200;
lambdaMin=0.1;
lambdaMax= lambda;

muMin=0.05;
muMax=mu;

% brightnessErr = zeros(1,iter);
% integrability = zeros(1,iter);

 ZOld = zeros(szy,szx);
 pOld = ones(szy,szx);
 qOld = ones(szy,szx);



 Z = ZOld;
 p = pOld;
 q = qOld;

Zx = zeros(szy,szx);
Zy = zeros(szy,szx);

for k = 1:iter
    for i=2:szy-1
        for j = 2:szx-1
            % Current Z first deratives (p and q)
            ZOldx = (ZOld(i,j+1) - ZOld(i,j) + ZOld(i+1,j+1) - ZOld(i+1,j))/2.0;
            ZOldy = (ZOld(i+1,j) - ZOld(i,j) + ZOld(i+1,j+1) - ZOld(i,j+1))/2.0;
            Zx(i,j)=ZOldx;
            Zy(i,j)=ZOldy;
            % Current p and q first deratives
            pOldx = (pOld(i,j+1) - pOld(i,j-1))/2.0;
            qOldy = (qOld(i+1,j) - qOld(i-1,j))/2.0;

            ZOldavg = (Z(i-1,j-1) + ZOld(i-1,j+1) + Z(i+1,j-1) + ZOld(i+1,j+1))/4.0;
            pOldavg = (p(i-1,j-1) + pOld(i-1,j+1) + p(i+1,j-1) + pOld(i+1,j+1))/4.0;
            qOldavg = (q(i-1,j-1) + qOld(i-1,j+1) + q(i+1,j-1) + qOld(i+1,j+1))/4.0;

            %New values of p and q
            %Reference Gradient at current picture cell
            p0 = pOld(i,j);
            q0 = qOld(i,j);
            dZOldx = ZOldx - p0;
            dZOldy = ZOldy - q0;
            dpOldavg = pOldavg - p0;
            dqOldavg = qOldavg - q0;
            L2 = 2.0*lambda + mu;

            %R, Rp and Rq are evaluated at reference cell (see Horn '89)
            A = 2.0*lambda*dpOldavg + mu*dZOldx + (image(i,j)- R(p0,q0,1,ps,qs,src_norm))*Rp(p0,q0,ps,qs,src_norm);
            B = 2.0*lambda*dqOldavg + mu*dZOldy + (image(i,j)- R(p0,q0,1,ps,qs,src_norm))*Rq(p0,q0,ps,qs,src_norm);
            D = L2*(L2 + Rp(p0,q0,ps,qs,src_norm)*Rp(p0,q0,ps,qs,src_norm) + Rq(p0,q0,ps,qs,src_norm)*Rq(p0,q0,ps,qs,src_norm));

            p(i,j) = p0 + ((L2 + Rq(p0,q0,ps,qs,src_norm)^2)*A - Rp(p0,q0,ps,qs,src_norm)*Rq(p0,q0,ps,qs,src_norm)*B)/D;
            q(i,j) = q0 + ((L2 + Rp(p0,q0,ps,qs,src_norm)^2)*B - Rp(p0,q0,ps,qs,src_norm)*Rq(p0,q0,ps,qs,src_norm)*A)/D;

            Z(i,j) = ZOldavg - 0.5*(pOldx + qOldy);
        end
    end


    %Impose natural boundary conditions on p and q if not available
    %p: Corners (diagonal copies without averaging)
    % 0--0 case
    p(1,1)              = p(2,2);
    p(1,width)          = p(2,width-1);
    p(height,1)         = p(height-1,2);
    p(height,width)     = p(height-1,width-1);

    %0--1 case
    p(1,2)              = p(2,3);
    p(1,width-1)        = p(2,width-2);
    p(height-1,1)       = p(height-2,2);
    p(height-1,width)   = p(height-1,width-2);

    %1--0 case
    p(2,1)              = p(3,2);
    p(2,width)          = p(3,width-1);
    p(height,2)         = p(height-1,3);
    p(height,width-1)   = p(height-1,width-2);

    %q: Corners (diagonal copies without averaging)

    % 0--0 case
    q(1,1)              = q(2,2);
    q(1,width)          = q(2,width-1);
    q(height,1)         = q(height-1,2);
    q(height,width)     = q(height-1,width-1);

    %0--1 case
    q(1,2)              = q(2,3);
    q(1,width-1)        = q(2,width-2);
    q(height-1,1)       = q(height-2,2);
    q(height-1,width)   = q(height-2,width-1);

    %1--0 case
    q(2,1)              = q(3,2);
    q(2,width)          = q(3,width-1);
    q(height,2)         = q(height-1,3);
    q(height,width-1)   = q(height-1,width-2);

    for i = 2:szy-1
        p(i,1)       = 0.5*(p(i-1,1)       + p(i+1,2));         % p left
        p(i,width)   = 0.5*(p(i-1,width-2) + p(i+1,width-2));   % p right

        q(i,1)       = 0.5*(q(i-1,2)       + q(i+1,2));         % q left
        q(i,width) = 0.5*(q(i-1,width-1) + q(i+1,width-1));   % q right
    end

    for j=3:szx-2
        p(1,j)        = 0.5*(p(2,j)        + p(2,j+1));          % p top
        p(height,j+1) = 0.5*(p(height-1,j) + p(height-1,j+2));   % p bottom

        q(1,j)        = 0.5*(q(2,j)        + q(2,j+1));          % q top
        q(height,j+1) = 0.5*(q(height-1,j) + q(height-1,j+2));   % q bottom
    end

    % Z: Corners
    Z(1,1)        = 0.5*(Z(2,2) - p(1,1) - q(1,1) + Z(2,2) - p(2,1) - q(2,1));
    Z(2,width)  = 0.5*(Z(2,width-1)  + p(1,width-1)  - q(1,width-1)  +    Z(2,width-1)  + p(2,width-1)  - q(2,width-1));
    Z(height,1) = 0.5*(Z(height-1,2) - p(height-1,1) + q(height-1,1) + 	  Z(height-1,2) - p(height-1,2) + q(height-1,2));
    Z(height,width) = 0.5*(Z(height-1,width-1) + p(height-1,width-1) + q(height-1,width-1)  +  Z(height-1,width-1) + p(height,width) + q(height-1,width));


    Z(1,2) = 0.5*(Z(2,3) - p(1,3) - q(1,3) + Z(2,3) - p(1,2) - q(1,2));
    Z(2,1) = 0.5*(Z(3,2) - p(3,1) - q(3,1) + Z(3,2) - p(2,1) - q(2,1));

    Z(1,width-1) = 0.5*(Z(2,width-2) + p(1,width-2) - q(1,width-2) +    Z(2,width-2) + p(1,width-1) - q(1,width-1));

    Z(2,width)   = 0.5*(Z(3,width-1) + p(3,width-1) - q(3,width-2) +    Z(3,width-1) + p(2,width-1) - q(2,width-1));

    Z(height-1,2) = 0.5*(Z(height-2,2) - p(height-2,1) + q(height-2,1) + Z(height-2,2) - p(height-1,1) + q(height-1,1));

    Z(height,2) = 0.5*(Z(height-1,3) - p(height-1,3) + q(height-1,3) + Z(height-1,2) - p(height-1,2) + q(height-1,2));

    Z(height-1,width) = 0.5*(Z(height-2,width-1) + p(height-2,width-1) + q(height-2,width-1) + Z(height-2,width-1) + p(height-1,width-1) + q(height-1,width-1));

    Z(height,width-1) = 0.5*(Z(height-1,width-2) + p(height-1,width-2) + q(height-1,width-2) +  Z(height-1,width-2) + p(height,width-1) + q(height-1,width-1));


    for i=1:szy-2
        Z(i+1,1) = 0.5*(Z(i,2) - p(i,1) + q(i,1) + Z(i+2,2) - p(i+1,1) - q(i+1,1));   %left
        Z(i+1,width) = 0.5*(Z(i,width-1) + p(i,width-1) + q(i,width-1) +  Z(i+2,width-1) + p(i+1,width-1)   - q(i+1,width-1));             %right
    end

    for j=1:szx-2
        Z(1,j+1) = 0.5*(Z(2,j) - q(1,j) + p(1,j) + Z(2,j+2) - p(1,j+1) - q(1,j+1));   % top
        Z(height,j+1) = 0.5*(Z(height-1,j) + q(height-1,j) + p(height-1,j) +  Z(height-1,j+2) - p(height-1,j+1) + q(height-1,j+1));         % bottom
    end
    % Linear Adjustment of lambda
    if adjust ==1 & k <adjDuration
        lambda = (lambdaMin - lambdaMax)*k/adjDuration + lambdaMax;
        mu     = (muMin - muMax)*k/adjDuration + muMax;
    end

    % Z <- Zold , p <- pOld , q <- qOld
    for i=1:szy,
        for j=1:szx
            ZOld(i,j) = Z(i,j);
            pOld(i,j) = p(i,j);
            qOld(i,j) = q(i,j);
        end
    end
    disp('Current Iteration : ' );
    disp(k);
end



function [reflect] = R(p,q,thres,ps,qs,src_norm)
%src_norm = (1 + ps*ps + qs*qs);
reflect = (1-p*ps-q*qs)/(sqrt(1 + p^2+q^2)*src_norm);
if thres==1
    reflect = max(0.0, reflect);
else
    reflect = reflect;
end

function [norm] = Rq(p,q,ps,qs,src_norm)
pq = 1+p*p + q*q;
%src_norm = (1 + ps*ps + qs*qs);
norm = -qs/(sqrt(pq)*src_norm) - (1.0 - ps*p - qs*q)*q/(sqrt(pq*pq*pq)*src_norm);

function [norm] = Rp(p,q,ps,qs,src_norm)
pq = 1+p*p + q*q;
%src_norm = (1 + ps*ps + qs*qs);
norm = (-ps/(sqrt(pq)*src_norm) - (1.0 - ps*p - qs*q)*p/(sqrt(pq*pq*pq)*src_norm));