(* 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 *)