I am currently trying to solve a non-linear system with 64 equations and 64 variables. The underlying problem is the relative orientation of a set of 16 points that are seen from two different stereoscopic images. Basically I'm trying to obtain 3d coordinates from points that are matched in two images A and B.
The equations that I'm trying to minimize are derived from the central projection transform and are as follows: $$ F_1 = a_{A1}X^1+a_{A2}Y^1+a_{A3}Z^1+a_{A4} - \xi^1_A·(b_{A1}X^1+b_{A2}Y^1+b_{A3}Z^1+1) $$ $$ F_2 = a_{A5}X^1+a_{A6}Y^1+a_{A7}Z^1+a_{A8} - \eta^1_A·(b_{A1}X^1+b_{A2}Y^1+b_{A3}Z^1+1) $$ $$ F_3 = a_{B1}X^1+a_{B2}Y^1+a_{B3}Z^1+a_{B4} - \xi^1_B·(b_{B1}X^1+b_{B2}Y^1+b_{B3}Z^1+1) $$ $$ F_4 = a_{B5}X^1+a_{B6}Y^1+a_{A7}Z^1+a_{A8} - \eta^1_B·(b_{B1}X^1+b_{B2}Y^1+b_{B3}Z^1+1) $$
- Superscripts reference the point number (from 1 to 16), subscripts A and B reference a 2D image (A or B).
- X, Y and Z are the 3D coordinates of each point.
- $a_n,b_n$ are a set of 11 parameters for each image (total 22) that model both the interior and exterior "orientation".
- $\xi,\eta$ are the coordinates of the points as seen from a particular image. $ \eta^3_B $ would be the y-coord of the third point in the B image.
- Two of the points have their 3D coordinates already set to $(0,0,0)$ and $(100,0,0)$.
- Every point provides four of the equations described above for a total of 64 equations.
- Since two of the points have XYZ already defined, we have $14·3=42$ total 3D coordinate variables. The $a_1,...,a_8,b_1,b_2,b_3$ parameters for each image give us a total of 22 parameters, so we already have our 64 variables. $$p=(a^A_1,...,b^A_3,a^B_1,...,b^B_3,X^1,Y^1,...,Y^{14},Z^{14})$$
I'm using MATLAB and so far I've tried my luck with Newton-Raphson since the Jacobian can be easily constructed at each time step. $$ p_{n+1}=p_n-J^{-1}_nF(p_n)$$ But the product of $J^{-1}_nF(p_n)$ already gets to $10^{12}$ at the first iteration. It's almost definitely caused by my random initialization of all the variables (i give the a,b parameters values between 0 and 1 and the XYZ coords values between -50 and 50).
Any thoughts on finding a better initialization for the variables or on using a different method to solve the system? I'm also planning to increase the number of points in the future and have an optimization problem.
Thanks