Numerical method for solving overdetermined system of quadratic equations.

281 Views Asked by At

This is a problem I am solving for finding a rotation and translation matrix for looking at points in two different coordinate systems. I seek to find a transformation matrix to find this relationship. I have a point $Z_i$ in coordinate system A that can be defined as $(x_i,y_i,z_i)$ and a point $Z_i'$ in coordinate system B that can be defined as $(x_i',y_i',z_i')$. The distance between the two points $Z_i$ and $Z_i'$ is D (Euclidean distance).

Suppose I have this relationship between variables:

$$(x_i - r11*x_i' - r12*y_i' - r13*z_i' - T_x)^2 + (y_i - r21*x_i' - r22*y_i' - r23*z_i' - T_y)^2 + (z_i - r31*x_i' - r32*y_i' - r33*z_i' - T_z)^2 = D_i^2$$

$i = 1,2,3,4...n$, where $n>12$ (overdetermined system of equations). Let's say n = 32 in this instance. I have 12 unknowns, 9 of which relate to rotation and 3 which relate to translation. I have $n$ pairs of (where $n>12$) points that are given... $(x_i, y_i, z_i)$ and $(x_i', y_i', z_i')$ and I know the distance $D_i$ between each set of points.

What numerical (or other) methods can I use to solve this overdetermined system of equations in order to find $r11, r12, r13, r21, r22, r23, r31, r32, r33, T_x, T_y,$ and $T_z$?

1

There are 1 best solutions below

0
On

In the ideal case, all $D_i$ terms are zero. In the real case, given $(x_i, y_i, z_i)$ and $(x_i', y_i', z_i')$, you can minimize $\sum_i D_i^2$. To do this, we write the sum, and take the derivative with respect to each of the unknowns and set them to $0$. For example:

$$\begin{align}\frac{d}{dr_{11}}\sum_iD_i^2=&-2\sum_ix_i'(x_i-r_{11}x_i'-r_{12}y_i'-r_{13}z_i'-T_x)\\=&r_{11}\cdot2\sum_ix_i'^2+r_{12}\cdot2\sum_ix_j'y_i'+r_{13}\cdot2\sum_ix_j'z_i'+\\&T_x\cdot2\sum_ix_i'-2\sum_ix_ix_i'\\=&0 \end{align}$$

You write similar equations for all other parameters. You calculate all the sums (since you already know $x_i, x_i',...)$ What you are left with is a system with 12 linear equations with 12 unknowns.