core/vnl/vnl_quaternion.txx
Go to the documentation of this file.
00001 // This is core/vnl/vnl_quaternion.txx
00002 #ifndef vnl_quaternion_txx_
00003 #define vnl_quaternion_txx_
00004 //:
00005 // \file
00006 //
00007 // Copyright (C) 1992 General Electric Company.
00008 //
00009 // Permission is granted to any individual or institution to use, copy, modify,
00010 // and distribute this software, provided that this complete copyright and
00011 // permission notice is maintained, intact, in all copies and supporting
00012 // documentation.
00013 //
00014 // General Electric Company,
00015 // provides this software "as is" without express or implied warranty.
00016 //
00017 // Created: VDN 06/23/92  design and implementation
00018 //
00019 // Quaternion IS-A vector, and is a special case of general n-dimensional space.
00020 // The IS-A relationship is enforced with public inheritance.
00021 // All member functions on vectors are applicable to quaternions.
00022 //
00023 // Rep Invariant:
00024 //   -  norm = 1, for a rotation.
00025 //   -  position vector represented by imaginary quaternion.
00026 // References:
00027 // -  Horn, B.K.P. (1987) Closed-form solution of absolute orientation using
00028 //       unit quaternions. J. Opt. Soc. Am. Vol 4, No 4, April.
00029 // -  Horn, B.K.P. (1987) Robot Vision. MIT Press. pp. 437-551.
00030 //
00031 
00032 
00033 #include "vnl_quaternion.h"
00034 
00035 #include <vcl_cmath.h>
00036 #include <vcl_limits.h>
00037 #include <vcl_iostream.h>
00038 
00039 #include <vnl/vnl_cross.h>
00040 #include <vnl/vnl_math.h>
00041 
00042 //: Creates a quaternion from its ordered components.
00043 // x, y, z denote the imaginary part, which are the  coordinates
00044 // of the rotation axis multiplied by the sine of half the
00045 // angle of rotation. r denotes  the  real  part,  or  the
00046 // cosine  of  half  the  angle of rotation. Default is to
00047 // create a null quaternion, corresponding to a null rotation
00048 // or  an  identity  transform,  which has undefined
00049 // rotation axis.
00050 
00051 template <class T>
00052 vnl_quaternion<T>::vnl_quaternion (T tx, T ty, T tz, T rea)
00053 {
00054   this->operator[](0) = tx;  // 3 first elements are
00055   this->operator[](1) = ty;  // imaginary parts
00056   this->operator[](2) = tz;
00057   this->operator[](3) = rea;  // last element is real part
00058 }
00059 
00060 //: Creates a quaternion from the normalized axis direction and the angle of rotation in radians.
00061 
00062 template <class T>
00063 vnl_quaternion<T>::vnl_quaternion(vnl_vector_fixed<T,3> const& Axis, double Angle)
00064 {
00065   double a = Angle * 0.5;  // half angle
00066   T s = T(vcl_sin(a));
00067   for (int i = 0; i < 3; i++)            // imaginary vector is sine of
00068     this->operator[](i) = T(s * Axis(i));// half angle multiplied with axis
00069   this->operator[](3) = T(vcl_cos(a));   // real part is cosine of half angle
00070 }
00071 
00072 //: Creates a quaternion from a vector.
00073 // 3D vector is converted into an imaginary quaternion with same
00074 // (x, y, z) components.
00075 
00076 template <class T>
00077 vnl_quaternion<T>::vnl_quaternion(vnl_vector_fixed<T,3> const& vec)
00078 {
00079   for (unsigned int i = 0; i < 3; ++i)
00080     this->operator[](i) = vec(i);
00081   this->operator[](3) = T(0);
00082 }
00083 
00084 //: Creates a quaternion from a vector.
00085 // 4D vector is assumed to be a 4-element quaternion, to
00086 // provide casting between vector and quaternion
00087 
00088 template <class T>
00089 vnl_quaternion<T>::vnl_quaternion(vnl_vector_fixed<T,4> const& vec)
00090 {
00091   for (unsigned int i = 0; i < 4; ++i) // 1-1 layout between vector & quaternion
00092     this->operator[](i) = vec[i];
00093 }
00094 
00095 
00096 //: Creates a quaternion from a rotation matrix.
00097 // Its orthonormal basis vectors are the matrix rows.
00098 // \note this matrix \e must have determinant +1; this is not verified!
00099 // \warning Takes the transpose of the rotation matrix, i.e.,
00100 // the orthonormal vectors must be the rows of the matrix, not the columns.
00101 template <class T>
00102 vnl_quaternion<T>::vnl_quaternion(vnl_matrix_fixed<T,3,3> const& rot)
00103 {
00104   double d0 = rot(0,0), d1 = rot(1,1), d2 = rot(2,2);
00105   double xx = 1.0 + d0 - d1 - d2;      // from the diagonal of the rotation
00106   double yy = 1.0 - d0 + d1 - d2;      // matrix, find the terms in
00107   double zz = 1.0 - d0 - d1 + d2;      // each Quaternion component
00108   double rr = 1.0 + d0 + d1 + d2;      // (using the fact that rr+xx+yy+zz=4)
00109 
00110   double max = rr;                     // find the maximum of all terms;
00111   if (xx > max) max = xx;              // dividing by the maximum makes
00112   if (yy > max) max = yy;              // the computations more stable
00113   if (zz > max) max = zz;              // and avoid division by zero
00114 
00115   if (rr == max) {
00116     T r4 = T(vcl_sqrt(rr)*2);
00117     this->r() = r4 / 4;
00118     r4 = T(1) / r4;
00119     this->x() = (rot(1,2) - rot(2,1)) * r4;     // find other components from
00120     this->y() = (rot(2,0) - rot(0,2)) * r4;     // off diagonal terms of
00121     this->z() = (rot(0,1) - rot(1,0)) * r4;     // rotation matrix.
00122   }
00123   else if (xx == max) {
00124     T x4 = T(vcl_sqrt(xx)*2);
00125     this->x() = x4 / 4;
00126     x4 = T(1) / x4;
00127     this->y() = (rot(0,1) + rot(1,0)) * x4;
00128     this->z() = (rot(0,2) + rot(2,0)) * x4;
00129     this->r() = (rot(1,2) - rot(2,1)) * x4;
00130   }
00131   else if (yy == max) {
00132     T y4 = T(vcl_sqrt(yy)*2);
00133     this->y() =  y4 / 4;
00134     y4 = T(1) / y4;
00135     this->x() = (rot(0,1) + rot(1,0)) * y4;
00136     this->z() = (rot(1,2) + rot(2,1)) * y4;
00137     this->r() = (rot(2,0) - rot(0,2)) * y4;
00138   }
00139   else {
00140     T z4 = T(vcl_sqrt(zz)*2);
00141     this->z() =  z4 / 4;
00142     z4 = T(1) / z4;
00143     this->x() = (rot(0,2) + rot(2,0)) * z4;
00144     this->y() = (rot(1,2) + rot(2,1)) * z4;
00145     this->r() = (rot(0,1) - rot(1,0)) * z4;
00146   }
00147 }
00148 
00149 
00150 //: Construct quaternion from Euler Angles
00151 // That is a rotation about the X axis, followed by Y, followed by
00152 // the Z axis, using a fixed reference frame.
00153 template <class T>
00154 vnl_quaternion<T>::vnl_quaternion(T theta_X, T theta_Y, T theta_Z)
00155 {
00156   vnl_quaternion<T> Rx(T(vcl_sin(double(theta_X)*0.5)), 0, 0, T(vcl_cos(double(theta_X)*0.5)));
00157   vnl_quaternion<T> Ry(0, T(vcl_sin(double(theta_Y)*0.5)), 0, T(vcl_cos(double(theta_Y)*0.5)));
00158   vnl_quaternion<T> Rz(0, 0, T(vcl_sin(double(theta_Z)*0.5)), T(vcl_cos(double(theta_Z)*0.5)));
00159   *this = Rz * Ry * Rx;
00160 }
00161 
00162 //: Rotation representation in Euler angles.
00163 // The angles returned will be [theta_X,theta_Y,theta_Z]
00164 // where the final rotation is found be first applying theta_X radians
00165 // about the X axis, then theta_Y about the Y-axis, etc.
00166 // The axes stay in a fixed reference frame.
00167 template <class T>
00168 vnl_vector_fixed<T,3> vnl_quaternion<T>::rotation_euler_angles() const
00169 {
00170   vnl_vector_fixed<T,3> angles;
00171 
00172   vnl_matrix_fixed<T,4,4> rotM = rotation_matrix_transpose_4();
00173   T xy = T(vcl_sqrt(double(vnl_math_sqr(rotM(0,0)) + vnl_math_sqr(rotM(0,1)))));
00174   if (xy > vcl_numeric_limits<T>::epsilon() * T(8))
00175   {
00176     angles(0) = T(vcl_atan2(double(rotM(1,2)), double(rotM(2,2))));
00177     angles(1) = T(vcl_atan2(double(-rotM(0,2)), double(xy)));
00178     angles(2) = T(vcl_atan2(double(rotM(0,1)), double(rotM(0,0))));
00179   }
00180   else
00181   {
00182     angles(0) = T(vcl_atan2(double(-rotM(2,1)), double(rotM(1,1))));
00183     angles(1) = T(vcl_atan2(double(-rotM(0,2)), double(xy)));
00184     angles(2) = T(0);
00185   }
00186   return angles;
00187 }
00188 
00189 
00190 //: Queries the rotation angle of the quaternion.
00191 //  Returned angle lies in [0, 2*pi]
00192 template <class T>
00193 double vnl_quaternion<T>::angle() const
00194 {
00195   return 2 * vcl_atan2(double(this->imaginary().magnitude()),
00196                        double(this->real()));    // angle is always positive
00197 }
00198 
00199 //: Queries the direction of the rotation axis of the quaternion.
00200 //  A null quaternion will return zero for angle and k direction for axis.
00201 template <class T>
00202 vnl_vector_fixed<T,3> vnl_quaternion<T>::axis() const
00203 {
00204   vnl_vector_fixed<T,3> direc = this->imaginary(); // direc parallel to imag. part
00205   T mag = direc.magnitude();
00206   if (mag == T(0)) {
00207     vcl_cout << "Axis not well defined for zero Quaternion. Using (0,0,1) instead.\n";
00208     direc[2] = T(1);                    // or signal exception here.
00209   }
00210   else
00211     direc /= mag;                       // normalize direction vector
00212   return direc;
00213 }
00214 
00215 
00216 //: Converts a normalized quaternion into a square rotation matrix with dimension dim.
00217 //  This is the reverse counterpart of constructing a quaternion from a transformation matrix.
00218 // WARNING this is inconsistent with the quaternion docs and q.rotate()
00219 
00220 template <class T>
00221 vnl_matrix_fixed<T,3,3> vnl_quaternion<T>::rotation_matrix_transpose() const
00222 {
00223   T x2 = x() * x(),  xy = x() * y(),  rx = r() * x(),
00224     y2 = y() * y(),  yz = y() * z(),  ry = r() * y(),
00225     z2 = z() * z(),  zx = z() * x(),  rz = r() * z(),
00226     r2 = r() * r();
00227   vnl_matrix_fixed<T,3,3> rot;
00228   rot(0,0) = r2 + x2 - y2 - z2;         // fill diagonal terms
00229   rot(1,1) = r2 - x2 + y2 - z2;
00230   rot(2,2) = r2 - x2 - y2 + z2;
00231   rot(0,1) = 2 * (xy + rz);             // fill off diagonal terms
00232   rot(0,2) = 2 * (zx - ry);
00233   rot(1,2) = 2 * (yz + rx);
00234   rot(1,0) = 2 * (xy - rz);
00235   rot(2,0) = 2 * (zx + ry);
00236   rot(2,1) = 2 * (yz - rx);
00237 
00238   return rot;
00239 }
00240 
00241 
00242 template <class T>
00243 vnl_matrix_fixed<T,4,4> vnl_quaternion<T>::rotation_matrix_transpose_4() const
00244 {
00245   vnl_matrix_fixed<T,4,4> rot;
00246   return rot.set_identity().update(this->rotation_matrix_transpose().as_ref());
00247 }
00248 
00249 //: Returns the conjugate of given quaternion, having same real and opposite imaginary parts.
00250 
00251 template <class T>
00252 vnl_quaternion<T> vnl_quaternion<T>::conjugate() const
00253 {
00254   return vnl_quaternion<T> (-x(), -y(), -z(), r());
00255 }
00256 
00257 //: Returns the inverse of given quaternion.
00258 //  For unit quaternion representing rotation, the inverse is the
00259 // same as the conjugate.
00260 
00261 template <class T>
00262 vnl_quaternion<T> vnl_quaternion<T>::inverse() const
00263 {
00264   vnl_quaternion<T> inv = this->conjugate();
00265   inv /= vnl_c_vector<T>::dot_product(this->data_, this->data_, 4);
00266   return inv;
00267 }
00268 
00269 //: Returns  the product of two quaternions.
00270 // Multiplication of two quaternions is not symmetric and has
00271 // fewer  operations  than  multiplication  of orthonormal
00272 // matrices. If object is rotated by r1, then by r2,  then
00273 // the  composed  rotation (r2 o r1) is represented by the
00274 // quaternion (q2 * q1), or by the matrix (m1 * m2).  Note
00275 // that  matrix  composition  is reversed because matrices
00276 // and vectors are represented row-wise.
00277 
00278 template <class T>
00279 vnl_quaternion<T> vnl_quaternion<T>::operator* (vnl_quaternion<T> const& rhs) const
00280 {
00281   T r1 = this->real();                  // real and img parts of args
00282   T r2 = rhs.real();
00283   vnl_vector_fixed<T,3> i1 = this->imaginary();
00284   vnl_vector_fixed<T,3> i2 = rhs.imaginary();
00285   T real_v = (r1 * r2) - ::dot_product(i1, i2); // real&img of product q1*q2
00286   vnl_vector_fixed<T,3> img = vnl_cross_3d(i1, i2);
00287   img += (i2 * r1) + (i1 * r2);
00288   return vnl_quaternion<T>(img[0], img[1], img[2], real_v);
00289 }
00290 
00291 //: Rotates 3D vector v with source quaternion and stores the rotated vector back into v.
00292 // For speed and greater accuracy, first convert quaternion into an orthonormal
00293 // matrix,  then  use matrix multiplication to rotate many vectors.
00294 
00295 template <class T>
00296 vnl_vector_fixed<T,3> vnl_quaternion<T>::rotate(vnl_vector_fixed<T,3> const& v) const
00297 {
00298   T rea = this->real();
00299   vnl_vector_fixed<T,3> i = this->imaginary();
00300   vnl_vector_fixed<T,3> i_x_v(vnl_cross_3d(i, v));
00301   return v + i_x_v * T(2*rea) - vnl_cross_3d(i_x_v, i) * T(2);
00302 }
00303 
00304 #undef VNL_QUATERNION_INSTANTIATE
00305 #define VNL_QUATERNION_INSTANTIATE(T) \
00306 template class vnl_quaternion<T >;\
00307 VCL_INSTANTIATE_INLINE(vcl_ostream& operator<< (vcl_ostream&, vnl_quaternion<T > const&))
00308 
00309 #endif // vnl_quaternion_txx_