eh = 2.5;
ew = 3;
ed = 2.5;

cubeMat = [0 ew 0  0  ew 0; 0 0  0  eh eh eh; 0 0  ed 0  0  ed; 1 1  1  1  1  1];

%pixelMat = [545 1687; 821 1641; 290 1652; 548 1329; 823 1302; 292 1310]';
%gridPixMat = [557 1669; 658 1659; 754 1649; 557 1525; 658 1518; 744 1511; 483 1657; 480 1526]';
%gridOrigMat = [0 0; 10 0; 20 0; 0 10; 10 10; 20 10; -2 1; -2 10]';
%gridOrigMat = [0 0; 10 0; 20 0; 0 10; 10 10; 20 10]';

pixelMat = [564 1333; 759 1308; 285 1320; 568 1004; 761 998; 290 996]';
gridPixMat = [575 1313; 667 1314; 576 1161; 668 1162; 469 1319; 468 1165]';
gridOrigMat = [0 0; 10 0; 0 10; 10 10; -5 0; -5 10]';

cameraMat = pixelMat * pinv(cubeMat);

gridMat = zeros(4, 6);
pixH = pixelMat(2, 2) - pixelMat(2, 1);
for i = 1:4
  pos = (gridPixMat(1, i) - pixelMat(1, 1)) / (pixelMat(1, 2) - pixelMat(1,1));
  gridMat(1, i) = ew * pos;
  gridMat(2, i) = -(gridPixMat(2, i) - pixH * pos - pixelMat(2, 1)) / (pixelMat(2, 1) - pixelMat(2, 4))*eh;
  gridMat(3, i) = 0;
  gridMat(4, i) = 1;
end;
pixH = pixelMat(2, 3) - pixelMat(2, 1);
for i = 5:6
  pos = 1.0-(gridPixMat(1, i) - pixelMat(1, 3)) / (pixelMat(1, 1) - pixelMat(1, 3));
  gridMat(1, i) = 0;
  gridMat(2, i) = -(gridPixMat(2, i) - pixH * pos - pixelMat(2, 1)) / (pixelMat(2, 1) - pixelMat(2, 4))*eh;
  gridMat(3, i) = ed * pos;
  gridMat(4, i) = 1;
end;

projMat = gridOrigMat * pinv(gridMat);

% uberMat = inv([cameraMat;projMat]);
M = [cameraMat;projMat];
A = M(1:4,1:3);
uberMat = inv(A'*A) * A';
B = M(1:4,4);

projX = projMat(1,1:3)';
projX = projX / norm(projX);
projY = projMat(2,1:3)';
projY = projY / norm(projY);
projNorm = cross(projX, projY);

uberMat
B
projNorm
norm(projMat(1,1:3)')
projX
projY