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 [itex]X_{i} = (X_{1},X_{2}...,X_{n})[/itex] and [itex]X^{'}_{i} = (X^{'}_{1},X^{'}_{2}...,X^{'}_{n})[/itex], where [itex]X^{'}_{i}[/itex] is the result of an applied rotation to [itex]X_{i}[/itex], 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

[itex]\begin{bmatrix}
x_{1} & x_{2} & x_{3}... & x_{n} \\
y_{1} & y_{2} & y_{3}... & y_{n} \\
z_{1} & z_{2} & z_{3}... & z_{n} \end{bmatrix}[/itex]

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

Next I have set up my rotation matrices as follows:

[itex] 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} [/itex]

[itex] R_{y}(\theta_{y}) = \begin{bmatrix} cos(\theta_{y}) & 0 & sin(\theta_{y}) \\ 0 & 1 & 0 \\ - sin(\theta_{-}) & 0 & cos(\theta_{y}) \end{bmatrix} [/itex]

[itex] 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} [/itex]

Multiplied together:

[itex]R_{t} = R_{z}*R_{y}*R_{x}[/itex]

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

Then the Jacobian [itex]J[/itex] of [itex]f[/itex] with respect to [itex]\theta_{x}, \theta_{y}, \theta_{z}[/itex] is computed

[itex]J*d\theta = df[/itex]

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

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

Finally, you get [itex]J^{T}*J*d\theta = J^{T}*d\beta[/itex]

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

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

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

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

[itex]\theta_{x,y,z} = \theta_{x,y,z} + d\theta[/itex] or [itex]\theta_{x,y,z} = \theta_{x,y,z} - d\theta[/itex]

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 [itex]d\beta[/itex] / [itex]d\ f[/itex] 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 [itex]d\ f[/itex]? I am assuming it has something to do with this equation [itex] A = X^{'}X^{T}(XX^{T})^{-1} [/itex]. Thanks

 

[nschaefe]

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

However, it seems like it should actually be [itex] d\beta = X^{'}X^{T}(XX^{T})^{-1} - R_{t}(\theta_{x},\theta_{y},\theta_{z}) [/itex] as this matches the definition on the wolfram page [itex]d\beta_{i} = y_{i} - f_{i}(\lambda_{1},\lambda_{2},... \lambda_{i})[/itex], 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?