I am working with the gyroscope and obtaining the rotationMatrix that I will use to rotate an object on SceneKit.
That rotation matrix coming from the gyro uses iPhone's axis:
But my application works landscape with the home button left. So, my axis are:
x = iphone's Y
y = negative iphone's X
z = iphone's Z
So, I need to take the rotation matrix I am receiving from the accelerometer that is based on [x,y,z] and create a new one that is [y, -x, -z]... (yes, negative Z because for this particular case I need the object to rotate against Z).
Ok, making it rotate negative is easy but how do I switch the axis X and Y from the original matrix to a new one?
This is what I have so far:
// I create a GLKMatrix4 from the CMRotationMatrix
GLKMatrix4 transform = GLKMatrix4Make(rotMatrix.m11, rotMatrix.m21, rotMatrix.m31, 0.0,
rotMatrix.m12, rotMatrix.m22, rotMatrix.m32, 0.0,
rotMatrix.m13, rotMatrix.m23, rotMatrix.m33, 0.0,
0.0, 0.0, 0.0, 1.0);
GLKMatrix4 negativeX = GLKMatrix4MakeXRotation(-M_PI);
GLKMatrix4 rotate = GLKMatrix4Multiply(transform, negativeX);
GLKMatrix4 negativeZ = GLKMatrix4MakeZRotation(-M_PI);
rotate = GLKMatrix4Multiply(rotate, negativeZ);
// ok, now I have [-x, y, -z]
// how do I switch X and Y and transform that into [y, -x, -z]?
A 3x3 matrix, such as your rotation matrix, is applied as:
[a b c] [x] [a*x + b*y + c*z] x * (a, e, i) +
[e f g] [y] = [e*x + f*y + g*z] = y * (b, f, j) +
[i j k] [z] [i*x + j*y + k*z] z * (c, g, z)
i.e. it is literally three vectors. In your rotMatrix, (m11, m12, m13) is the vector that tells you the direction the transformed x axis will take, (m21, m22, m23) is y and (m31, m32, m33) is z.
If what you're currently using for the x vector is what you actually want to use for the y value, just swap the columns. If you want to negate one axis, just negate the column.
Related
I place an IMU on my wrist and extend my arm as shown in the photo below. I spin in a circle once, while my arm remains in the fixed position. I calculate the euler pitch and quaternion angle. In the photo below, the euler pitch remains approximately constant ( my hand shakes a bit ), while the quaternion angle increases linearly it seems. My data is located here: (sample_data.csv)
Question:
What changes should I make to measure the angle from the quaternion? ( I suspect it is in form of RPR', adjust to world axis, but am unsure what P would be )
Matlab code:
clc;
clear;
table = readtable("sample_data.csv", 'Delimiter', ',');
euler_angles = zeros(length(table.w),1);
quaternion_angles = zeros(length(table.w),1);
for idx = 1:length(table.w)
w = table.w(idx);
x = table.x(idx);
y = table.y(idx);
z = table.z(idx);
euler_angles(idx) = getEulerAngle(w,x,y,z);
quaternion_angles(idx) = getQuaternionAngle(w,x,y,z);
end
figure(1);
clf;
hold on;
plot(euler_angles,'ro');
ylabel("Angle in deg");
xlabel("Sample");
plot(quaternion_angles, 'bo');
hold off;
legend('Euler','Quaternion');
function angle = getQuaternionAngle(w,x,y,z)
q = quaternion(w,x,y,z);
angle = acosd(w);
end
function angle = getEulerAngle(w, x, y, z)
mag = (2*(y * w - z * x));
angle = rad2deg(asin(mag));
end
Assuming you rotate around an axis [x;y;z] , the post you refer to states that the quaternion associated with the rotation is of the form:
q(1) = cos(r/2);
q(2) = sin(r/2)*x;
q(3) = sin(r/2)*y;
q(4) = sin(r/2)*z;
Where r is the angle in radians and [x;y;z] is the 3d vector representing the axis around which you rotate.
To get the instantaneous rotation r you need to calculate 2*acos(q(1)) for an angle in radians, or 2*acosd(q(1)) for an angle in degrees.
Now plotting the data from your csv file gives the following:
Which is coherent with the assumption that w is the first coordinate of your quaternion, and that you indeed rotate mostly around z.
Euler angles (Or more likely Tait Bryan angles) are a different way to represent a rotation. A rotation is represented by a composition of 3 elemental rotations. These 3 rotations are sometimes called yaw, pitch and roll. If you want to fully represent your rotation, you will have to calculate all of these 3 elemental rotations:
table = readtable("sample_data.csv", 'Delimiter', ',');
w = table.w;
x = table.x;
y = table.y;
z = table.z;
[yaw,pitch,roll] = getAngles(w, x, y, z);
quaternion_angles = getQuaternionAngle(w);
figure; plot([quaternion_angles,yaw,pitch,roll]);
legend({'quat','yaw','pitch','roll'})
function angle = getQuaternionAngle(w)
angle = 2*acosd(w);
end
function [yaw,pitch,roll] = getAngles(w, x, y, z)
yaw = atan2d(2*(y.*z + w.*x), w.*w - x.*x - y.*y + z.*z);
pitch = asind(-2*(x.*z-w.*y));
roll = atan2d(2*(x.*y + w.*z), w.*w + x.*x - y.*y - z.*z);
end
See? The angle around Z is represented by the roll (the discontinuity is due to the use of arctan). When you rotate around yourself, the angle increases steadily between 0 and 360ยบ.
You can also see that you have a bit of yaw, i.e your IMU is a bit tilted
I have this piece of MATLAB code that outputs x,y, and z angles and I would like draw a line using them. Can someone point me in the right direction on how to do this?
C = pi;
A = pi;
B = pi;
Z = [cos(C),-sin(C),0; sin(C),cos(C),0; 0,0,1];
X = [1,0,0;0,cos(A),-sin(A);0,sin(A),cos(A)];
Y = [cos(B),0,sin(B);0,1,0;-sin(B),0,cos(B)];
R =(X*Y)*Z;
yaw=atan2(R(2,1),R(1,1))
pitch=atan2(-R(3,1),sqrt(R(3,2)^2+R(3,3)^2))
roll=atan2(R(3,2),R(3,3))
X, Y, and Z are not angles, they are rotation matrices defined by the angles A, B, and C.
it's not clear what's the meaning of "draw a line using them", they are just used to rotate vectors in the 3D space.
here is an example of drawing a rotated vector with them:
% define rotation angles (around the axes)
C = pi/2;
A = pi/4;
B = pi/4;
% generate rotation matrices
Z = [cos(C),-sin(C),0; sin(C),cos(C),0; 0,0,1];
X = [1,0,0;0,cos(A),-sin(A);0,sin(A),cos(A)];
Y = [cos(B),0,sin(B);0,1,0;-sin(B),0,cos(B)];
R =(X*Y)*Z;
% generate a vector and rotate it
v = [1;1;1];
u = R*v;
% plot
quiver3(0,0,0,v(1),v(2),v(3));
hold on
quiver3(0,0,0,u(1),u(2),u(3));
xlim([-1 1]); ylim([-1 1]); zlim([-1 1])
axis square
legend('original','rotated')
I have calibrated my camera and I have now cameraParams, rotation and translation matrices (R ,t)
I know that there is a way to get the world coordinates from the pixel indices by the function "pointsToWorld(__)" but I want to do the otherwise , I can't find anything about that in the Matlab help !
So I don't know what to do, any suggestions?
Currently you have to do that yourself. If you have R and t, you can use the cameraMatrix function to compute the camera projection matrix P. Then you can compute the projection of a world point into the image as follows:
P = cameraMatrix(cameraParams, R, t);
p = [X, Y, Z, 1] * P;
x = p(1) / p(3);
y = p(2) / p(3);
X, Y, and Z are the world coordinates. x and y are the image coordinates in pixels.
Is it possible to draw a feather plot of inclined arrows, over a non-horizontal axis in Matlab? for instance, over a circle or other curve.
Thanks!
Sure. You want to use quiver and define the tails (x and y) and direction vectors (dx and dy)
t = linspace(0, 2*pi, 20);
x = cos(t);
y = sin(t);
dx = 0.25 * x;
dy = 0.25 * y;
q = quiver(x, y, dx, dy, 1);
You'll need to do a little math to come up with the exact positions and vectors based upon your problem but this should definitely be able to give you the desired result.
I have this code, which takes a meshgrid, and applies a transformation to every point:
function [newx, newy] = transform(x, y)
newx = 10 * x + y*y;
newy = 5 * y;
end
[x, y] = meshgrid(1:5, 1:5);
[u, v] = arrayfun(#transform, x, y);
I want to plot the new mesh in 2D. The closest I can get is to do so in 3D by adding a Z component of 0:
mesh(u, v, zeros(size(u)))
How can I get matlab/octave to just show this plot on a 2d set of axes?
Maybe I'm missing the point here, but what's wrong with a simple plot(u,v,'b-x',u',v','b-x')?