Lucas-Kanade method on julia

lucasKanade.jl

using Images
# FFTW.set_num_threads(2)
"""
opticalFlow(im1::Array, im2::Array, kps::Array{CartesianIndex{2},1}; windowSize=15)
Parameters:
 - im1: An Image array, the first frame in time.
 - im2: The second frame in time, must be the same size as im1
 - kps: Key points, or points of interest within the image where the optical flow is expected.
 - windowSize: Window size for solving the flow equations. Default 15.
"""
function opticalFlowLK(im1::Array, im2::Array, kps::Array{CartesianIndex{2},1}; windowSize=15)
    h,w = size(im1)
    im1 = Array{Float64,2}(Gray.(im1))
    im2 = Array{Float64,2}(Gray.(im2))

    # Kernels
    kx = Array{Float64}(.25 .* [-1  1; -1 1]);
    ky = Array{Float64}(.25 .* [-1 -1;  1 1]);
    kt = Array{Float64}(.25 .* [ 1  1;  1 1]);

    # Derivatives on x, y and time
    imx = (conv2(kx, im1) +
           conv2(kx, im2))[2:end, 2:end];
    imy = (conv2(ky, im1) +
           conv2(kx, im1))[2:end, 2:end];
    imt = (conv2(-kt, im2) +
           conv2(-kt, im1))[2:end, 2:end];

    sz = size(im1);
    hitMap = zeros(sz);
    u = [];
    v = [];
    hsz = floor(windowSize/2);

    X2 = Array{Float64,2}(imx.^2);
    Y2 = Array{Float64,2}(imy.^2);
    XY = Array{Float64,2}(imx.*imy);
    XT = Array{Float64,2}(imx.*imt);
    YT = Array{Float64,2}(imy.*imt);

    for kp in kps
        i,j = kp[1],kp[2]
        left = j-hsz; right = j+hsz;
        top = i-hsz; bottom = i+hsz;

        if (left<=0);  left   = 1; end
        if (right>h);  right  = h; end
        if (top<=0);   top    = 1; end
        if (bottom>w); bottom = w; end

        ws = (right-left+1)*(bottom-top+1);

        A = [X2[i,j] XY[i,j];
             XY[i,j] Y2[i,j]]/ws;

        B = [XT[i,j];
             YT[i,j]]/ws;

        hitMap[i,j] = 1;
        U = pinv(A)*B;
        push!(u, U[1]);
        push!(v, U[2]);
    end
    return u, v, hitMap
end