Nonlinear Least Squares Fitting

[nschaefe]

Hello,

So I was hoping to get some help implementing a nonlinear least squares fitting algorithm. Technically this is an extension of my previous thread, however the problem I am having now is correctly computing the algorithm

So the problem definition is this:

Given two sets of n 3D points $X_{i} = (X_{1},X_{2}...,X_{n})$ and $X^{'}_{i} = (X^{'}_{1},X^{'}_{2}...,X^{'}_{n})$, where $X^{'}_{i}$ is the result of an applied rotation to $X_{i}$, find the corresponding rotation matrix and Euler Angles

The formulas I am following are from these two links:
Euler Angles Eqs 71 - 77 and NonLinear Least Squares Fitting

I am going to attempt to follow the convention of those articles. So first off I set up matrices X and X' as such

$\begin{bmatrix}<br /> x_{1} & x_{2} & x_{3}... & x_{n} \\<br /> y_{1} & y_{2} & y_{3}... & y_{n} \\<br /> z_{1} & z_{2} & z_{3}... & z_{n} \end{bmatrix}$

where $x_{i},y_{i},z_{i}$ are the components of $X_{i} / X^{'}_{i}$

Next I have set up my rotation matrices as follows:

$R_{x}(\theta_{x}) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(\theta_{x}) & -sin(\theta_{x}) \\ 0 & sin(\theta_{x}) & cos(\theta_{x}) \end{bmatrix}$

$R_{y}(\theta_{y}) = \begin{bmatrix} cos(\theta_{y}) & 0 & sin(\theta_{y}) \\ 0 & 1 & 0 \\ - sin(\theta_{-}) & 0 & cos(\theta_{y}) \end{bmatrix}$

$R_{z}(\theta_{z}) = \begin{bmatrix} cos(\theta_{z}) & -sin(\theta_{z}) & 0 \\ sin(\theta_{z}) & cos(\theta_{z}) & 0 \\ 0 & 0 & 1 \end{bmatrix}$

Multiplied together:

$R_{t} = R_{z}*R_{y}*R_{x}$

Next take matrix $A$ (which I take to the be rotation matrix $R_{t}$) and turn it into a column vector called $f$

Then the Jacobian $J$ of $f$ with respect to $\theta_{x}, \theta_{y}, \theta_{z}$ is computed

$J*d\theta = df$

This is where the first article stops. Moving to the next article,

$J$ is $A$, $d\theta$ is $d\lambda$, and $df$ is $d\beta$ I presume. However, I will keep the original convention.

Finally, you get $J^{T}*J*d\theta = J^{T}*d\beta$

so $d\theta = (J^{T}*J)^{-1}*J^{T}*d\beta$
So my questions are these:

1. How do I form $d\beta$? Right now I am taking my "guess" at the angles (will call this $\theta_{xo,yo,zo}$) to compute $R_{t0}$.

Then $d\beta = X^{'} - R_{t0}*X$, and it is turned into a column vector just like $R_t$ is turned into $f$. Is this correct?

2. When I compute my new angles, should it be

$\theta_{x,y,z} = \theta_{x,y,z} + d\theta$ or $\theta_{x,y,z} = \theta_{x,y,z} - d\theta$

I have successfully programmed all of these steps into a VB.NET program, but the solution is not converging and I cannot figure out why.

Any help greatly appreciated. Thanks

 

[nschaefe]

So I am realizing I think how I am calculating $d\beta$ / $d\ f$ is incorrect, as this should yield an equation that is a 3x3 matrix which is transformed into the 1x9 column vector. Can someone please explain how to find $d\ f$? I am assuming it has something to do with this equation $A = X^{'}X^{T}(XX^{T})^{-1}$. Thanks

 

[nschaefe]

So I think it figured it out, but it still seems strange. I took $d\beta = R_{t}(\theta_{x},\theta_{y},\theta_{z}) - X^{'}X^{T}(XX^{T})^{-1}$ where $\theta_{x}, \theta_{y}, \theta_{z}$ are updated at each iteration by $\theta_{x,y,z} = \theta_{x,y,z} - d\theta_{x,y,z}$, and it appears to be working.

However, it seems like it should actually be $d\beta = X^{'}X^{T}(XX^{T})^{-1} - R_{t}(\theta_{x},\theta_{y},\theta_{z})$ as this matches the definition on the wolfram page $d\beta_{i} = y_{i} - f_{i}(\lambda_{1},\lambda_{2},... \lambda_{i})$, but the solution refuses to converge unless I write it as above.

Can anyone shed some light on this discrepancy and confirm I am calculating this correctly?