I need the angular velocity expressed as a quaternion for updating the quaternion every frame with the following expression in OpenCV:
q(k)=q(k-1)*qwt;
My angular velocity is
Mat w; //1x3
I would like to obtain a quaternion form of the angles
Mat qwt; //1x4
I couldn't find information about this, any ideas?
If I understand properly you want to pass from this Axis Angle form to a quaternion.
As shown in the link, first you need to calculate the module of the angular velocity (multiplied by delta(t) between frames), and then apply the formulas.
A sample function for this would be
// w is equal to angular_velocity*time_between_frames
void quatFromAngularVelocity(Mat& qwt, const Mat& w)
{
const float x = w.at<float>(0);
const float y = w.at<float>(1);
const float z = w.at<float>(2);
const float angle = sqrt(x*x + y*y + z*z); // module of angular velocity
if (angle > 0.0) // the formulas from the link
{
qwt.at<float>(0) = x*sin(angle/2.0f)/angle;
qwt.at<float>(1) = y*sin(angle/2.0f)/angle;
qwt.at<float>(2) = z*sin(angle/2.0f)/angle;
qwt.at<float>(3) = cos(angle/2.0f);
} else // to avoid illegal expressions
{
qwt.at<float>(0) = qwt.at<float>(0)=qwt.at<float>(0)=0.0f;
qwt.at<float>(3) = 1.0f;
}
}
Almost every transformation regarding quaternions, 3D space, etc is gathered at this website.
You will find time derivatives for quaternions also.
I find it useful the explanation of the physical meaning of a quaternion, which can be seen as an axis angle where
a = angle of rotation
x,y,z = axis of rotation.
Then the conversion uses:
q = cos(a/2) + i ( x * sin(a/2)) + j (y * sin(a/2)) + k ( z * sin(a/2))
Here is explained thoroughly.
Hope this helped to make it clearer.
One little trick to go with this and get rid of those cos and sin functions. The time derivative of a quaternion q(t) is:
dq(t)/dt = 0.5 * x(t) * q(t)
Where, if the angular velocity is {w0, w1, w2} then x(t) is a quaternion of {0, w0, w1, w2}. See David H Eberly's book section 10.5 for proof
Related
I have a implemented this tutorial https://webglfundamentals.org/webgl/lessons/webgl-2d-rotation.html but there's some things I don't understand.
In the shader the rotations is applied by creating a new vec2 rotatedPosition:
...
vec2 rotatedPosition = vec2(
a_position.x * u_rotation.y + a_position.y * u_rotation.x,
a_position.y * u_rotation.y - a_position.x * u_rotation.x);
...
but how exactly is this actually providing a rotation? With rotation=[1,0]->u_rotation=vec2(1,0) the object is rotated 90 degrees. I understand the unit circle maths, what I don't understand is how the simple equation above actually performs the transformation.
a_position is a vec2, u_rotation is a vec2. If i do the calculation above outside the shader and feed it into the shader as a position, that position simply becomes vec2 rotatedPosition = vec2(y, -x). But inside the shader then this calculation vec2 rotatedPosition = vec2( a_position.x * u_rota... performs a rotation (it does not become vec2(y, -x) but instead uses a rotation matrix).
What magic ensures that rotatedPostion actually gets rotated when the vec2 is calculated inside the shader, as opposed to outside the shader? What 'tells' the vertex shader that it's supposed to do a rotation matrix calculation, as opposed to normal arithmetic?
Check the 2D rotation in Wikipedia. The magic (linear algebra) is that your u_rotation vector probably has the cos and sin from the angle θ in radians.
The rotation is counterclockwise of a two-dimensional Cartesian coordinate system. Example:
// your point
a = (a.x, a.y) = (1, 0)
// your angle in radians
θ = PI/2
// intermediaries
cos(θ) = 0
sin(θ) = 1
// your rotation vector
r = (r.x, r.y) = (cos(θ), sin(θ)) = (0, 1)
// your new `v` vector, the rotation of `a` around
// the center of the coordinate system, with angle `θ`
// be careful, 'v' to be a new vector, because if you try
// to reuse 'a' or 'r' you will mess the math
v.x = a.x * r.x - a.y * r.y = 1 * 0 - 0 * 1 = 0
v.y = a.x * r.y + a.y * r.x = 1 * 1 + 0 * 0 = 1
Here v will be x=0, y=1 as it should be. Your code does not seem to do the same.
Maybe you also want to know, how to rotate the point around an arbitrary other point, not always around the center of the coordinate system. You have to translate your point relatively to the new rotation center, rotate it, then translate it back like this:
// your arbitrary point to rotate around
center = (10, 10)
// translate (be careful not to subtract `a` from `center`)
a = a - center;
// rotate as before
...
// translate back
a = a + center;
I'm trying to convert Twist to the left and right wheels' speed with the formula:
float speed_wish_right = (cmd_vel.angle*WHEEL_DIST)/2 + cmd_vel.speed;
float speed_wish_left = cmd_vel.speed*2-speed_wish_right;
Twist.angular is a vector [x, y, z] and so is Twist.linear. What do x, y, z mean in the vector and how can I get angles and speed out of the two vectors?
This is my callback function in Arduino
const int WHEEL_DIST = 16;
void velCallback(geometry_msgs::Twist vel) {
float linear = vel.linear.x;
float angle = vel.angular.z;
float speed_wish_right = (angle * WHEEL_DIST) / 2 + linear;
float speed_wish_left = linear * 2 - speed_wish_right;
motors.setSpeeds(speed_wish_left, speed_wish_right);
}
Consider that you are in some space, then there are 3 axes - x, y and z which are mutually perpendicular to each other and their point of intersection is called the origin (x = 0, y = 0, z = 0). This can be a frame of reference i.e. you can define various points and directions w.r.t. them.
The x, y, and z in Twist.linear are the linear velocities in x, y and z directions w.r.t. that frame of reference.
Similarly, the x, y, and z in Twist.angular are the angular velocities about the x, y and z directions respectively w.r.t. the same frame of reference.
Since you have a ground robot, most probably your angular velocity will be in z i.e. robot's turning speed. And your linear velocity will be mostly in x i.e. robot's moving straight speed. This is the case for the Turtlebot 2 at least.
I am working on a project wich involves Aruco markers and opencv.
I am quite far in the project progress. I can read the rotation vectors and convert them to a rodrigues matrix using rodrigues() from opencv.
This is a example of a rodrigues matrix I get:
[0,1,0;
1,0,0;
0,0,-1]
I use the following code.
Mat m33(3, 3, CV_64F);
Mat measured_eulers(3, 1, CV_64F);
Rodrigues(rotationVectors, m33);
measured_eulers = rot2euler(m33);
Degree_euler = measured_eulers * 180 / CV_PI;
I use the predefined rot2euler to convert from rodrigues matrix to euler angles.
And I convert the received radians to degrees.
rot2euler looks like the following.
Mat rot2euler(const Mat & rotationMatrix)
{
Mat euler(3, 1, CV_64F);
double m00 = rotationMatrix.at<double>(0, 0);
double m02 = rotationMatrix.at<double>(0, 2);
double m10 = rotationMatrix.at<double>(1, 0);
double m11 = rotationMatrix.at<double>(1, 1);
double m12 = rotationMatrix.at<double>(1, 2);
double m20 = rotationMatrix.at<double>(2, 0);
double m22 = rotationMatrix.at<double>(2, 2);
double x, y, z;
// Assuming the angles are in radians.
if (m10 > 0.998) { // singularity at north pole
x = 0;
y = CV_PI / 2;
z = atan2(m02, m22);
}
else if (m10 < -0.998) { // singularity at south pole
x = 0;
y = -CV_PI / 2;
z = atan2(m02, m22);
}
else
{
x = atan2(-m12, m11);
y = asin(m10);
z = atan2(-m20, m00);
}
euler.at<double>(0) = x;
euler.at<double>(1) = y;
euler.at<double>(2) = z;
return euler;
}
If I use the rodrigues matrix I give as an example I get the following euler angles.
[0; 90; -180]
But I am suppose to get the following.
[-180; 0; 90]
When is use this tool http://danceswithcode.net/engineeringnotes/rotations_in_3d/demo3D/rotations_in_3d_tool.html
You can see that [0; 90; -180] doesn't match the rodrigues matrix but [-180; 0; 90] does. (I am aware of the fact that the tool works with ZYX coordinates)
So the problem is I get the correct values but in a wrong order.
Another problem is that this isn't always the case.
For example rodrigues matrix:
[1,0,0;
0,-1,0;
0,0,-1]
Provides me the correct euler angles.
If someone knows a solution to the problem or can provide me with a explanation how the rot2euler function works exactly. It will be higly appreciated.
Kind Regards
Brent Convens
I guess I am quite late but I'll answer it nonetheless.
Dont quote me on this, ie I'm not 100 % certain but this is one
of the files ( {OPENCV_INSTALLATION_DIR}/apps/interactive-calibration/rotationConverters.cpp ) from the source code of openCV 3.3
It seems to me that openCV is giving you Y-Z-X ( similar to what is being shown in the code above )
Why I said I wasn't sure because I just looked at the source code of cv::Rodrigues and it doesnt seem to call this piece of code that I have shown above. The Rodrigues function has the math harcoded into it ( and I think it can be checked by Taking the 2 rotation matrices and multiplying them as - R = Ry * Rz * Rx and then looking at the place in the code where there is a acos(R(2,0)) or asin(R(0,2) or something similar,since one of the elements of "R" will usually be a cos() or sine which will give you a solution as to which angle is being found.
Not specific to OpenCV, but you could write something like this:
cosine_for_pitch = math.sqrt(pose_mat[0][0] ** 2 + pose_mat[1][0] ** 2)
is_singular = cosine_for_pitch < 10**-6
if not is_singular:
yaw = math.atan2(pose_mat[1][0], pose_mat[0][0])
pitch = math.atan2(-pose_mat[2][0], cosine_for_pitch)
roll = math.atan2(pose_mat[2][1], pose_mat[2][2])
else:
yaw = math.atan2(-pose_mat[1][2], pose_mat[1][1])
pitch = math.atan2(-pose_mat[2][0], cosine_for_pitch)
roll = 0
Here, you could explore more:
https://www.learnopencv.com/rotation-matrix-to-euler-angles/
http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf
I propose to use the PCL library to do that with this formulation
pcl::getEulerAngles(transformatoin,roll,pitch,yaw);
you need just to initialize the roll, pitch, yaw and a pre-calculated transformation matrix you can do it
WebGL draws coordinates that vary from -1 to 1. These coordinates become normalized by dividing by w -- the perspective divide. How does this happen with an orthographic projection because the orthographic projection matrix is the identity matrix. That is w will remain 1. How are the coordinates then normalized from [-1,1] with an orthographic projection?
What do you mean by "normalized"?
WebGL doesn't care what your matrices are it just cares what you set gl_Position to.
A typical orthographic matrix just scales and translates x and y (and z) and sets w to 1.
The formula for how what you set gl_Position to gets converted to a pixel is something like
var x = gl_Position.x / gl.Position.w;
var y = gl_Position.y / gl.Position.w;
// convert from -1 <-> 1 to 0 to 1
var zeroToOneX = x * 0.5 + 0.5;
var zeroToOneY = y * 0.5 + 0.5;
// convert from 0 <-> 1 to viewportX <-> (viewportX + viewportWidth)
// and do something similar for Y
var pixelX = viewportX + zeroToOneX * viewportWidth;
var pixelY = viewportY + zeroToOneY * viewportHeight;
Where viewportX, viewportY, viewportWidth, and viewportHeight are set with gl.viewport
If you want the exact formula you can look in the spec under rasterization.
Maybe you might find these tutorials helpful.
I have two items, lets call them Obj1 and Obj2... Both have a current position pos1 and pos2.. Moreover they have current velocity vectors speed1 and speed2 ... How can I make sure that if their distances are getting closer (with checking current and NEXT distance), they will move farther away from eachother ?
I have a signed angle function that gives me the signed angle between 2 vectors.. How can I utilize it to check how much should I rotate the speed1 and speed2 to move those sprites from eachother ?
public float signedAngle(Vector2 v1, Vector2 v2)
{
float perpDot = v1.X * v2.Y - v1.Y * v2.X;
return (float)Math.Atan2(perpDot, Vector2.Dot(v1, v2));
}
I check the NEXT and CURRENT distances like that :
float currentDistance = Vector2.Distance(s1.position, s2.position);
Vector2 obj2_nextpos = s2.position + s2.speed + s2.drag;
Vector2 obj1_nextpos = s1.position + s1.speed + s1.drag;
Vector2 s2us = s2.speed;
s2us.Normalize();
Vector2 s1us = s1.speed;
s1us.Normalize();
float nextDistance = Vector2.Distance(obj1_nextpos , obj2_nextpos );
Then depending whether they are getting bigger or smaller I want to move them away (either by increasing their current speed at the same direction or MAKING THEM FURTHER WHICH I FAIL)...
if (nextDistance < currentDistance )
{
float angle = MathHelper.ToRadians(180)- signedAngle(s1us, s2us);
s1.speed += Vector2.Transform(s1us, Matrix.CreateRotationZ(angle)) * esc;
s2.speed += Vector2.Transform(s2us, Matrix.CreateRotationZ(angle)) * esc;
}
Any ideas ?
if objects A and B are getting closer, one of the object components (X or Y) is opposite.
in this case Bx is opposite to Ax, so only have to add Ax to the velocity vector of object B, and Bx to velocity vector of object A
If I understood correctly, this is the situation and you want to obtain the two green vectors.
The red vector is easy to get: redVect = pos1 - pos2. redVect and greenVect2 will point to the same direction, so the only step you have is to scale it so its length will match speed2's one: finalGreenVect2 = greenvect2.Normalize() * speed2.Length (although I'm not actually sure about this formula). greenVect1 = -redVect so finalGreenVect1 = greenVect1.Normalize() * speed1.Length. Then speed1 = finalGreenVect1 and speed2 = finalGreenVect2. This approach will give you instant turn, if you prefer a smooth turn you want to rotate the speed vector by:
angle = signedAngle(speed) + (signedAngle(greenVect) - signedAngle(speed)) * 0.5f;
The o.5f is the rotation speed, adjust it to any value you need. I'm afraid that you have to create a rotation matrix then Transform() the speed vector with this matrix.
Hope this helps ;)