(* Simulation of the reconstruction problem with spherical cameras                                                  *)
(* This is a Mathematica test program to measure the error of the reconstruction                                    *)
(* We take MMM random points, and NNN random cameras in space. First, the pragram takes pictures from each camera   *)
(* then addes an error to the observed picture points, then does the reconstruction and finds the maximal error     *)

(* April 10, 2007 - July 13, 2007, Oliver Knill                                                                     *)
(* see the paper with Jose Ramirez in  http://arxiv.org/abs/0708.2438                                               *)

(* MMM=100; NNN=100;   numberofruns=100; numberofepsilons=50; epsilonmin=0.0; epsilonmax=0.1; *)     (* longer run  *)
MMM=10; NNN=10;   numberofruns=10; numberofepsilons=5; epsilonmin=0.0; epsilonmax=0.1; (* demo                      *)

F[epsilon_]:=Module[{},
R:=3 Random[]-1;
P=Table[{R,R,R},{i,MMM}];                                                              (* world points              *)
P=Prepend[P,{1,1,1}]; P=Prepend[P,{0,0,0}];                                            (* 2 pt freez (transl,scale) *)
ep := epsilon*(Random[]-0.5); eps:={ep,ep,ep};                                         (* error                     *)
Q=Table[{R,R,R},{i,NNN}];                                                              (* world points              *)
m=Length[P];  n=Length[Q];                                                             (* number of points and obser*)
ActualPlanarData=Flatten[{Table[Q[[j,1]],{j,n}],Table[Q[[j,2]],{j,n}],
                          Table[P[[i,1]],{i,3,m}],Table[P[[i,2]],{i,2,m}]}];           (* write planar data as vect *) 
ActualSpaceData=Flatten[{Table[Q[[j,3]],{j,n}],Table[P[[i,3]],{i,2,m}]}];              (* write space  data as vect *)
L[Aa_,Bb_]:= Arg[(Aa[[1]]-Bb[[1]]) + I (Aa[[2]]-Bb[[2]])];                             (* theta angle               *)
Obs= Table[L[P[[i]]+eps,Q[[j]]+eps],{i,m},{j,n}];                                      (* list of angles            *)
OC=  Table[Cos[Obs[[i,j]]],{i,m},{j,n}];                                               (* cos(theta)                *)
OS=  Table[Sin[Obs[[i,j]]],{i,m},{j,n}];                                               (* sin(theta)                *)
RR = Table[N[Sqrt[(P[[i,1]]-Q[[j,1]])^2 + (P[[i,2]]-Q[[j,2]])^2]],{i,m},{j,n}];        (* distances                 *)
ZZ = Table[P[[i,3]]-Q[[j,3]],{i,m},{j,n}];                                             (* z coordinates of world    *)
Obs1=Table[Arg[I ZZ[[i,j]] + RR[[i,j]]],{i,m},{j,n}];                                  (* list of vertical slopes   *)
O1C =Table[Cos[Obs1[[i,j]]],{i,m},{j,n}];                                              (* cos(phi)                  *)
O1S =Table[Sin[Obs1[[i,j]]],{i,m},{j,n}];                                              (* sin(phi)                  *)
(* Obs and Obs1 are the only things which are needed from now on. This is where the 2d reconstruction starts        *)
PP=Table[{x[i],y[i],z[i]},{i,m}];                                                      (* variables for points      *)                         
QQ=Table[{a[j],b[j],c[j]},{j,n}];                                                      (* variables for camera      *)
Eq=Flatten[Table[OC[[i,j]](PP[[i,2]]-QQ[[j,2]])-OS[[i,j]](PP[[i,1]]-QQ[[j,1]]),{i,m},{j,n}]];  (* the equations     *)
Va=Union[Flatten[Table[{x[i],y[i],a[j],b[j]},{i,m},{j,n}]]];                           (* all variables as 1 vector *)                        
mm=Length[Eq];  nn=Length[Va];                                                         (* number of variables       *)
A=Table[D[Eq[[i]],Va[[j]]],{i,mm},{j,nn}];                                             (* matrix of the system      *)
AT=Transpose[A];                                                                       (* its transpose for columns *)
x1pos=Position[Va,x[1]][[1,1]];                                                        (* row containing x[1]       *)
x2pos=Position[Va,x[2]][[1,1]];                                                        (* row containing x[2]       *)
y1pos=Position[Va,y[1]][[1,1]];                                                        (* row containing y[1]       *)
bbb=-AT[[x2pos]];                                                                      (* x2=1 gives inhomog. part  *)
ss=Table[k,{k,nn}];                                                                    (* list of rows              *)
ss1=Complement[ss,{x1pos,x2pos,y1pos}];                                                (* normalize x1=y1=z1=0,x2=1 *)
AT1=Table[AT[[ss1[[k]]]],{k,Length[ss1]}];                                             (* Without x1,x2,y1 row      *)
A1=Transpose[AT1];                                                                     (* Without x1,x2,y1 column   *)
ComputedPlanarData=Inverse[AT1.A1].AT1.bbb;                                            (* A x = b by (A^TA)^-1 A^Tb *)
Max[Abs[ActualPlanarData-ComputedPlanarData]];                                         (* maximal error             *)
(* Now we have the (x_i,y_i,a_i,b_i) coordinates. Now we start the 3d reconstruction                                *)
aa=Table[ComputedPlanarData[[j]],{j,n}];                                               (* x-positions camera points *)
bb=Table[ComputedPlanarData[[j]],{j,n+1,2n}];                                          (* y-positions camera points *)
xx=Flatten[{{0,1},Table[ComputedPlanarData[[j]],{j,2n+1,2n+1+m-3}]}];                  (* x-position of world       *)
yy=Flatten[{{0},Table[ComputedPlanarData[[j]],{j,2n+m-1,2n+2m-3}]}];                   (* y-position of world       *)
rr=Table[N[Sqrt[(xx[[i]]-aa[[j]])^2 + (yy[[i]]-bb[[j]])^2]],{i,m},{j,n}];              (* distances                 *)
Va1=Union[Flatten[Table[{z[i],c[j]},{i,m},{j,n}]]];                                    (* all variables as 1 vector *)                        
Eq1=Flatten[Table[O1C[[i,j]](PP[[i,3]]-QQ[[j,3]])-O1S[[i,j]] rr[[i,j]],{i,m},{j,n}]];  (* the equations             *)
mm1=Length[Eq1];  nn1=Length[Va1];                                                     (* number of eq and variab   *)
A =Table[D[Eq1[[i]],Va1[[j]]],{i,mm1},{j,nn1}];                                        (* matrix of the system      *)
AT=Transpose[A];                                                                       (* its transpose for columns *)
z1pos=Position[Va1,z[1]][[1,1]];                                                       (* row containing z[1]       *)
ss=Table[k,{k,nn1}];                                                                   (* list of rows              *)
ss1=Complement[ss,{z1pos}];                                                            (* normalize z1=0            *)
AT1=Table[AT[[ss1[[k]]]],{k,Length[ss1]}];                                             (* Without z1 row            *)
A1=Transpose[AT1];                                                                     (* Without z1 column         *)
b1 = -Table[Eq1[[i]]-Sum[A[[i,j]] Va1[[j]],{j,Length[Va1]}],{i,Length[Eq1]}];          (* the inhomogenous part     *)
b1 = Chop[Simplify[b1]];                                                               (* remove                    *)
ComputedSpaceData=Inverse[AT1.A1].AT1.b1;                                              (* A x = b by (A^TA)^-1 A^Tb *)
UU=Max[Abs[ActualSpaceData-ComputedSpaceData]];                                        (* maximal error             *)
{epsilon,UU}
]
statistics={};
Do[Do[uu=F[epsilonmin+(kkk/numberofepsilons)*(epsilonmax-epsilonmin)]; 
      statistics=Append[statistics,uu],{numberofruns}]; Print[{kkk,numberofepsilons}],{kkk,1,numberofepsilons}];
Save["statistics.data",statistics];
