Develop Reference

Converting a rotation matrix to euler angles and back - special case?

I'm disassembling a rotation matrix to Euler angles (Tait-Bryan angles more specifically in the order x-y-z, that is rotation around x axis first) and back to a rotation matrix. I used the transforms3d python library (https://github.com/matthew-brett/transforms3d) and also followed this tutorial www.gregslabaugh.net/publications/euler.pdf Both give the same result.
The problem is that the reassambled rotation matrix doesn't match the one that I started with.
The matrix I'm working with was created by the "decomposeHomographyMat" function from openCV, so I expect it to be a valid rotation matrix. Maybe it is a special case?
The matrix is
The three angles are [-1.8710997 , 0.04623301, -0.03679793]. If I convert them back to a rotation matrix I get
where R_23 cannot be a rounding error.
Following the paper above, rotation around the y axis (beta) can be calculated by asin(-R_31). Another valid angle would be pi-asin(-R_31).
The angle around the x axis (alpha) can be calculated by atan2(R_32,R_33). I could also get alpha by asin(R_32/cos(beta)) or by acos(R_33/cos(beta)). If I use the latter two equations I only get the same result for alpha if I use beta=pi-arcsin(-R_31), which implies that there is only one valid solution for beta. atan2(R_32,R_33) gives a different result from both.
Anyway something seems to be wrong with my matrix or I cannot figure out why the disassambly doesn't work.
import numpy as np
def rot2eul(R):
beta = -np.arcsin(R[2,0])
alpha = np.arctan2(R[2,1]/np.cos(beta),R[2,2]/np.cos(beta))
gamma = np.arctan2(R[1,0]/np.cos(beta),R[0,0]/np.cos(beta))
return np.array((alpha, beta, gamma))
def eul2rot(theta) :
R = np.array([[np.cos(theta[1])*np.cos(theta[2]), np.sin(theta[0])*np.sin(theta[1])*np.cos(theta[2]) - np.sin(theta[2])*np.cos(theta[0]), np.sin(theta[1])*np.cos(theta[0])*np.cos(theta[2]) + np.sin(theta[0])*np.sin(theta[2])],
[np.sin(theta[2])*np.cos(theta[1]), np.sin(theta[0])*np.sin(theta[1])*np.sin(theta[2]) + np.cos(theta[0])*np.cos(theta[2]), np.sin(theta[1])*np.sin(theta[2])*np.cos(theta[0]) - np.sin(theta[0])*np.cos(theta[2])],
[-np.sin(theta[1]), np.sin(theta[0])*np.cos(theta[1]), np.cos(theta[0])*np.cos(theta[1])]])
return R
R = np.array([[ 0.9982552 , -0.03323557, -0.04880523],
[-0.03675031, 0.29723396, -0.95409716],
[-0.04621654, -0.95422606, -0.29549393]])
ang = rot2eul(R)
eul2rot(ang)
import transforms3d.euler as eul
ang = eul.mat2euler(R, axes='sxyz')
eul.euler2mat(ang[0], ang[1], ang[2], axes='sxyz')

It turns out the rotation matrix has a negative determinant, which makes it an improper rotation matrix. The openCV function "decomposeHomographyMat" has a bug: https://github.com/opencv/opencv/issues/4978

May be you can use scipy
from scipy.spatial.transform import Rotation
### first transform the matrix to euler angles
r = Rotation.from_matrix(rotation_matrix)
angles = r.as_euler("zyx",degrees=True)
#### Modify the angles
print(angles)
angles[0] += 5
#### Then transform the new angles to rotation matrix again
r = Rotation.from_euler("zyx",angles,degrees=True)
new_rotation_matrix = new_r.as_matrix()

Trouble in obtaining correct geometric transformation of cartesian coordinates

I am working in python and i have previous (x_prev,y_prev) = (1.5, 3) coordinate and current (x,y) = (2, 3.2)coordinate and angle difference between them and i want the next coordinate to be at a certain distance d with the same orientation as the current (x,y)coordinate. I have tried using the rotation and translation formula but it fails to give the proper answer. here is the code so far what i tried.
d = 0.5
angle = np.arctan2((y - y_prev), (x - x_prev))
x_ = x * np.cos(angle) - y * np.sin(angle) + (d * np.sinc(angle_/2)* np.cos(angle/2))
y_ = x * np.sin(angle) + y * np.cos(angle) + (d * np.sinc(angle_/2)* np.sin(angle/2))
the expected coordinate is approximately (x_,y_) = (2.5, 3.6) with the same orientation as the current but it results in wrong coordinate so is there anything i am missing.
Thanks in advance

I partly agree with #ImportanceOfBeingErnest that your question is a geometrical one. However, I'm adding an answer because numpy lets you avoid all that trigonometric work that you are trying to do in the first place.
What you want is to find the point (x_new,y_new) based on (x_prev,y_prev) and (x_now,y_now) such that the three points lie on the same line and the distance between (x_prev,y_prev) and (x_new,y_new) is a preset d.
You don't need trigonometry if you can work with proper two-dimensional vectors. You can normalize the vector (x_now,y_now) - (x_prev,y_prev) to get an orientation vector of the line along which you need to move from (x_prev,y_prev) in order to end up at (x_new,y_new). Numpy lets you handle this elegantly:
import numpy as np
x_prev,y_prev = (1.5, 3)
x_now,y_now = (2, 3.2)
d = 0.5
# use 2d arrays for elegant vector operations
# of course we can directly define these from coordinates if we want to
p_prev = np.array([x_prev,y_prev])
p_now = np.array([x_now,y_now])
# compute the unit direction vector for p_new - p_prev
t = p_now - p_prev
t /= np.linalg.norm(t) # use Euclidean norm by default
# p_new is simple now:
p_new = p_prev + d*t
print(p_prev)
print(p_now)
print(p_new)
The above results in (x_new,y_new)=(1.96423835,3.18569534). Your points are actually such that (x_now,y_now) is almost at 0.5 distance from (x_prev,y_prev), so the resulting vector is hardly different from the original one. But anyway, the above procedure will always give you a new point which is at the same angle from (x_prev,y_prev) as (x_now,y_now) but at the fixed distance.

Distance between point and arc in 3D

I want to compute the distance between an arc and a point in a 3D space. All I found is the distance between a circle and a point link (which is either wrong, or where I made a mistake, as I get wrong values):
P = np.array([1,0,1])
center = np.array([0,0,0])
radius = 1
n2 = np.array([0,0,1])
Delta = P-center
dist_tmp = np.sqrt( (n2*Delta)**2 + (np.abs(np.cross(n2, Delta))-radius)**2 )
dist = np.linalg.norm(dist_tmp)
I have a circle in the x-y-plane with origin at x-y-z = 0 and radius = 1. The point of interest is in distance 1 above the circle. The result of the distance from the code is 1.73.. and not 1.
What is the right equation for point-circle distance?
How can I extend it to point-arc distance?

You have several errors in your code. Here is the answer to your first question.
First, you try to implement the dot product of n2 and Delta as n2*Delta, but that is not what the multiplication of 2 np arrays does. Use np.dot() instead. Next, you try to take the "absolute value" (magnitude) of a vector with np.abs, but that latter is for real and complex numbers only. One way to get the vector magnitude is np.linalg.norm(). Changing those gives you the proper answer, and you don't need the calculation you used for variable dist. So use
Delta = P-center
dist = np.sqrt(np.dot(n2, Delta)**2 + (np.linalg.norm(np.cross(n2, Delta))- radius)**2)
That gives the proper answer for dist, 1.0.

How do I retrieve the angle between two vectors 3D?

I am new in python.
I have two vectors in 3d space, and I want to know the angle between two
I tried:
vec1=[x1,y1,z1]
vec2=[x2,y2,z2]
angle=np.arccos(np.dot(vec1,vec2)/(np.linalg.norm(vec1)*np.linalg.norm(vec2)))
but when change the order, vec2,vec1 obtain the same angle and no higher.
I want to give me a greater angle when the order of the vectors changes.

Use a function to help you choose which angle do you want. In the beggining of your code, write:
def angle(v1, v2, acute):
# v1 is your firsr vector
# v2 is your second vector
angle = np.arccos(np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2)))
if (acute == True):
return angle
else:
return 2 * np.pi - angle
Then, when you want to calculate an angle (in radians) in your program just write
angle(vec1, vec2, 'True')
for acute angles, and
angle(vec2, vec1, 'False')
for obtuse angles.
For example:
vec1 = [1, -1, 0]
vec2 = [1, 1, 0]
#I am explicitly converting from radian to degree
print(180* angle(vec1, vec2, True)/np.pi) #90 degrees
print(180* angle(vec2, vec1, False)/np.pi) #270 degrees

If you're working with 3D vectors, you can do this concisely using the toolbelt vg. It's a light layer on top of numpy.
import numpy as np
import vg
vec1 = np.array([x1, y1, z1])
vec2 = np.array([x2, y2, z2])
vg.angle(vec1, vec2)
You can also specify a viewing angle to compute the angle via projection:
vg.angle(vec1, vec2, look=vg.basis.z)
Or compute the signed angle via projection:
vg.signed_angle(vec1, vec2, look=vg.basis.z)
I created the library at my last startup, where it was motivated by uses like this: simple ideas which are verbose or opaque in NumPy.

What you are asking is impossible as the plane that contains the angle can be oriented two ways and nothing in the input data gives a clue about it.
All you can do is to compute the smallest angle between the vectors (or its complement to 360°), and swapping the vectors can't have an effect.
The dot product isn't guilty here, this is a geometric dead-end.

The dot product is commutative, so you'll have to use a different metric. It doesn't care about the order.

Since the dot product is commutative, simply reversing the order you put the variables into the function will not work.
If your objective is to find the obtuse(larger) angle rather than the acute(smaller) one, subtract the value returned by your function from 360 degrees. Since you seem to have a criteria for when you want to switch the variables around, you should use that same criteria to determine when to subtract your found value from 360. This will give you the value you are looking for in these cases.

Converting G Force data to rotation?

I've been playing around with an accelerometer with 3 axis: X, Y and Z. It says on the supplier's site that it measures gravitational force.
I'm sending this data to the blender games engine where I am rotating a cube in real time depending on the data values coming from the accelerometer. However the values coming through don't seem to match up.
On each axis the accelerometer spits out values from -700 to 700 on each axis and I need to convert these values to something I can use in Blender. My maths knowledge is not up to scratch so I don't know where to start with this one.
If anybody could shed some light on this, that would be great.
Many thanks
Will
EDIT
Currently I'm using a bit of python code to convert the rotation values to a matrix:
def reorient(alpha, beta, gamma):
a = math.cos(alpha)
b = math.sin(alpha)
c = math.cos(beta)
d = math.sin(beta)
e = math.cos(gamma)
f = math.sin(gamma)
ad = a*d
bd = b*d
matrix = [[c*e, -a*f+b*d*e, b*f+a*d*e], [c*f, a*e+b*d*f, -b*e+a*d*f], [-d, b*c, a*c]]
return matrix
I am then using setOrientation(matrix) to affect the rotation of the cube. However I am currently throwing the wrong values into the matrix reorient() function

I guess you are using the measured acceleration to find the direction of gravitational pull (ie, down). If you are moving the accelerometer, apart from just turning it, there will be some additional force; think of the accelerometer having a pendulum weight handing from it, as you move it the pendulum sways (although in this case it would be a very short, fast-reacting pendulum?). You could try doing some sort of movement compensation, but it might be simpler to just try to keep the sensor in a fixed location.
Edit: ok, it looks like I totally misread the question - you want to know how to do the rotation in a script?
It looks like each Blender object has three properties (.RotX, .RotY, .RotZ) which contain the current values (in radians) and a method (.rot(new_rotx, new_roty, new_rotz)) which performs a rotation (see documentation at http://www.blender.org/documentation/249PythonDoc/Object.Object-class.html). I am currently looking at how the rotations are applied; more shortly.
Edit2: it looks like the angles are specified as Euler angles (http://en.wikipedia.org/wiki/Euler_angles); they give some conversion matrices. It also looks like your accelerometer data is underconstrained (you need one more constraint, specifying rotation about the 'down' direction - maybe some sort of inertial 'least distance from previous position' calculation?)
Edit3: there is a sample script which may be helpful; on my machine it is at C:\Users\Me\AppData\Roaming\Blender Foundation\Blender.blender\scripts\object_random_loc_sz_rot.py It shows how to get the currently selected object and tweak its rotation. Hope that helps!
Edit4: for sake of discussion, here is some sample code; it may be a bit redundant (I haven't worked in Blender before) and it doesn't solve the problem, but it will at least give us a common basis for further discussion ;-)
#!BPY
"""
Name: 'Set rotation by accelerometer'
Blender: 249
Group: 'Object'
Tooltip: 'Set the selected objects rotation by accelerometer'
"""
__bpydoc__=\
'''
This script sets the selected objects rotation by accelerometer.
'''
from Blender import Draw, Scene
import math
def reorient(alpha, beta, gamma):
a = math.cos(alpha)
b = math.sin(alpha)
c = math.cos(beta)
d = math.sin(beta)
e = math.cos(gamma)
f = math.sin(gamma)
ad = a*d
bd = b*d
return = [
[c*e, -a*f+b*d*e, b*f+a*d*e],
[c*f, a*e+b*d*f, -b*e+a*d*f],
[-d, b*c, a*c ]
]
def getAccel():
# test stub -
# need to get actual values from accelerometer here
dx = -700
dy = 100
dz = 250
return (dx,dy,dz)
def normalize(vec):
"Return scaled unit vector"
x,y,z = vec
mag = (x*x + y*y + z*z)**0.5
return (x/mag, y/mag, z/mag)
def main():
scn = Scene.GetCurrent()
try:
obj = scn.objects.context
euler = (obj.RotX, obj.RotY, obj.RotZ)
except AttributeError:
return
down = normalize(getAccel())
matrix = None
# do something here to find new rotation-matrix
# based on euler and down
# then
if matrix:
obj.setOrientation(matrix)
else:
# test value:
# if reorient() is working properly, the
# object's rotation should not change!
obj.setOrientation(reorient(*euler))
if __name__=="__main__":
main()

Let's assume that you can use the accelerometers to correctly determine which way is 'up'. We can call that vector N. It seems to me that you want the 'up' direction of your cube to align with N. As already mentioned, that leaves the cube free to spin, but you can still find a rotation matrix that might accomplish what you're going for, but you need to account for two separate cases or else you'll have a singularity in the solution. I'll assume that 'Z' is 'up' on the cube.
If you treat the three accelerometer values as a vector and normalize it (to get N), you've got the new 'Z' axis portion of your rotation matrix so that a vector pointed in the z direction will now align with the 'up' vector.
| a d N.x | |0| |N.x|
| b e N.y | * |0| = |N.y|
| c f N.z | |1| |N.z|
So we need to decide what to do with a-f. One common thing to do is this: if N is pointing mostly along the original 'Z' axis, then make the new 'Y' axis portion of the matrix be M = N cross X:
d = 0
e = N.z
f = -N.y
Normalize M and then find the 'X' axis portion of the matrix: L = M cross N. Normalize L.
If N is not pointing mostly along the 'Z' axis (N.z < .707), then you find the new 'Y' axis portion as M = N cross Z. Normalize M and find L = M cross N and, finally, normalize L.
Edit:
So we have our three accelerometer values: A.x, A.y, A.z. First step is to normalize them:
a = sqrt(A.x*A.x + A.y*Ay + A.z*A.z); and then
N.x = A.x/a; N.y = A.y/a; N.z = A.z/a;
We assume that if N == [0, 0, 1] then the correct rotation matrix is the identity matrix. If N doesn't point directly along the z-axis, then we want to form a matrix that will rotate the z-axis of the cube so that it lines up with N.