(* The number of points used to mark the axes along the walls. *)
numPoints = 9

Clear[RWC,finalrot,rot,finalfocal,cam,p,q,nn,minmax]
(* Real World Coordinates of the points on the walls, in inches *)
RWC = {{63.75,0,82},{0,0,82},{0,50.25,82},{63.75,0,37},{0,0,37},{0,50.25,37},{63.75,0,0},{0,0,0},{0,50.25,0}};
(* Each row is a camera's view of each point above, in pixels. *)
(* First row is Right camera, second row is Center, third row is Left. *)
cam = {{{139,50},{405,54},{456,37},{142,226},{402,222},{451,251},{144,360},{401,354},{447,414}},
 {{198,98},{388,105},{526,97},{202,265},{388,251},{524,261},{205,393},{387,357},{522,388}},
{{127,43},{180,83},{369,85},{127,254},{180,239},{369,242},{129,421},{178,363},{368,367}}};
 Do [
Do [
(* Moves the origin of the camera coordinates from the top left [of the "image"] *)
(*   to the center, and flips the y coordinates so 'up' is positive. *)
cam[[kk]][[i]] = {cam[[kk]][[i]][[1]]-360,240-cam[[kk]][[i]][[2]],0},{i,1,numPoints}],
{kk,1,3}]

(* 'p' and 'q' pair up points along each axis. e.g. (p[[1]][[1]], q[[1]][[1]]) *)
(* pairs points 1 and 2 (along the x-axis) for the coordinates above. *)
(* Lists are: x-axis, y-axis, z-axis. 'nn' is the number of vectors along in each axis, corresponding to the number of items in p and q lists. *)
p = {{1,4,7},{2,5,8},{1,4,1,2,2,5,3,3,6}};
q = {{2,5,8},{3,6,9},{4,7,7,5,8,8,6,9,9}};
nn = {3,3,9};
rot = {0,0,0};
finalrot= {0,0,0};

(* Determine the focal length iteratively. This value is very precise, changing it by .5 can cause ~18" of error to the guessed coordinates. *)
focal =978

(* Append the focal length to each pixel coordinate *)
Do[ Do [cam[[kk]][[i]][[3]] =focal,{i,1,numPoints}],{kk,1,3}];

Do [
    
     rc= {};  
Do [         
     a2sum = 0;
     b2sum= 0;
     c2sum = 0;
     absum = 0;
     acsum = 0;
     bcsum = 0;
(* Pairing up points for each camera, using 'p' and 'q', above, cross each pair to get a normal vector. *)
     Do [v1 =cam[[kk]][[p[[jj]][[i]]]];
            v2 = cam[[kk]][[q[[jj]][[i]]]];                    
            norm= N[Cross[v1,v2]];
            norm = Normalize[norm];          
            a2sum = a2sum + norm[[1]]^2;
            b2sum = b2sum + norm[[2]]^2;
            c2sum = c2sum + norm[[3]]^2;
            absum = absum + 2*norm[[1]]*norm[[2]];
            acsum = acsum + 2*norm[[1]]*norm[[3]];
            bcsum = bcsum + 2*norm[[2]]*norm[[3]],
{i,1,nn[[jj]]}];
(* Perform least squares minimization by creating a symmetric matrix using the normal vectors. *) 
    mm = {{a2sum,absum/2,acsum/2},{absum/2,b2sum,bcsum/2},{acsum/2,bcsum/2,c2sum}};
    eg = Eigensystem[mm];   
(* Create a rotation matrix from the eigenvectors corresponding with the smallest eigenvalues [of each symmetric matrix above, one for each camera] *)
    rc = Append[rc,eg[[2]][[3]]]
,{jj,1,3}];
       rr = Transpose[rc];     

       (* The following code corrects an "almost orthogoanl matrix into an exact one *) 
       (* by turning the rotation matrix into a quaternion and finding LSV *)qq = {{rr[[1]][[1]] + rr[[2]][[2]]+rr[[3]][[3]],rr[[3]][[2]]-rr[[2]][[3]],rr[[1]][[3]]-rr[[3]][[1]],rr[[2]][[1]]-rr[[1]][[2]]},
                 {rr[[3]][[2]]-rr[[2]][[3]],rr[[1]][[1]] - rr[[2]][[2]]-rr[[3]][[3]],rr[[2]][[1]]+rr[[1]][[2]],rr[[3]][[1]]+rr[[1]][[3]]},
                 {rr[[1]][[3]]-rr[[3]][[1]],rr[[2]][[1]]+rr[[1]][[2]],-rr[[1]][[1]] + rr[[2]][[2]]-rr[[3]][[3]],rr[[3]][[2]]+rr[[2]][[3]]},
                 {rr[[2]][[1]]-rr[[1]][[2]],rr[[3]][[1]]+rr[[1]][[3]],rr[[3]][[2]]+rr[[2]][[3]],-rr[[1]][[1]] - rr[[2]][[2]]+rr[[3]][[3]]}};
        ss = Eigensystem[qq];
        ev = ss[[2]][[1]];
        w= ev[[1]];
        x= ev[[2]];
        y = ev[[3]];
        z = ev[[4]];
       newr = {{w*w+x*x-y*y-z*z,2*x*y-2*w*z,2*x*z+2*w*y},{2*x*y+2*w*z,w*w+y*y-x*x-z*z,2*y*z-2*w*x},    {2*x*z-2*w*y,2*y*z+2*w*x,w*w+z*z-x*x-y*y}};
       (* This ends the orthogonal correction code. *)           

       rot[[kk]] = rr;
       finalrot[[kk]]   = newr;
       
        (* Fix signs in matrix. *)
       If [finalrot[[kk]][[1]][[1]]>0,Do [finalrot[[kk]][[i]][[1]] = - finalrot[[kk]][[i]][[1]],{i,1,3}]];
       If [finalrot[[kk]][[1]][[2]]<0,Do [finalrot[[kk]][[i]][[2]] = - finalrot[[kk]][[i]][[2]],{i,1,3}]];
       If [finalrot[[kk]][[2]][[3]]<0,Do [finalrot[[kk]][[i]][[3]] = - finalrot[[kk]][[i]][[3]],{i,1,3}]];                     
,{kk,1,3}];
     
     
Print["Focal length = ",focal];
Do [ Print["Cam ",kk," Computed R = " ,MatrixForm[rot[[kk]]],"  Corrected R= ",MatrixForm[finalrot[[kk]]]],{kk,1,3}];

(* Now compute camera translations *)
trans = {0,0,0};
Do [ (* for each camera kk*)
fsum = 0;
Do [ 
   Do [
           v1 = cam[[kk]][[ss]];          
           v2 = cam[[kk]][[tt]];
           norm =N[ Cross[v1,v2]];
           norm = Normalize[norm];
	(* For every pair of points, calculate the world coordinate vector between them, *)
	(*   and sum the squares of all such vectors. *)
	(* {t1, t2, t3} is the unknown translation vector [one for each camera] to be calculated below. *)
          fsum  = fsum + (norm.(finalrot[[kk]].({t1,t2,t3}-RWC[[ss]])))^2;
          fsum  = fsum + (norm.(finalrot[[kk]].({t1,t2,t3}-RWC[[tt]])))^2;          
          ,{ss,tt+1,numPoints}],{tt,1,numPoints}];

ff[t1_,t2_,t3_] = fsum;
eq1[t1_,t2_,t3_] = D[ff[t1,t2,t3],t1];  (* The 3 partial derivative *)
eq2[t1_,t2_,t3_] = D[ff[t1,t2,t3],t2];  (* set = 0 to find minimium *)
eq3[t1_,t2_,t3_] = D[ff[t1,t2,t3],t3];
b1 = -eq1[0,0,0];
b2 = -eq2[0,0,0];                                        
b3 = -eq3[0,0,0]; 
a11 = Coefficient[eq1[t1,t2,t3],t1];
a12 = Coefficient[eq1[t1,t2,t3],t2];
a13 = Coefficient[eq1[t1,t2,t3],t3];

a21 = Coefficient[eq2[t1,t2,t3],t1];
a22 = Coefficient[eq2[t1,t2,t3],t2];
a23 = Coefficient[eq2[t1,t2,t3],t3];

a31 = Coefficient[eq3[t1,t2,t3],t1];
a32 = Coefficient[eq3[t1,t2,t3],t2];
a33 = Coefficient[eq3[t1,t2,t3],t3];

(* Solving system of three equations with three unknowns. *)

AA= {{a11,a12,a13},{a21,a22,a23},{a31,a32,a33}};
BB= {b1,b2,b3};

trans[[kk]] = Inverse[AA].BB ;

,{kk,1,3}];

Do [ Print["Camera ",kk," Translation = ",trans[[kk]]],{kk,1,3}];

   (* Now check results agains known RWC *)
xx= {0,0,0};
vv = {0,0,0};
sumsq=0;
Do [
Do [  
       xx[[kk]] = N[Transpose[finalrot[[kk]]].cam[[kk]][[i]]];
        xx[[kk]]= Normalize[xx[[kk]]],{kk,1,3}];

(* Very similar  to above, but now {t1, t2, t3} are known for each camera, so no need to sum the squares of the results to over-constrain. *)
 vv[[1]] = trans[[1]]-trans[[2]] + t1*xx[[1]] - t2*xx[[2]];  
 vv[[2]] = trans[[1]]-trans[[3]] + t1*xx[[1]] - t2*xx[[3]];
 vv[[3]] = trans[[2]]-trans[[3]] + t2*xx[[2]] - t3*xx[[3]];
ff[t1_,t2_,t3_] = vv[[1]].vv[[1]]+ vv[[2]].vv[[2]]+ vv[[3]].vv[[3]];
eq1[t1_,t2_,t3_] = D[ff[t1,t2,t3],t1];  (* The 3 partial derivative *)
eq2[t1_,t2_,t3_] = D[ff[t1,t2,t3],t2];  (* set = 0 to find minimium *)
eq3[t1_,t2_,t3_] = D[ff[t1,t2,t3],t3];
b1 = -eq1[0,0,0];
b2 = -eq2[0,0,0];                                        
b3 = -eq3[0,0,0]; 
a11 = Coefficient[eq1[t1,t2,t3],t1];
a12 = Coefficient[eq1[t1,t2,t3],t2];
a13 = Coefficient[eq1[t1,t2,t3],t3];

a21 = Coefficient[eq2[t1,t2,t3],t1];
a22 = Coefficient[eq2[t1,t2,t3],t2];
a23 = Coefficient[eq2[t1,t2,t3],t3];

a31 = Coefficient[eq3[t1,t2,t3],t1];
a32 = Coefficient[eq3[t1,t2,t3],t2];
a33 = Coefficient[eq3[t1,t2,t3],t3];

AA= {{a11,a12,a13},{a21,a22,a23},{a31,a32,a33}};
BB= {b1,b2,b3};


tt = Inverse[AA].BB ;
qq = {trans[[1]] + tt[[1]]*xx[[1]],trans[[2]] + tt[[2]]*xx[[2]],trans[[3]] + tt[[3]]*xx[[3]]};
(* Each camera "reports" a slightly different location for
the point (along its vector) so average the results *)
avg = (qq[[1]]+qq[[2]]+qq[[3]])/3.0;
dif = RWC[[i]]-avg;
sq = dif.dif;
sumsq = sumsq+ sq;
Print["Measured = ",RWC[[i]]," Computed = ", avg];

,{i,1,numPoints}];

finalrot // MatrixForm

trans // MatrixForm

trans[[1]]