6 DOF Distance formula
How to find the distance between two 3d points with angular movement(yaw,pitch,roll.
For simple 3d points use
sqrt((x1-x2)(x1-x2)+(y1-y2)(y1-y2)+(z1-z2)(z1-z2))
but how to find with yaw pitch roll involve in it.
-----------------------
wwww.ashscholar.com
Yaw, pitch, and roll are rotations, they don't have anything to do with distance. Please restate your question more clearly if this answer doesn't satisfy you.
Any transformation that does not change the distance between 2 points is a rotation, by definition. This is true if both points are defined in the same space. Thus if your two points were in the same space to begin with, the rotation will not change the distance between them.
If the points are in two different spaces, then a rotation of one of the spaces can affect the distance between the points. For instance, if one point is in world space, and the other point is specified in the local space of a model, then if the model rotates, then the world space representation of the model point rotates as well, and the distance between the two points changes.
If this is the type of setup that you are referring to, you need to get both of your points into the same space before you can apply the distance formula.
Here is a possible function if you want to find the distance between a point in world space (W), and a point in model space (M). This assumes that the yaw (y), pitch (p) and roll (r) angles are in the correct domains, and are given in radians. Also, this function allows the origin of the model space to be translated (T) from the origin in world space. If the model space and the world space share the same origin, then T is just zero.
Of course, if I were going to do this, I would prefer the rotation to be represented by a matrix, rather than angles, so that I wouldn't have to do all of the sines and cosines every time I wanted to check a distance between a point in world space, and a point in the model space.
If the points are in two different spaces, then a rotation of one of the spaces can affect the distance between the points. For instance, if one point is in world space, and the other point is specified in the local space of a model, then if the model rotates, then the world space representation of the model point rotates as well, and the distance between the two points changes.
If this is the type of setup that you are referring to, you need to get both of your points into the same space before you can apply the distance formula.
Here is a possible function if you want to find the distance between a point in world space (W), and a point in model space (M). This assumes that the yaw (y), pitch (p) and roll (r) angles are in the correct domains, and are given in radians. Also, this function allows the origin of the model space to be translated (T) from the origin in world space. If the model space and the world space share the same origin, then T is just zero.
float distance(const Vector& W, cont Vector& M, float y, float p, float r, const Vector& T){ //evaluate all of the sines and cosines float cy = cos(y); float cp = cos(p); float cr = cos(r); float sy = sin(y); float sp = sin(p); float sr = sin(r); //calculate the components of the rotation matrix float m11 = cr * cp; float m12 = cr * sp * sy - sr * cy; float m13 = cr * sp * cy + sr * sy; float m21 = sr * cp; float m22 = sr * sp * sy + cr * cy; float m23 = sr * sp * cy - cr * sy; float m31 = -sr; float m32 = cp * sy; float m33 = cr * cy; //calculate the deltas float dx = W.x - M.x * m11 - M.y * m12 - M.z * m13 - T.x; float dy = W.y - M.x * m21 - M.y * m22 - M.z * m23 - T.y; float dz = W.z - M.x * m31 - M.y * m32 - M.z * m33 - T.z; //return the distance return sqrt(dx*dx + dy*dy + dz*dz);}
Of course, if I were going to do this, I would prefer the rotation to be represented by a matrix, rather than angles, so that I wouldn't have to do all of the sines and cosines every time I wanted to check a distance between a point in world space, and a point in the model space.
There is a natural way to talk about distance between "points with angular movement."
A position together with an orientation -- a frame, positioned somewhere in space -- is called an element of SE(3), the "Special Euclidean Group." You can think of a point G in SE(3) as a rotation matrix R together with a point p; i.e., G=(R,p). The notion of distance is the geodesic distance on this manifold (in the same way that there is a shortest distance on the sphere between two points on the sphere).
Here is how you compute it. Given two elements G1=(R1,p1), G2=(R2,p2) of SE(3), the geodesic distance between them is
dist(G1, G2) = sqrt(dist(R1,R2)^2 + dist(p1, p2)^2)
where
dist(p1, p2) = ||p2 - p1||
is the usual Euclidean distance, and dist(R1, R2) is the angle through which the rotation matrix R2 R1^T rotates. To compute it, just convert (R2 R1^T) to axis-angle coordinates; the distance is the angle. For how to do that, see here; in particular, the angle is,
.
A position together with an orientation -- a frame, positioned somewhere in space -- is called an element of SE(3), the "Special Euclidean Group." You can think of a point G in SE(3) as a rotation matrix R together with a point p; i.e., G=(R,p). The notion of distance is the geodesic distance on this manifold (in the same way that there is a shortest distance on the sphere between two points on the sphere).
Here is how you compute it. Given two elements G1=(R1,p1), G2=(R2,p2) of SE(3), the geodesic distance between them is
dist(G1, G2) = sqrt(dist(R1,R2)^2 + dist(p1, p2)^2)
where
dist(p1, p2) = ||p2 - p1||
is the usual Euclidean distance, and dist(R1, R2) is the angle through which the rotation matrix R2 R1^T rotates. To compute it, just convert (R2 R1^T) to axis-angle coordinates; the distance is the angle. For how to do that, see here; in particular, the angle is,
.
Quote:Original post by Eric_Brown
Any transformation that does not change the distance between 2 points is a rotation, by definition.
A rotation is an isometry, but not all isometries are rotations.
Quote:Original post by nilkn
A rotation is an isometry, but not all isometries are rotations.
To expand/clarify: There are also reflections.
I have an objection to Emergent's distance definition: It depends on the units used. The usual euclidean distance has units of length, but the distance in the space of rotations has units of radians (or "doesn't have units", more correctly). You need to multiply the angle part by a proportionality constant with units of length.
Quote:Original post by alvaro
I have an objection to Emergent's distance definition: It depends on the units used. The usual euclidean distance has units of length, but the distance in the space of rotations has units of radians (or "doesn't have units", more correctly). You need to multiply the angle part by a proportionality constant with units of length.
Hmmmm.... yeah, that throws a wrench in...
If you're thinking about SE(3) as the configuration manifold for a solid body, then the natural Riemannian metric on this space is the Kinetic Energy metric, which represented in coordinates is just the generalized mass matrix...
Unfortunately, that makes what the geodesics are and hence what the distances are less obvious, so I'd need to think about it...
My problem is that i have lots of orientation values at same position i want to find minimum orientation value
for example
x=3 y=4 z=9 yaw=70 pitch=50 roll=40
i have following values in DB i want closest value
x1=3 y1=4 z1=9 yaw1=80 pitch1=40 roll1=150
x2=3 y2=4 z2=9 yaw2=170 pitch2=50 roll2=140
x3=3 y3=4 z3=9 yaw3=80 pitch3=40 roll3=50
All above values are same position but with respect to orientation yaw3,pitch3.roll3 is closest i want this values which formula or algorithm i apply to find minimum values of orientation
for example
x=3 y=4 z=9 yaw=70 pitch=50 roll=40
i have following values in DB i want closest value
x1=3 y1=4 z1=9 yaw1=80 pitch1=40 roll1=150
x2=3 y2=4 z2=9 yaw2=170 pitch2=50 roll2=140
x3=3 y3=4 z3=9 yaw3=80 pitch3=40 roll3=50
All above values are same position but with respect to orientation yaw3,pitch3.roll3 is closest i want this values which formula or algorithm i apply to find minimum values of orientation
Oh, I see.
If all of the points are at the same position, then you only need to worry about the angular peice.
Find the rotation matrix, or quaternion that represents the set of angles. I prefer quaternions. You construct the quaternion like this
Here ψ is the yaw angle, θ is the pitch angle, and φ is the roll angle. Of course, these angles are given within their canonical ranges.
If you have 2 quaternions that represent 2 different rotations, then you can always construct a single quaternion that will take you from one rotation to the other
If you want to find the angle that this delta quaternion represents, you use the scalar part of Δq in the following formula.
Note, that you don't have to perform a full blown quaternion multiply if you want to find the scalar part of Δ q. The scalar part of this product is just the 4D dot product of the two quaternions q1 and q2.
It seems to me that what you are looking for is the set of coordinates that minimizes the angle α.
I don't recommend using matrices to solve this problem, because it is more expensive all around. But if you were using matrices instead, you would find the delta matrix
Then you would use the formula posted by Emergent
[Edited by - Eric_Brown on April 1, 2010 11:38:44 AM]
If all of the points are at the same position, then you only need to worry about the angular peice.
Find the rotation matrix, or quaternion that represents the set of angles. I prefer quaternions. You construct the quaternion like this
Here ψ is the yaw angle, θ is the pitch angle, and φ is the roll angle. Of course, these angles are given within their canonical ranges.
If you have 2 quaternions that represent 2 different rotations, then you can always construct a single quaternion that will take you from one rotation to the other
If you want to find the angle that this delta quaternion represents, you use the scalar part of Δq in the following formula.
Note, that you don't have to perform a full blown quaternion multiply if you want to find the scalar part of Δ q. The scalar part of this product is just the 4D dot product of the two quaternions q1 and q2.
It seems to me that what you are looking for is the set of coordinates that minimizes the angle α.
I don't recommend using matrices to solve this problem, because it is more expensive all around. But if you were using matrices instead, you would find the delta matrix
Then you would use the formula posted by Emergent
[Edited by - Eric_Brown on April 1, 2010 11:38:44 AM]
This topic is closed to new replies.
Advertisement
Popular Topics
Advertisement