mirror of https://github.com/emgucv/emgucv.git
Browse Source
Added Quaternions class.
Added Quaternions class.
git-svn-id: https://emgucv.svn.sourceforge.net/svnroot/emgucv/trunk@693 d7f09016-e345-0410-b530-edf29a71df78UWP10
canming
16 years ago
4 changed files with 325 additions and 0 deletions
-
113Emgu.CV.Extern/quaternions.cpp
-
27Emgu.CV.Extern/quaternions.h
-
184Emgu.CV/CameraCalibration/Quaternions.cs
-
1Emgu.CV/Emgu.CV.csproj
@ -0,0 +1,113 @@ |
|||
#include "quaternions.h"
|
|||
|
|||
CVAPI(void) eulerToQuaternions(double x, double y, double z, Quaternions* quaternions) |
|||
{ |
|||
double |
|||
sinX = sin(x / 2.0), |
|||
cosX = cos(x / 2.0), |
|||
sinY = sin(y / 2.0), |
|||
cosY = cos(y / 2.0), |
|||
sinZ = sin(z / 2.0), |
|||
cosZ = cos(z / 2.0); |
|||
|
|||
quaternions->w = cosX*cosY*cosZ + sinX*sinY*sinZ; |
|||
quaternions->x = sinX*cosY*cosZ - cosX*sinY*sinZ; |
|||
quaternions->y = cosX*sinY*cosZ + sinX*cosY*sinZ; |
|||
quaternions->z = cosX*cosY*sinZ - sinX*sinY*cosZ; |
|||
} |
|||
|
|||
CVAPI(void) quaternionsToEuler(Quaternions* quaternions, double* x, double* y, double* z) |
|||
{ |
|||
double q0 = quaternions->w; |
|||
double q1 = quaternions->x; |
|||
double q2 = quaternions->y; |
|||
double q3 = quaternions->z; |
|||
|
|||
*x = atan2(2 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2*q2)); |
|||
*y = asin(2 * (q0 * q2 - q3 * q1)); |
|||
*z = atan2(2 * (q0 * q3 + q1 * q2), 1.0 - 2.0 * (q2*q2 + q3*q3)); |
|||
} |
|||
|
|||
CVAPI(void) quaternionsToRotationMatrix(Quaternions* quaternions, CvMat* rotation) |
|||
{ |
|||
double a = quaternions->w; |
|||
double b = quaternions->x; |
|||
double c = quaternions->y; |
|||
double d = quaternions->z; |
|||
|
|||
cv::Mat r = cv::cvarrToMat(rotation); |
|||
CV_Assert(r.rows == 3 && r.cols == 3); |
|||
cv::MatIterator_<double> rIter = r.begin<double>(); |
|||
*rIter++ = a*a+b*b-c*c-d*d; *rIter++ = 2.0*b*c-2.0*a*d; *rIter++ = 2.0*b*d+2.0*a*c; |
|||
*rIter++ = 2.0*b*c+2.0*a*d; *rIter++ = a*a-b*b+c*c-d*d; *rIter++ = 2.0*c*d-2.0*a*b; |
|||
*rIter++ = 2.0*b*d-2.0*a*c; *rIter++ = 2.0*c*d+2.0*a*b; *rIter++ = a*a-b*b-c*c+d*d; |
|||
} |
|||
|
|||
void quaternionsRotate(double w, double x, double y, double z, double v1, double v2, double v3, double*vr) |
|||
{ |
|||
double |
|||
t2 = w*x, |
|||
t3 = w*y, |
|||
t4 = w*z, |
|||
t5 = -x*x, |
|||
t6 = x*y, |
|||
t7 = x*z, |
|||
t8 = -y*y, |
|||
t9 = y*z, |
|||
t10 = -z*z; |
|||
|
|||
*vr++ = 2*( (t8 + t10)* v1 + (t6 - t4)* v2 + (t3 + t7)* v3 ) + v1, |
|||
*vr++ = 2*( (t4 + t6)* v1 + (t5 + t10)* v2 + (t9 - t2)* v3 ) + v2, |
|||
*vr = 2*( (t7 - t3)* v1 + (t2 + t9)* v2 + (t5 + t8)* v3 ) + v3; |
|||
} |
|||
|
|||
CVAPI(void) quaternionsRotatePoint(Quaternions* quaternions, CvPoint3D64f* point, CvPoint3D64f* pointDst) |
|||
{ |
|||
quaternionsRotate(quaternions->w, quaternions->x, quaternions->y, quaternions->z, point->x, point->y, point->z, (double*) pointDst); |
|||
} |
|||
|
|||
CVAPI(void) quaternionsRotatePoints(Quaternions* quaternions, CvMat* pointSrc, CvMat* pointDst) |
|||
{ |
|||
cv::Mat p = cv::cvarrToMat(pointSrc); |
|||
cv::Mat pDst = cv::cvarrToMat(pointDst); |
|||
CV_Assert((p.rows == 3 && p.cols == 1) || p.cols ==3); |
|||
CV_Assert(pDst.rows == p.rows && pDst.cols == p.cols); |
|||
|
|||
double *v; |
|||
|
|||
cv::MatIterator_<double> pIter = p.begin<double>(); |
|||
cv::MatIterator_<double> pDstIter = pDst.begin<double>(); |
|||
|
|||
if ((p.rows == 3 && p.cols == 1)) |
|||
{ |
|||
v = pIter.ptr; |
|||
quaternionsRotate(quaternions->w, quaternions->x, quaternions->y, quaternions->z, *v, *(v+1), *(v+2), pDstIter.ptr); |
|||
} else |
|||
{ |
|||
for(int i = 0; i < p.rows; i++, pIter+=3, pDstIter+=3) |
|||
{ |
|||
v = pIter.ptr; |
|||
quaternionsRotate(quaternions->w, quaternions->x, quaternions->y, quaternions->z, *v, *(v+1), *(v+2), pDstIter.ptr); |
|||
} |
|||
} |
|||
} |
|||
|
|||
CVAPI(void) quaternionsMultiply(Quaternions* quaternions1, Quaternions* quaternions2, Quaternions* quaternionsDst) |
|||
{ |
|||
double w1 = quaternions1->w; |
|||
double x1 = quaternions1->x; |
|||
double y1 = quaternions1->y; |
|||
double z1 = quaternions1->z; |
|||
|
|||
double w2 = quaternions2->w; |
|||
double x2 = quaternions2->x; |
|||
double y2 = quaternions2->y; |
|||
double z2 = quaternions2->z; |
|||
|
|||
quaternionsDst->w = (w1*w2 - x1*x2 - y1*y2 - z1*z2); |
|||
quaternionsDst->x = (w1*x2 + x1*w2 + y1*z2 - z1*y2); |
|||
quaternionsDst->y = (w1*y2 - x1*z2 + y1*w2 + z1*x2); |
|||
quaternionsDst->z = (w1*z2 + x1*y2 - y1*x2 + z1*w2); |
|||
} |
|||
|
|||
|
|||
@ -0,0 +1,27 @@ |
|||
#include "cxcore.h" |
|||
|
|||
typedef struct Quaternions |
|||
{ |
|||
double w; |
|||
double x; |
|||
double y; |
|||
double z; |
|||
} Quaternions; |
|||
|
|||
/* convert eluer angle (in radian) to quaternions */ |
|||
CVAPI(void) eulerToQuaternions(double x, double y, double z, Quaternions* quaternions); |
|||
|
|||
/* convert quaternions to eluer angle (in radian) */ |
|||
CVAPI(void) quaternionsToEuler(Quaternions* quaternions, double* x, double* y, double* z); |
|||
|
|||
/* convert quaternions to (3x3) rotation matrix */ |
|||
CVAPI(void) quaternionsToRotationMatrix(Quaternions* quaternions, CvMat* rotation); |
|||
|
|||
/* rotate the (3x1 or nx3) matrix of points using the quaternions */ |
|||
CVAPI(void) quaternionsRotatePoint(Quaternions* quaternions, CvPoint3D64f* point, CvPoint3D64f* pointDst); |
|||
|
|||
/* rotate the (3x1 or nx3) matrix of points using the quaternions */ |
|||
CVAPI(void) quaternionsRotatePoints(Quaternions* quaternions, CvMat* pointSrc, CvMat* pointDst); |
|||
|
|||
/* quaternionsDst = quaternions1 * quaternions2 */ |
|||
CVAPI(void) quaternionsMultiply(Quaternions* quaternions1, Quaternions* quaternions2, Quaternions* quaternionsDst); |
|||
@ -0,0 +1,184 @@ |
|||
using System; |
|||
using System.Collections.Generic; |
|||
using System.Text; |
|||
using System.Runtime.InteropServices; |
|||
using System.Runtime.Serialization; |
|||
using Emgu.CV.Structure; |
|||
|
|||
namespace Emgu.CV |
|||
{ |
|||
/// <summary>
|
|||
/// A unit quaternions that defines rotation in 3D
|
|||
/// </summary>
|
|||
[Serializable] |
|||
//TODO: Check if mono has System.Windows.Media.Media3D.Quaternion implemented and if it comes as a package on Ubuntu and fedora
|
|||
public struct Quaternions |
|||
{ |
|||
private double _w; |
|||
private double _x; |
|||
private double _y; |
|||
private double _z; |
|||
|
|||
/// <summary>
|
|||
/// The W component of the quaternion.
|
|||
/// </summary>
|
|||
public double W |
|||
{ |
|||
get { return _w; } |
|||
set { _w = value; } |
|||
} |
|||
|
|||
/// <summary>
|
|||
/// The X component of the quaternion.
|
|||
/// </summary>
|
|||
public double X |
|||
{ |
|||
get { return _x; } |
|||
set { _x = value; } |
|||
} |
|||
|
|||
/// <summary>
|
|||
/// The Y component of the quaternion.
|
|||
/// </summary>
|
|||
public double Y |
|||
{ |
|||
get { return _y; } |
|||
set { _y = value; } |
|||
} |
|||
|
|||
/// <summary>
|
|||
/// The Z component of the quaternion.
|
|||
/// </summary>
|
|||
public double Z |
|||
{ |
|||
get { return _z; } |
|||
set { _z = value; } |
|||
} |
|||
|
|||
[DllImport(CvInvoke.EXTERN_LIBRARY)] |
|||
private extern static void eulerToQuaternions(double x, double y, double z, ref Quaternions q); |
|||
|
|||
[DllImport(CvInvoke.EXTERN_LIBRARY)] |
|||
private extern static void quaternionsToEuler(ref Quaternions q, ref double x, ref double y, ref double z); |
|||
|
|||
[DllImport(CvInvoke.EXTERN_LIBRARY)] |
|||
private extern static void quaternionsToRotationMatrix(ref Quaternions quaternions, IntPtr rotation); |
|||
|
|||
[DllImport(CvInvoke.EXTERN_LIBRARY)] |
|||
private extern static void quaternionsRotatePoint(ref Quaternions quaternions, ref MCvPoint3D64f point, ref MCvPoint3D64f pointDst); |
|||
|
|||
[DllImport(CvInvoke.EXTERN_LIBRARY)] |
|||
private extern static void quaternionsRotatePoints(ref Quaternions quaternions, IntPtr pointSrc, IntPtr pointDst); |
|||
|
|||
[DllImport(CvInvoke.EXTERN_LIBRARY)] |
|||
private extern static void quaternionsMultiply(ref Quaternions quaternions1, ref Quaternions quaternions2, ref Quaternions quaternionsDst); |
|||
|
|||
/// <summary>
|
|||
/// Set the value of the quaternions using euler angle
|
|||
/// </summary>
|
|||
/// <param name="x">Rotation around x-axis (roll) in radian</param>
|
|||
/// <param name="y">Rotation around y-axis (pitch) in radian</param>
|
|||
/// <param name="z">rotation around z-axis (yaw) in radian</param>
|
|||
public void SetEuler(double x, double y, double z) |
|||
{ |
|||
eulerToQuaternions(x, y, z, ref this); |
|||
} |
|||
|
|||
/// <summary>
|
|||
/// Get the equaivalent euler angle
|
|||
/// </summary>
|
|||
/// <param name="x">Rotation around x-axis (roll) in radian</param>
|
|||
/// <param name="y">Rotation around y-axis (pitch) in radian</param>
|
|||
/// <param name="z">rotation around z-axis (yaw) in radian</param>
|
|||
public void GetEuler(ref double x, ref double y, ref double z) |
|||
{ |
|||
quaternionsToEuler(ref this, ref x, ref y, ref z); |
|||
} |
|||
|
|||
/// <summary>
|
|||
/// Fill the (3x3) rotation matrix with the value such that it represent the quaternions
|
|||
/// </summary>
|
|||
/// <param name="rotation">The (3x3) rotation matrix which values will be set to represent this quaternions</param>
|
|||
public void GetRotationMatrix(Matrix<double> rotation) |
|||
{ |
|||
quaternionsToRotationMatrix(ref this, rotation); |
|||
} |
|||
|
|||
/// <summary>
|
|||
/// Rotate the points in <paramref name="pointsSrc"/> and save the result in <paramref name="pointsDst"/>. Inplace operation is supported (<paramref name="pointsSrc"/> == <paramref name="pointsDst"/>).
|
|||
/// </summary>
|
|||
/// <param name="pointsSrc">The points to be rotated</param>
|
|||
/// <param name="pointsDst">The result of the rotation, should be the same size as <paramref name="pointsSrc"/>, can be <paramref name="pointSrc"/> as well for inplace rotation</param>
|
|||
public void RotatePoints(Matrix<double> pointsSrc, Matrix<double> pointsDst) |
|||
{ |
|||
quaternionsRotatePoints(ref this, pointsSrc, pointsDst); |
|||
} |
|||
|
|||
/// <summary>
|
|||
/// Rotate the specific point and return the result
|
|||
/// </summary>
|
|||
/// <param name="point">The point to be rotated</param>
|
|||
/// <returns>The rotated point</returns>
|
|||
public MCvPoint3D64f RotatePoint(MCvPoint3D64f point) |
|||
{ |
|||
MCvPoint3D64f result = new MCvPoint3D64f(); |
|||
quaternionsRotatePoint(ref this, ref point, ref result); |
|||
return result; |
|||
} |
|||
|
|||
/// <summary>
|
|||
/// Get the rotation axis of the quaternion
|
|||
/// </summary>
|
|||
public MCvPoint3D64f RotationAxis |
|||
{ |
|||
get |
|||
{ |
|||
return new MCvPoint3D64f(X, Y, Z); |
|||
} |
|||
set |
|||
{ |
|||
X = value.x; |
|||
Y = value.y; |
|||
Z = value.z; |
|||
} |
|||
} |
|||
|
|||
/// <summary>
|
|||
/// Returns the rotation angle in radian
|
|||
/// </summary>
|
|||
public double RotationAngle |
|||
{ |
|||
get |
|||
{ |
|||
return 2.0* Math.Acos(W); |
|||
} |
|||
set |
|||
{ |
|||
W = Math.Cos(value / 2.0); |
|||
} |
|||
} |
|||
|
|||
/// <summary>
|
|||
/// Multiply the current Quaternions with <paramref name="quaternionsOther"/>
|
|||
/// </summary>
|
|||
/// <param name="quaternionsOther">The other rotation</param>
|
|||
/// <return>A composition of the two rotations</return>
|
|||
public Quaternions Multiply(Quaternions quaternionsOther) |
|||
{ |
|||
Quaternions result = new Quaternions(); |
|||
quaternionsMultiply(ref this, ref quaternionsOther, ref result); |
|||
return result; |
|||
} |
|||
|
|||
/// <summary>
|
|||
/// Computes the multiplication of two quaternions
|
|||
/// </summary>
|
|||
/// <param name="q1">The quaternions to be multiplied</param>
|
|||
/// <param name="q2">The quaternions to be multiplied</param>
|
|||
/// <returns>The multiplication of two quaternions</returns>
|
|||
public static Quaternions operator *(Quaternions q1, Quaternions q2) |
|||
{ |
|||
return q1.Multiply(q2); |
|||
} |
|||
} |
|||
} |
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue