(* simulation of the reconstruction problem with spherical camera  and moving bodies                                *)
(* Cameras observe moving bodies. The reconstruction finds the cameras as well as the body motion                   *)
(* This is an example, where the point manifold is a 6 dimensional space                                            *)
(* Having the possibility to have the point manifold to be a jet of functions justifies our relatively abstract     *)
(* setup for the structure from motion problem                                                                      *)
(* The retinal surface is no more a hypersurface. The camera Q is a map from R^6 to R^2, satisfying Q^2=Q.          *)

(* This is a Mathematica test program to demonstrate the  reconstruction when particles move                        *)
(* 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     *)

(* May 28, 2007, Oliver Knill                                                                                       *)
(* see the paper with Jose Ramirez in  http://arxiv.org/abs/0708.2438                                               *)



ep := 0.01*(Random[]-0.5); eps:={ep,ep,ep};                                            (* error                     *)
P={{0,0,0},{1,0,0},{0,2,3},{1.4,1.8,2},{2,2,3},{2,1,2},{1.4,2,3},{2,1,3}};             (* world points              *)
Pv={{0,0,0},{.2,0,0},{0,.3,0},{0.2,-.1,0},{.5,.2,0},{0,0,0},{1,0,0},{.3,.12,.1}};      (* world point velocities    *)
r[t_]:= {0.3+Cos[t],0.4+Sin[t], Sin[2 t]};                                             (* camera path               *)
T={0.00,0.1,0.15,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.85,0.9,0.95,0.98}*2Pi;                  (* observation times         *)
Q=Table[r[T[[k]]],{k,Length[T]}];                                                      (* camera path 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[Pv[[i,1]],{i,2,m}],
                          Table[P[[i,2]],{i,2,m}],
                          Table[Pv[[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}],
                         Table[Pv[[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]]+T[[j]]*Pv[[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]]+T[[j]]*Pv[[i,1]]-Q[[j,1]])^2 
                + (P[[i,2]]+T[[j]]*Pv[[i,2]]-Q[[j,2]])^2]],{i,m},{j,n}];               (* distances                 *)
ZZ = Table[P[[i,3]]+T[[j]]*Pv[[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      *) 
PPv=Table[{xv[i],yv[i],zv[i]},{i,m}];                                                  (* variables for velocities  *)                         
QQ=Table[{a[j],b[j],c[j]},{j,n}];                                                      (* variables for camera      *)
Eq=Flatten[Table[OC[[i,j]](PP[[i,2]]+T[[j]]*PPv[[i,2]]-QQ[[j,2]])
                -OS[[i,j]](PP[[i,1]]+T[[j]]*PPv[[i,1]]-QQ[[j,1]]),{i,m},{j,n}]];       (* the equations             *)
Va=Union[Flatten[Table[{x[i],y[i],xv[i],yv[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]       *)
xv1pos=Position[Va,xv[1]][[1,1]];                                                      (* row containing xv[1]      *)
yv1pos=Position[Va,yv[1]][[1,1]];                                                      (* row containing xv[1]      *)
bbb=-AT[[x2pos]];                                                                      (* x2=1 gives inhomog. part  *)
ss=Table[k,{k,nn}];                                                                    (* list of rows              *)
ss1=Complement[ss,{x1pos,x2pos,y1pos,xv1pos,yv1pos}];                                  (* x1=y1=xv1=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+m-2}]}];                    (* x-position of world       *)

xxv=Flatten[{{0},Table[ComputedPlanarData[[j]],{j,2n+ m-1,2n+2m-3}]}];                 (* x-velocity of world       *)
yy=Flatten[{{0},Table[ComputedPlanarData[[j]],{j,2n+2m-2,2n+3m-4}]}];                  (* y-position of world       *)
yyv=Flatten[{{0},Table[ComputedPlanarData[[j]],{j,2n+3m-3,2n+4m-5}]}];                 (* y-velocity of world       *)
rr=Table[N[Sqrt[(xx[[i]]+T[[j]]*xxv[[i]] -aa[[j]])^2 
              + (yy[[i]]+T[[j]]*yyv[[i]] -bb[[j]])^2]],{i,m},{j,n}];                   (* distances                 *)
Va1=Union[Flatten[Table[{z[i],zv[i],c[j]},{i,m},{j,n}]]];                              (* all variables as 1 vector *)                        
Eq1=Flatten[Table[O1C[[i,j]](PP[[i,3]]+T[[j]]*PPv[[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]       *)
zv1pos=Position[Va1,zv[1]][[1,1]];                                                     (* row containing zv[1]      *)
ss=Table[k,{k,nn1}];                                                                   (* list of rows              *)
ss1=Complement[ss,{z1pos,zv1pos}];                                                     (* 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             *)

