Optimizing using Fsolve (more equations than unknowns)



Good day

I?ve fiddled for ~ 2 weeks plugging away using fsolve and im having
difficulty looping and solving my equations for n points per
equation.

Problem: 34 equations with 33 unknowns using Fsolve +
Levenberg-Marquardt.

Desired Optimization variables: 3D world points and 6 camera camera
parameters

Given: Eight 2D images and n= 100 known points (x,y) per image.

Six camera parameters remain constant in the eight images:
PA,SA,IS,SOD,SID,IS.

Input of those 6 camera parameters to function file outputs
Projection matrix, P, 3x4 matrix

P= [m1 m2 m3 m4
m5 m6 m7 m8
m9 m10 m11 m12]

For Image 0: 2D points (x0,y0) to 3D points (X0,Y0,Z0) relationship
yields:

x0 = m1*X0 + m2*Y0 + m3*Z0 + m4
-------------------------------
m9*X0 + m10*Y0 + m11*Z0 + m12

y0 = m5*X0 + m6*Y0 + m7*Z0 + m8
---------------------------------
m9*X0 + m10*Y0 + m11*Z0 + m12

For Image 1: 2D points (x1,y1) to 3D points (X1,Y1,Z1) relationship
yields:

x1 = m1*X1 + m2*Y1 + m3*Z1 + m4
------------------------------
m9*X1 + m10*Y1 + m11*Z1 + m12

y1 = m5*X1 + m6*Y1 + m7*Z1 + m8
--------------------------------
m9*X1 + m10*Y1 + m11*Z1 + m12

For both images we have 2 additional equations:

(x1-x0) = m1*X1 + m2*Y1 + m3*Z1 + m4
------------------------------- -
m9*X1 + m10*Y1 + m11*Z1 + m12

m1*X0 + m2*Y0 + m3*Z0 + m4
-------------------------------
m9*X0 + m10*Y0 + m11*Z0 + m12

(y1-y0) = m5*X1 + m6*Y1 + m7*Z1 + m8
------------------------------- -
m9*X1 + m10*Y1 + m11*Z1 + m12

m5*X0 + m6*Y0 + m7*Z0 + m8
---------------------------------
m9*X0 + m10*Y0 + m11*Z0 + m12

For the two above images we have six above equations and 12 unknowns
(X0,Y0,Z0,X1,Y1,Z1, + 6 camera parameters). From above, all equations
can be written equal to 0.

In total we will have 34 similar equations and 33 unknowns (
X0,Y0,Z0,?X7,Y8,Z8 + 6 camera parameters) The 34 equations are valid
for a single 2D point in each image. Since we have n= 100 points per
image we have n*34 equations with n*33unknowns.

Here is my code in the function command:

function F = TestEQNS(Initial, TwoDpts, N)

%Where Initial is 36x100 matrix with last column containing %6
parameters of camera. [X0,Y0,Z0,1,X1,Y1,Z1,1,...]

%TwoDpts is a 18x100 matrix %containing 2D image points %columns
(x0,y0, x1,y1?..x8,y8)

%N= number of points per image to analyze

%Get 3x4 projection matrix from another function

ProjMaterror= ProjectionMatrix(Initial(4,end), Initial(4,end),
Initial(1,end), Initial(2,end), Initial(5,end), Initial(3,end),
Initial(6,end));

m= reshape(ProjMaterror',12,1);

for ctr= 1: N

% Get 3D points
X0= Initial(ctr,1); Y0= Initial(ctr,2); Z0= Initial(ctr,3);
X1= Initial(ctr,5); Y1= Initial(ctr,6); Z1= Initial(ctr,7);
X2= Initial(ctr,9); Y2= Initial(ctr,10);Z2= nitial(ctr,11);
X3= Initial(ctr,13);Y3= Initial(ctr,14);Z3=Initial(ctr,15);
X4= Initial(ctr,17);Y4= Initial(ctr,18);Z4=Initial(ctr,19);
X5= Initial(ctr,21);Y5= Initial(ctr,22);Z5=Initial(ctr,23);
X6= Initial(ctr,25);Y6= Initial(ctr,26);Z6=Initial(ctr,27);
X7= Initial(ctr,29);Y7= Initial(ctr,30);Z7=Initial(ctr,31);
X8= Initial(ctr,33);Y8= Initial(ctr,34);Z8=Initial(ctr,35);

% Get 2D points
x0= TwoDpts(ctr,1); y0= TwoDpts(ctr,2);
x1= TwoDpts(ctr,3); y1= TwoDpts(ctr,4);
x2= TwoDpts(ctr,5); y2= TwoDpts(ctr,6);
x3= TwoDpts(ctr,7); y3= TwoDpts(ctr,8);
x4= TwoDpts(ctr,9); y4= TwoDpts(ctr,10);
x5= TwoDpts(ctr,11); y5= TwoDpts(ctr,12);
x6= TwoDpts(ctr,13); y6= TwoDpts(ctr,14);
x7= TwoDpts(ctr,15); y7= TwoDpts(ctr,16);
x8= TwoDpts(ctr,17); y8= TwoDpts(ctr,18);

F(:,ctr)=

[m(1)*X0 + m(2)*Y0 + m(3)*Z0 + m(4) - x0*(m(9)*X0 + m(10)*Y0 +
m(11)*Z0 + m(12));
m(1)*X1 + m(2)*Y1 + m(3)*Z1 + m(4) - x1*(m(9)*X1 + m(10)*Y1 +
m(11)*Z1
.