nschaefe
- 12
- 0
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
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
Last edited: