function [mm] = gmeLsme(coorX, coorY, npx, npy, iMap, iniH)

% one step to further remove outliers according to last least square
% estimate

if ~isempty(iniH)
    
    [mmpx, mmpy] = mvGen_f(iniH, coorX, coorY); % SNR check-up
    
    C = 2;
    
%     ee = abs(mmpx-npx)+abs(mmpy-npy);
%     cmee = mean(ee(:))*C;
%     eW = 1-(ee./cmee).^2;
%     eW(eW<0) = 0;
%     eW = eW.^2;

    ee = abs(mmpx-npx)+abs(mmpy-npy);
    cmee = mean(ee(:))*C;
%     cmee = mean(ee(iMap>0))*C;
    eW = 1-(ee./cmee).^2;
    eW(eW<0) = 0;
    ty1W = eW; % just for testing
%     colorSegMap(ty1W, 1-iMap, 16);
    eW = eW .* iMap;
    eW = eW.^2;
else
%     eW = ones(size(coorX));
    eW = iMap;
end

rcoorX = coorX + npx;
rcoorY = coorY + npy;

[mm] = inhomo_ls(rcoorX(:), rcoorY(:), coorX(:), coorY(:), eW(:));


function [H] = inhomo_ls(rcoorX, rcoorY, coorX, coorY, eW)

    x1 = [coorX'; coorY'; ones(1,length(coorX))];
    x2 = [rcoorX'; rcoorY'];  
    
    V = x2(:);

    Npts = length(x1);
    A = zeros(2*Npts,8);
    A(1:2:end, 1:3) = x1';
    A(2:2:end, 4:6) = x1';
    A(1:2:end, 7) = -rcoorX.*coorX;
    A(2:2:end, 7) = -rcoorY.*coorX;
    A(1:2:end, 8) = -rcoorX.*coorY;
    A(2:2:end, 8) = -rcoorY.*coorY;
    
    w = [eW';eW'];
    rM=find(w(:)>0);
    diagW = diag(w(rM));
    
    AW = A(rM,:)'*diagW;
    
%     AWA = A'*diagW*A;
    AWA = AW*A(rM,:);
    H = AWA\AW*V(rM);
    

function colorSegMap(t1Map, t2Map, ext)
[m, n] = size(t1Map);
d1 = ones(m, n).*255; % three color R, G, B,
d2 = ones(m, n).*255; % three color R, G, B,
d3 = ones(m, n).*255; % three color R, G, B,

id = find(t1Map==0);
d1(id) = 0;
d2(id) = 0;
id = find(t2Map==1);
d1(id) = 0;
d3(id) = 0;

RGB(:,:,1) = kron(d1, ones(ext));
RGB(:,:,2) = kron(d2, ones(ext));
RGB(:,:,3) = kron(d3, ones(ext));

figure, imshow(uint8(RGB));
