I have a 3D point in space, and I need to know how to pitch/yaw/roll my current heading (in the form of a 3d unit vector) to face a point. I am familiar with quaternions and rotation matrices, and I know how to represent the total rotation necessary to get my desired answer.
However, I only have control over pitch, yaw, and roll velocities (I can 'instantaneously' set their respective angular velocities), and only occasional updates on my new orientation (once every second or so). The end goal is to have some sort of PID controller (or three separates ones, but I suspect it won't work like that) controlling my current orientation. the end effect would be a slow (and hopefully convergent) wobble towards a steady state in the direction of my destination.
I have no idea how to convert the current desired quaternion/rotation matrix into a set of pitch-yaw-roll angular velocities (some sort of quaternion derivative or something?). I'm not even sure what to search for. I'm also uncertain how to apply a PID controller to this system, because I suspect there will need to be one controller for the trio as opposed to treating them each independently (although intuitively I feel this should be possible). Can anyone offer any guidance?
As a side note, if there is a solution that just involves a duo (pitch/yaw, roll/pitch, etc), then that works just fine too. I should only need 2 rotational degrees of freedom for this, but that is further from a realm that I am familiar with so I was less confident forming the question around it.
First take a look if your problem can be solved using quaternion SLERP [1], which can let you specify a scalar between 0 and 1 as the control to move from q1-->q2.
If you still need to control using the angular rotations then you can calculate the error quaternion as Nico Schertler suggested.
From that error quaternion you can use the derivative property of the quaternion (Section 4 of http://www.ecsutton.ece.ufl.edu/ens/handouts/quaternions.pdf [2]) to work out the angular rates required.
I'm pretty sure that will work, but if it does not you can also look at using the SLERP derivative (eq. 23 of http://www.geometrictools.com/Documentation/Quaternions.pdf [3]) and equating that to the Right-Hand-Side of the equation in source [2] to again get angular rates. The disadvantage to this is that you need code implementations for the quaternion exponentiation and logarithm operations.
Related
I have successfully calculated Rotation, Translation with the intrinsic camera matrix of two cameras.
I also got rectified images from the left and right cameras. Now, I wonder how I calculate the 3D coordinate of a point, just one point in an image. Here, please see the green points. I have a look at the equation, but it requires baseline which I don't know how to calculate. Could you show me the process of calculating the 3d coordinate of the green point with the given information (R, T, and intrinsic matrix)?
FYI
1. I also have a Fundamental matrix and Essential matrix, just in case we need them.
2. Original image size is 960 x 720. Rectified ones are 925 x 669
3. The green point from the left image: (562, 185), from the right image: (542, 185)
The term "baseline" usually just means translation. Since you already have your rotation, translation and intrinsics matrices (let's not them R, T and K). you can triangulate and don't need either the Fundamental or Essential matrices (they could be used to extract R, T etc but you already have them). You don't really need your images to be rectified either, since it doesn't change the triangulation process that much. There are many ways to triangulate, each with their pros and cons, and many libraries that implement them. So, all I can do here is give you and overview of the problem and potential solutions, as well as pointers to resources that you can either use as their are or as a source of inspiration to write your own code.
Formalization and solution outlines. Let's formalize what we are after here. You have a 3d point X, with two observations x_1 and x_2 respectively in the left and right images. If you backproject them, you obtain two rays:
ray_1=K^{1}x_1
rat_2=R*K^{-1}x_2+T //I'm assuming that [R|T] is the pose of the second camera expressed in the referential of the first camera
Ideally, you'd want those two rays to meet at point X. Since in practice we always have some noise (discretization noise, rounding errors and so on) the two rays wont meet at X, so the best answer would be a point Q such that
Q=argmin_X {d(X,ray_1)^2+d(X,ray_2)^2}
where d(.) denotes the Euclidian distance between a line and a point. You can solve this problem as a regular least squares problem, or you can just take the geometric approach (called midpoint) of considering the line segment l that is perpendicular to both ray_1 and ray_2, and take its middle as your solution. Another quick and dirty way is to use the DLT. Basically, you re-write the constrains (i.e. X should be as close as possible to both rays) as a linear system AX=0 and solve it with SVD.
Usually, the geometric (midpoint) method is the less precise. The DLT based one, while not the most stable numerically, usually produces acceptable results.
Ressources that present in depth formalization
Hartley-Zisserman's book of course! Chapter 12. A simple DLT-based method, which is the one used in opencv (both in the calibration and sfm modules) is explained on page 312. It is very easy to implement, it shouldn't take more that 10 minutes in any language.
Szeliski'st book. It has an intersting discussion on triangulation in the chapter on SFM, but is not as straight-forward or in depth as Hartley-Zisserman's.
Code. You can use the triangulation methods from opencv, either from the calib3d module, or from the contribs/sfm module. Both use the DLT, but the code from the SFM module is more easily understandable (the calib3d code has a lot of old-school C code which is not very pleasant to read). There is also another lib, called openGV, which has a few interesting methods for triangulation.
cv::triangulatePoints
cv::sfm::triangulatePoints
OpenGV
The openGV git repo doesn't seem very active, and I'm not a big fan of the design of the library, but if I remember correctly (feel free to tell me otherwise) it offers methods other that the DLT for triangulations.
Naturally, those are all written in C++, but if you use other languages, finding wrappers or similar libraries wont be difficult (with python you still have opencv wrappers, and MATLAB has a bundle module, etc.).
Background:
I am currently implementing a skeletal animation shader in GLSL, and to save space and complexity I am using Quaternions for the bone rotations, using weighted quaternion multiplication (of each bone) to accumulate a "final rotation" for each vertex.
Something like: (pseudo-code, just assume the quaternion math works as expected)
float weights[5];
int bones[5];
vec4 position;
uniform quaternion allBoneRotations[100];
uniform vec3 allBonePositions[100];
main(){
quaternion finalQuaternion;
for(i=0;i<5;i++){finalQuaternion *= allBoneRotations[bones[i]]*weights[i];}
gl_position = position.rotateByQuaternion(finalQuaternion);
}
The real code is complicated, sloppy, and working as expected, but this should give the general idea, since this is mostly a math question anyway, the code isn't of much consequence, it's just provided for clarity.
Problem:
I was in the process of adding "pivot points"/"joint locations" to each bone (negative translate, rotate by "final quaternion", translate back) when I realized that the "final quaternion" will not have taken the different pivot points into account when combining the quaternions themselves. In this case each bone rotation will have been treated as if it was around point (0,0,0).
Given that quaternions represent only a rotation, it seems I'll either need to "add" a position to the quaternions (if possible), or simply convert all of the quaternions into matrices, then do matrix multiplication to combine the series of translations and rotations. I am really hoping the latter is not necessary, since it seems like it would be really inefficient, comparatively.
I've searched through mathoverflow, math.stackexchange, and whatever else Google provided and read the following resources so far in hopes of figuring out an answer myself:
http://shankel.best.vwh.net/QuatRot.html
http://mathworld.wolfram.com/Quaternion.html
plus various other small discussions found through Googling (I can only post 2 links)
The consensus is that Quaternions do not encode "translation" or "position" in any sense, and don't seem to provide an intuitive way to simulate it, so pure quaternion math seems unlikely to be a viable solution.
However it might be nice to have a definitive answer to this here. Does anyone know any way to "fake" a position component of a quaternion, that in some way that would keep the quaternion math efficiency, or some other method to "accumulate" rotations around different origin points that is more efficient than just computing the matrix of the quaternions, and doing matrix translation and rotation multiplications for each and every quaternion? Or perhaps some mathematical assurance that differing pivot points don't actually make any difference, and can, in fact be applied later (but I doubt it).
Or is using quaternions in this situation just a bad idea on the face of it?
Indeed, there is no such thing as a position component of a quaternion, so you'll need to track it separately. Suppose individual transformations end up being like
x' = R(q)*(x-pivot)+pivot = R(q)*x + (pivot-R(q)*pivot) = R(q)*x+p,
where q is your quaternion, R(q) is the rotation matrix built from it, and p=pivot-R(q)*pivot is the position/translation component. If you want to combine two such transformations, you can do it without going full-matrix multiplication:
x'' = R(q2)*x'+p2 = R(q2)*R(q)*x + (R(q2)*p+p2) = R(q2*q)*x + (R(q2)*p+p2).
This way the combined quaternion will be q2*q, and the combined position, R(q2)*p+p2. Note that you can even apply quaternions to vectors (R(q2)*p and so on) without explicitly building rotation matrices, if you want to absolutely avoid them.
That said, there is also a notion of "dual quaternions" which, in fact, do contain a translation component, and are presumably better for representing screw motions. Check them out on Wiki, and here (the last link also points to a paper).
After extensive additional searching, and reading more about quaternions than any sane person should, I finally discovered my answer here:
http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/other/dualQuaternion/index.htm
It turns out Dual Quaternions operate similarly to actual quaternions, with many of the mathematical operations based off of regular quaternion math, but they provide both orientation, and displacement both, and can be combined for any rotation-translation sequence needed, much like Transformation Matrix multiplication, but without the shear/scale ability.
The page also has a section that derives exactly the "rotating around an arbitrary point" functionality that I was requiring by using dual quaternion multiplication. Perhaps I should have researched a bit more before asking, but at least the answer is here now in case anyone else comes looking.
I am quite new to this, and iv'e heard that i need to get my inversed projection matrix and so on to create a ray from a 2D point to a 3D world point, however since im using OpenglES and there are not as many methods as there would be regulary to help me with this. (And i simply don't know how to do it) im using a trigenomeric formula for this insted.
For each time i iterate one step down the negative Z-axis i multiply the Y-position on the screen (-1 to 1) with
(-z / (cot(myAngle / 2))
And the X position likewise but with a koefficent equally to the aspect ratio.
myAngle is the frustum perspective angle.
This works really good for me and i get very accurate values, so what i wonder is: Why should i use the inverse of the projection matrix and multiply it with some stuff instead of using this?
Most of the time you have a matrix lying around for your OpenGl camera. Using an inverse matrix is simple when you already have a camera matrix on hand. It is also (oh so very slightly at computer speeds) faster to do a matrix multiply. And in cases where you are doing a bajillion of these calculations per frame, it can matter.
Here is some good info on getting started on a camera class if you are interested:
Camera Class
And some matrix resources
Depending on what you are working on, I wouldn't worry too much about the 'best way to do it.' You just want to make sure you understand what your code is doing then keep improving it.
I am using a 3D engine called Electro which is programmed using Lua. It's not a very good 3D engine, but I don't have any choice in the matter.
Anyway, I'm trying to take a flat quadrilateral and transform it to be in a specific location and orientation. I know exactly where it is supposed to go (i.e. I know the exact vertices where the corners should end up), but I'm hitting a snag in getting it rotated to the right place.
Electro does not allow you to apply transformation matrices. Instead, you must transform models by using built-in scale, position (that is, translate), and rotation functions. The rotation function takes an object and 3 angles (in degrees):
E.set_entity_rotation(entity, xangle, yangle, zangle)
The documentation does not speficy this, but after looking through Electro's source, I'm reasonably certain that the rotation is applied in order of X rotation -> Y rotation -> Z rotation.
My question is this: If my starting object is a flat quadrilateral lying on the X-Z plane centered at the origin, and the destination position is in a different location and orientation where the destination vertices are known, how could I use Electro's rotation function to rotate it into the correct orientation before I move it to the correct place?
I've been racking my brain for two days trying to figure this out, looking at math that I don't understand dealing with Euler angles and such, but I'm still lost. Can anyone help me out?
Can you tell us more about the problem? It sounds odd phrased in this way. What else do you know about the final orientation you have to hit? Is it completely arbitrary or user-specified or can you use more knowledge to help solve the problem? Is there any other Electro API you could use to help?
If you really must solve this general problem, then too bad, it's hard, and underspecified. Here's some guy's code that may work, from euclideanspace.com.
First do the translation to bring one corner of the quadrilateral to the point you'd like it to be, then apply the three rotational transformations in succession:
If you know where the quad is, and you know exactly where it needs to go, and you're certain that there are no distortions of the quad to fit it into the place where it needs to go, then you should be able to figure out the angles using the vector scalar product.
If you have two vectors, the angle between them can be calculated by taking the dot product.
So I have written a Quaternion based 3D Camera oriented toward new programmers so it is ultra easy for them to integrate and begin using.
While I was developing it, at first I would take user input as Euler angles, then generate a Quaternion based off of the input for that frame. I would then take the Camera's Quaternion and multiply it by the one we generated for the input, and in theory that should simply add the input rotation to the current state of the camera's rotation, and things would be all fat and happy. Lets call this: Accumulating Quaternions, because we are storing and adding Quaternions only.
But I noticed that there was a problem with this method. The more I used it, even if I was only rotating on one Euler angle, say Yaw, it would, over some iterations, begin bleeding over into another, say Pitch. It was slight, but fairly unacceptable.
So I did some more research and found an article stating it was better to accumulate Euler angles, so the camera stores it's current rotation as Euler angles, and input is simply added to them each frame. Then I generate a Quaternion from them each frame, which is in turn used to generate my rotation matrix. And this fixed the issue of rotation bleeding into improper axes.
So do any Stackoverflow members have any insight into this problem? Is that a proper way of doing things?
Multiplying quaternions is going to suffer from accumulation of floating-point roundoff issues (even simple angles like 45 degrees won't be exact). It's a great way to composite rotations, but the precision of each of your quaternion components is going to drop-off over time. The bleed-through is one side-effect, a visually worse one though is your quaternion could start incorporating a scale factor - to recover that, you'd have to renormalize back to Euler angles in any case. A fixed-point Euler angle isn't going to accumulate roundoff.
Recalculating the quaternion per-frame is minimal. I wouldn't bother trying to optimize it out. You could probably allow a few quaternions to accumulate before you renormalized to get the accuracy back, but it really isn't worth the effort.
Accumulation is an inexact process. Accumulating lots of incremental rotations will accumulate roundoff error whether you do it with quaternions or matrices.
I imagine something like this: you got your code up and running, but noticed that after a certain amount of navigation your camera was heeling over annoyingly -- violating an invariant you hadn't thought of in advance. Effectively, you've realized you don't want to accumulate rotations; instead you want to do something else.
You can look at this as more of an interface design issue than a numerical accuracy issue. Basically, people expect a camera to navigate according to pitch, yaw, and roll, so choosing to control and represent the angles directly can avoid a lot of problems.
The bummer here is that the quaterions seem to have become redundant (for this particular usage, at least). You still want the quaternions, though -- interpolating with the raw pitch/yaw/roll angles can be ugly. Again, it's an interface design question: you need to figure out where you'll need the quaternions, and how to get them in and out...
I've seen both argued for. I think the real question you'll have to deal with is flexibility in your camera system down the line; IMO yaw is generally more interesting in a third-person view (because you're going to rotate about the character's vertical axis). While you can arguably "yaw" around the vertical in first-person view as well, I'm not sure it's really the same thing.
However, I do think it's kind of a waste to recalculate your quaternions per-frame. Perhaps it would be better to store the latest quaternions and mark them dirty if your frame receives input?