# Math Help - Inverse Rotation Matrix

1. ## Inverse Rotation Matrix

Hi,

I'm totally new to this forum so please forgive me if I'm posting this in a wrong folder. I'm working on robotics and I'm currently running in to some rotation problems which kinda run over my head at the moment. I try to understand what I'm doing wrong but can't seems to find what I'm doing wrong. Hopefullly somebody can help me out.

Using a cartesian coordinate system; I'm using a Pitch-Roll-Yaw rotation matrix to calculate the position of point P from from the Origin (O) with the given angles (Z-theta, Y-beta, X-alfa)

the Rotation Matrix:
$Q = Qz,theta * Qy,beta * Qx,alfa$

Q = [cos(b)*cos(t), -cos(a)*sin(t)+cos(t)*sin(a)*sin(b), sin(a)*sin(t)+cos(a)*cos(t)*sin(b)
;cos(b)*sin(t), cos(a)*cos(t)-sin(a)*sin(b)*sin(t), -cos(t)*sin(a)+cos(a)*sin(b)*sin(y)
;-sin(b), cos(b)*sin(a), cos(a)cos(b)]

This equation allows me to calculate the position of point P with the given angles alfa, beta and theta.
$P[x;y;z] = Q * O[x;y;z]$

I'm looking for a method to inverse this matrix. Meaning that I would like to calculate the 3 angles from the 2 given points P[x;y;z] and O[x;y;z].

Can somebody tell me how to find rotations (alfa, beta, theta) from two given positions?

Help is very appriciated!

Thanks!

Xan

2. Well, I can tell you a few things that might help out.

Fact 1: The inverse of a simple rotation matrix (for example, JUST the rotation about the x-axis) is the same rotation matrix with a negative angle. Or, you can simply take the transpose of the original rotation matrix. It amounts to the same thing.

Fact 2: The inverse of the product of three matrices $Q_{1}Q_{2}Q_{3}$ is the product of the inverses in reverse order. That is,

$(Q_{1}Q_{2}Q_{3})^{-1}=Q_{3}^{-1}Q_{2}^{-1}Q_{1}^{-1}=Q_{3}^{T}Q_{2}^{T}Q_{1}^{T}.$

The last equality is true if all three matrices in question are simple rotations about either the x, y, or z axis.

However, this does not seem to be what you're asking about. I don't think you really mean that you need to invert a matrix. Instead, what I think you're asking is about what's called an inverse problem. The forward problem is the following: given one point and a rotation matrix, find the point to which the rotation matrix maps the given point. The inverse problem, which sounds like what you're trying to solve, goes like this: given two points, find the rotation matrix that takes one point to the other point. Is that correct?

If so, I have a couple more comments to make:

1. The solution may not be unique.
2. You'll have to assume that the two points in question are an equal distance away from the prescribed origin. A rotation matrix does not change the length of a vector. Is this a safe assumption?

Here's one possible solution method: pick a vector of the same length as both P and O, but in the direction of the x axis, say. Call this vector R. Find the rotation matrix that takes P to R, and then find the rotation matrix that takes R to O. I'm assuming it's easier to find the P to R rotation matrix than the P to O rotation matrix directly. The solution would then be the product of these two rotations.

Make sense? If so, what ideas do you have?

3. Hi Adrian,

First of all, thanks for your quick and detailed reply.

Although my description seems to be on some loose screws you picked out my problem perfectly. Sorry for that.

Indeed, what I need is the inverse problem. I've got 2 points in space (3d) and need the angles or rotation matrix as solution.

In my last post I only talked about a single vector, trying to keep my question as simple as possible. I'll try to give more detail because maybe I'm missing things.

The vector I discribed is part of a triangle plane which represents the body of a robot. The triangle has got 3 points (P, B, C) and an origin O. This can be seen in my beautifull ASCII art
Code:
P  +
/ |
/  |
/   |
/    |
/     |
/      |
C +  O +  |
\      |
\     |
\    |
\   |      ^  Y
\  |      |
\ |      |
B +       +---> X
The distance between O and the given points (P, B, C) are static. The position of P reffering to O can be given in $d_x$ and $d_y$. $d_z$ = 0 for all points.

The body can be rotated using the rotation matrix Q as shown in my previous post. This can be done by multiplying Q with the lengths. $P = Q*[d_{x}, d_{y}, d_{z}]^T$. This gives me point P, B or C.

About my inverse problem. I also need to be able to determine the angels of the plane in 3d with 3 given points (P, B and C). Exactly like you described.

First attempt:
So my first thought was to start with vector B-P to calculate both Pitch and Yaw angles. I thought I would only need $d_{y}$ for point P and B. Made a 4x4 matrix where r14, r24, r34 is the position of point B. R is the rotation matrix shown as Q in my previous post.

$T=\left[\begin{matrix}R_{q}&B\\0&1\end{matrix}
\right]*\left[\begin{matrix}d_{x}\\d_{y}\\d_{z}\\1\end{matrix}\r ight]$

$P=\left[\begin{matrix}-d_{x}cos(b)cos(t)+X_{B}\\-d_{x}cos(b)sin(t)+Y_{B}\\d_{x}sin(b)+Z_{B}\end{mat rix}\right]=\left[\begin{matrix}X_{p}\\Y_{P}\\Z_{P}\end{matrix}\righ t]
$

To substitute Pitch and Yaw angles I removed $d_{x}$ since B and P don’t have an Y offset. This simplifies in:

$b = asin(\frac{X_{B}-X_{P}}{d_{x}})$

$t = acos(\frac{X_{B}-X_{P}}{d_{x}cos(b)})$

This results in the correct angles for beta and theta as long as I keep alfa zero!
So I think that I need to add $d_{y}$ to the equation but this makes substituting the angles much harder.

So to get back to your post.
Yes the distance between the points are static.
The distance on the z axis is 0.
The distance on the y axis is 0 between point C and origin O.

Maybe it is better to work from position O to each of the points? I think this is the solution method you are trying to tell me with the R vector but it’s not totally clear to me. I don’t know how to do this since point O is totally unknown. Note: the position of O is also unknown. I only know the positions of the other points.

I’ve been struggling with this for some days now. I hope you get me on the right track again

I hope my long story makes any sense
Thanks for you time!

Xan

4. Originally Posted by Ackbeet
Here's one possible solution method: pick a vector of the same length as both P and O, but in the direction of the x axis, say. Call this vector R. Find the rotation matrix that takes P to R, and then find the rotation matrix that takes R to O. I'm assuming it's easier to find the P to R rotation matrix than the P to O rotation matrix directly. The solution would then be the product of these two rotations.

Make sense? If so, what ideas do you have?
Hi Adrian,

I've been thinking this over for the last couple of days and it is starting to make sense. I guess...

What you're saying is that I should take a vector with the same length as P and call it R both vectors will have a common Origen. Then I should be able to find the rotation matrix between vector P and vector R. I've been searching the web but can't find a good explanation about how to get a rotation matrix from a given vector.

Can you or somebody help me out and tell me how to do this?

Thanks in advance,

Xan

5. In two dimensions, it's as simple as a coordinate transform. To get the rotation angle corresponding to the vector $(x,y)^{T},$ you have

$\begin{bmatrix}x\\y\end{bmatrix}=\begin{bmatrix}\c os(\tan^{-1}(y/x)) &-\sin(\tan^{-1}(y/x))\\ \sin(\tan^{-1}(y/x)) &\cos(\tan^{-1}(y/x))\end{bmatrix}\begin{bmatrix}\sqrt{x^{2}+y^{2}}\ \0\end{bmatrix}.$

6. The vector doesn't have a length when it is on his home position. But since it can rotate in 3D space it can end up having one. This means that I also need to include the Z axis to get Pitch, Yaw and Roll...

Any idea how I can do this?

Thanks!

7. The vector doesn't have a length when it is on his home position.
That makes no sense. Simple rotations do not change the length of a vector. Hence, if you have a vector of a particular length after a simple rotation, you had to have started with a vector of the same length. Did you mean "z coordinate" instead of "length"? That substitution would make your sentences make sense.

I'll need to think some more about this problem. Just to be clear: in order to rotate point O to point P, you're applying first a rotation about the x axis, then one about the y axis, and finally one about the z axis. Is that correct?

8. Sorry for my horrible explanation, again...

Yeah you are totally right. The length of the vector is static! The Z-coordinate starts with a 0 value. So after rotation this will change.

And you're right about the second part as well. The rotation matrix I'm using rotates in the order of x, y, z indeed.

Thanks for looking in to this!

9. All right. I've got some ideas, finally. You may not like the solution, but I'm not sure I know of another one.

A different way of solving this problem is to rotate, essentially, just once, about an axis that is perpendicular to both the original vectors. So, take vectors $\mathbf{P}$ and $\mathbf{Q}$, your original vectors. You want to find the rotation matrix that rotates $\mathbf{Q}$ to $\mathbf{P}$. To do that, use the following steps:

1. Determine the angle between the two vectors, call it $\theta.$ Since you have the cartesian representation of both vectors, you can employ either the dot product equation

$\mathbf{P}\cdot\mathbf{Q}=|\mathbf{P}|\,|\mathbf{Q }|\,\cos(\theta),$ and solve for $\theta,$

or you can use the equivalent cross product equation

$|\mathbf{P}\times\mathbf{Q}|=|\mathbf{P}|\,|\mathb f{Q}|\,\sin(\theta),$

where $0\le\theta\le\pi.$

With me so far?

2. Find $\mathbf{C}:=\mathbf{P}\times\mathbf{Q}.$ This will be your axis of rotation, since $\mathbf{C}$ is perpendicular to both $\mathbf{P}$ and $\mathbf{Q}.$

3. Follow the procedure here to perform the desired rotation. Note that you will not have to do the translation steps, because your rotation axis already passes through the origin. So, for you, $T=T^{-1}=I.$

Then you're done! Your result is a matrix not quite of the form you intended, but you do get this:

$\mathbf{P}=R_{x}^{-1}R_{y}^{-1}R_{z}R_{y}R_{x}\mathbf{Q}.$

Incidentally, you might possibly run into an issue where the rotation matrix doesn't give you $\mathbf{P}$ when applied to $\mathbf{Q}.$ If so, try the same matrix with $-\theta$ instead. There might have been an orientation mix-up somewhere in there.

To determine, then, how this rotation could be done in only three steps is a difficult problem.

Hope this helps.

10. I've followed the steps like you said.

1. Gives me the angle between the 2 vectors called \theta.
2. Gives me a vector C which is represents the cross product.
3. I passed over step 1. The first thing to do in step 2 is $d=sqrt(b^2+c_2)$ The picture on top of the website shows the representation of $d$, $b$ and $c$. I'm not quite sure what vectors I should take here. Is $c=C$ found in point 2 or is it the Z position of the vector $P$? I think that vector $P$ from the website represents the vector after rotation. Is that correct?

Do I need to replace the value c/d and b/d from the matrix $R_x$ with cos(t) and sin(t) from point 2 or with the equation: $\frac{(0,0,c)dot(0,b,c)}{c * d} = c/d$?

Sorry for the new set of questions. I do think you are on to something.

Thanks in advance

Xan

11. Hmm. Perhaps the most helpful thing to do would be to provide you a your-problem-to-website correspondence. As I see it, you have this (website values on the left of the $\leftrightarrow$, your problem values on the right):

$\mathbf{P}_{1}\leftrightarrow\text{origin}$

$\mathbf{P}_{2}\leftrightarrow \mathbf{C}:=\mathbf{P}\times\mathbf{Q}.$

$\langle a,b,c\rangle=\mathbf{U}\leftrightarrow \dfrac{\mathbf{C}}{|\mathbf{C}|}$

$\theta=t.$

Does that help?

One thing I found confusing was I didn't know whether you were referring to my steps in Post # 9, or the website's steps. Please distinguish between by writing like this: "your step #2" versus "the website's step #2". Make sense?

12. Thanks for the additional information. I will try this out as soon as possible and keep you updated.

You're right about the confusing step numbers in my previous post. I'll add the resource next time

Thanks!

13. Ok, I think I'm stuck again. As usual...

I think it might be a smart idea to drop my work in here so we both talk about the same. I'm using scilab to do the math. I filled in some values to check the results with what I have from the rotation matrix.

Origin vector

$Q = \begin{bmatrix}8.2\\4.3\\0\end{bmatrix}= P_{1}$

I apply my Roll-Pitch-Yaw rotation (Matrix shown in post #1)

$R^{Q}_{P} = R_{Z,\gamma}R_{Y,\beta}R_{X,\alpha}$

Vector after rotation

$P = \begin{bmatrix}5.399\\7.247\\2.012\end{bmatrix}$

Apply your Step #1, Find theta

$\theta = \mathbf{P}\cdot\mathbf{Q} = 0.8799709$

Apply your Step #2, find perpendicular vector C

$C = \mathbf{P}\times\mathbf{Q} = \begin{bmatrix}8.6516\\-16.4984\\36.2097\end{bmatrix}$

Find U

$U = \dfrac{\mathbf{C}}{|\mathbf{C}|} = \dfrac{\mathbf{C}}{\sqrt{C_{(x)}^2 + C_{(y)}^2 + C_{(z)}^2}} = \begin{bmatrix}17.3032\\-32.9968\\72.4194\end{bmatrix}$

where $C = \begin{bmatrix}C_{(x)}\\C_{(y)}\\C_{(z)}\end{bmatr ix}$

Website Step #1, is not needed since the origin is at 0,0,0

Website Step #2, Rotation

$a = U_{(x)}=17.3032$

$b = U_{(y)}=-32.9968$

$c = U_{(z)}=72.4194$

$d = \sqrt{b^2 + c^2}=79.582399$

The next 2 formulas given are

$\cos(\theta) = \frac{(0,0,c)\cdot(0,b,c)}{c d} = c/d$

and

$\sin(\theta) = \frac{||(0,0,c)\times(0,b,c)||}{c d} = b/d$

The result of the functions are used in the matrixes

$R_{x} = \begin{bmatrix}1&0&0&0
\\0&c/d&-b/d&0
\\0&b/d&c/d&0
\\0&0&0&1\end{bmatrix}$

$R_{x}^{-1} = \begin{bmatrix}1&0&0&0
\\0&c/d&b/d&0
\\0&-b/d&c/d&0
\\0&0&0&1\end{bmatrix}$

This is where it get's a bit unclear to me. If I got the formulas right I can simply take $cos(\theta) = c/d$ and $sin(\theta) = b/d$ since $\theta$ is already known from your step #1?

This makes:

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

$R_{x}^{-1} = \begin{bmatrix}1&0&0&0
\\0&cos(\theta)&sin(\theta)&0
\\0&-sin(\theta)&cos(\theta)&0
\\0&0&0&1\end{bmatrix}
= \begin{bmatrix}1&0&0&0
\\0&0.9998&0.0153&0
\\0&-0.0153&0.9998&0
\\0&0&0&1\end{bmatrix}$

Not quite sure if I'm still on the right track here...

14. I would double-check your U. It's supposed to be a unit vector (length one), and your candidate can't possibly be that length. I will look into the rest of your computations as I have time.