Rotation.cpp
Go to the documentation of this file.
00001 /*
00002  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
00003  *
00004  * Copyright 2007-2012 VTT Technical Research Centre of Finland
00005  *
00006  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
00007  *          <http://www.vtt.fi/multimedia/alvar.html>
00008  *
00009  * ALVAR is free software; you can redistribute it and/or modify it under the
00010  * terms of the GNU Lesser General Public License as published by the Free
00011  * Software Foundation; either version 2.1 of the License, or (at your option)
00012  * any later version.
00013  *
00014  * This library is distributed in the hope that it will be useful, but WITHOUT
00015  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
00017  * for more details.
00018  *
00019  * You should have received a copy of the GNU Lesser General Public License
00020  * along with ALVAR; if not, see
00021  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
00022  */
00023 
00024 #include "ar_track_alvar/Alvar.h"
00025 #include "ar_track_alvar/Rotation.h"
00026 
00027 using namespace std;
00028 
00029 namespace alvar {
00030 using namespace std;
00031 
00032 Rotation::Rotation()
00033 {
00034         cvInitMatHeader(&quaternion_mat, 4, 1, CV_64F, quaternion);
00035         Reset();
00036 }
00037 
00038 Rotation::Rotation(const Rotation& r) {
00039         cvInitMatHeader(&quaternion_mat, 4, 1, CV_64F, quaternion);
00040         cvCopy(&r.quaternion_mat, &quaternion_mat);
00041 }
00042 
00043 Rotation::Rotation(CvMat *data, RotationType t)
00044 {
00045         cvInitMatHeader(&quaternion_mat, 4, 1, CV_64F, quaternion);
00046 
00047         Reset();
00048 
00049         switch (t)
00050         {
00051                 case QUAT :
00052                         SetQuaternion(data);
00053                         break;
00054                 case MAT :
00055                         SetMatrix(data);
00056                         break;
00057                 case EUL :
00058                         SetEuler(data);
00059                         break;
00060                 case ROD :
00061                         SetRodriques(data);
00062                         break;
00063         }
00064 }
00065 
00066 void Rotation::Transpose()
00067 {
00068         double tmp[9];
00069         CvMat tmp_mat = cvMat(3, 3, CV_64F, tmp);
00070         GetMatrix(&tmp_mat);
00071         cvTranspose(&tmp_mat, &tmp_mat);
00072         SetMatrix(&tmp_mat);
00073 }
00074         
00075 void Rotation::MirrorMat(CvMat *mat, bool x, bool y, bool z) {
00076         CvMat *mat_mul = cvCloneMat(mat);
00077         cvSetIdentity(mat_mul);
00078         if (x) cvmSet(mat_mul, 0, 0, -1);
00079         if (y) cvmSet(mat_mul, 1, 1, -1);
00080         if (z) cvmSet(mat_mul, 2, 2, -1);
00081         cvMatMul(mat_mul, mat, mat);
00082         cvReleaseMat(&mat_mul);
00083 }
00084         
00085 void Rotation::Mirror(bool x, bool y, bool z)
00086 {
00087         double tmp[9];
00088         CvMat tmp_mat = cvMat(3, 3, CV_64F, tmp);
00089         GetMatrix(&tmp_mat);
00090         MirrorMat(&tmp_mat, x, y, z);
00091         SetMatrix(&tmp_mat);
00092 }
00093 
00094 void Rotation::Reset()
00095 {
00096         cvZero(&quaternion_mat); cvmSet(&quaternion_mat, 0, 0, 1);
00097 }
00098 
00099 void Rotation::Mat9ToRod(double *mat, double *rod)
00100 {
00101         CvMat mat_m, rod_m;
00102         cvInitMatHeader(&mat_m, 3, 3, CV_64F, mat);
00103         cvInitMatHeader(&rod_m, 3, 1, CV_64F, rod);
00104         cvRodrigues2(&mat_m, &rod_m);
00105 }
00106 
00107 void Rotation::RodToMat9(double *rod, double *mat)
00108 {
00109         CvMat mat_m, rod_m;
00110         cvInitMatHeader(&mat_m, 3, 3, CV_64F, mat);
00111         cvInitMatHeader(&rod_m, 3, 1, CV_64F, rod);
00112         cvRodrigues2(&rod_m, &mat_m, 0);
00113 }
00114 
00115 void Rotation::QuatInv(const double *q, double *qi)
00116 {
00117         qi[0] =    q[0];
00118         qi[1] = -1*q[1];
00119         qi[2] = -1*q[2];
00120         qi[3] = -1*q[3];
00121 }
00122 
00123 void Rotation::QuatNorm(double *q)
00124 {
00125         double l = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3] );
00126         
00127         if(l != 0)
00128                 for(unsigned i = 0; i < 4; ++i)
00129                         q[i] = q[i] / l;
00130 }
00131 
00132 void Rotation::QuatMul(const double *q1, const double *q2, double *q3)
00133 {
00134         double w1 = q1[0];
00135         double x1 = q1[1];
00136         double y1 = q1[2];
00137         double z1 = q1[3];
00138 
00139         double w2 = q2[0];
00140         double x2 = q2[1];
00141         double y2 = q2[2];
00142         double z2 = q2[3];
00143 
00144         q3[0] = w1*w2 - x1*x2 - y1*y2 - z1*z2;
00145         q3[1] = w1*x2 + x1*w2 + y1*z2 - z1*y2;
00146         q3[2] = w1*y2 + y1*w2 + z1*x2 - x1*z2;
00147         q3[3] = w1*z2 + z1*w2 + x1*y2 - y1*x2;
00148 
00149         QuatNorm(q3);
00150 }
00151 
00152 void Rotation::QuatToMat9(const double *quat, double *mat)
00153 {
00154         double W = quat[0];
00155         double X = quat[1];
00156         double Y = quat[2];
00157         double Z = quat[3];
00158 
00159         double xx = X * X;
00160         double xy = X * Y;
00161         double xz = X * Z;
00162         double xw = X * W;
00163 
00164         double yy = Y * Y;
00165         double yz = Y * Z;
00166         double yw = Y * W;
00167 
00168         double zz = Z * Z;
00169         double zw = Z * W;
00170 
00171         mat[0] = 1 - 2 * ( yy + zz ); //(0,0)
00172         mat[1] =     2 * ( xy - zw ); //(0,1)
00173         mat[2] =     2 * ( xz + yw ); //(0,2)
00174 
00175         mat[3] =     2 * ( xy + zw ); //(1,0)
00176         mat[4] = 1 - 2 * ( xx + zz ); //(1,1)
00177         mat[5] =     2 * ( yz - xw ); //(1,2)
00178 
00179         mat[6] =     2 * ( xz - yw ); //(2,0)
00180         mat[7] =     2 * ( yz + xw ); //(2,1)
00181         mat[8] = 1 - 2 * ( xx + yy ); //(2,2)
00182 }
00183 
00184 // TODO: Now we don't want to eliminate the translation part from the matrix here. Did this change break something???
00185 void Rotation::QuatToMat16(const double *quat, double *mat)
00186 {
00187         //memset(mat, 0, 16*sizeof(double));
00188 
00189         double W = quat[0];
00190         double X = quat[1];
00191         double Y = quat[2];
00192         double Z = quat[3];
00193 
00194         double xx = X * X;
00195         double xy = X * Y;
00196         double xz = X * Z;
00197         double xw = X * W;
00198 
00199         double yy = Y * Y;
00200         double yz = Y * Z;
00201         double yw = Y * W;
00202 
00203         double zz = Z * Z;
00204         double zw = Z * W;
00205 
00206         mat[0] = 1 - 2 * ( yy + zz ); //(0,0)
00207         mat[1] =     2 * ( xy - zw ); //(0,1)
00208         mat[2] =     2 * ( xz + yw ); //(0,2)
00209 
00210         mat[4] =     2 * ( xy + zw ); //(1,0)
00211         mat[5] = 1 - 2 * ( xx + zz ); //(1,1)
00212         mat[6] =     2 * ( yz - xw ); //(1,2)
00213 
00214         mat[8] =     2 * ( xz - yw ); //(2,0)
00215         mat[9] =     2 * ( yz + xw ); //(2,1)
00216         mat[10] = 1 - 2 * ( xx + yy ); //(2,2)
00217 
00218         //mat[15] = 1;
00219 }
00220 
00221 void Rotation::QuatToEul(const double *q, double *eul)
00222 {
00223         double qw = q[0];
00224         double qx = q[1];
00225         double qy = q[2];
00226         double qz = q[3];
00227 
00228         double heading = 0, bank = 0, attitude = 0;
00229 
00230         if ((2*qx*qy + 2*qz*qw) == 1.0)
00231         {
00232                 heading = 2 * atan2(qx,qw);
00233                 bank = 0;
00234         }
00235         else if ((2*qx*qy + 2*qz*qw) == -1.0)
00236         {
00237                 heading = -2 * atan2(qx,qw);
00238                 bank = 0;
00239         }
00240         else
00241         {
00242                 heading = atan2(2*qy*qw-2*qx*qz , 1 - 2*qy*qy - 2*qz*qz);
00243                 bank    = atan2(2*qx*qw-2*qy*qz , 1 - 2*qx*qx - 2*qz*qz);
00244         }
00245 
00246         attitude = asin(2*qx*qy + 2*qz*qw);
00247 
00248         heading  = 180.0 * heading  / PI;
00249         attitude = 180.0 * attitude / PI;
00250         bank     = 180.0 * bank     / PI;
00251 
00252         eul[0] = heading;
00253         eul[1] = attitude;
00254         eul[2] = bank;
00255 }
00256 
00257 void Rotation::Mat9ToQuat(const double *mat, double *quat)
00258 {
00259         quat[0] = sqrt(max(0., 1 + mat[0] + mat[4] + mat[8])) / 2.0;  // w
00260         quat[1] = sqrt(max(0., 1 + mat[0] - mat[4] - mat[8])) / 2.0;  // x
00261         quat[2] = sqrt(max(0., 1 - mat[0] + mat[4] - mat[8])) / 2.0;  // y
00262         quat[3] = sqrt(max(0., 1 - mat[0] - mat[4] + mat[8])) / 2.0;  // z
00263 
00264         quat[1] = quat[1]*Sign(mat[7] - mat[5]); // x
00265         quat[2] = quat[2]*Sign(mat[2] - mat[6]); // y
00266         quat[3] = quat[3]*Sign(mat[3] - mat[1]); // z
00267 
00268         QuatNorm(quat);
00269 }
00270 
00271 void Rotation::EulToQuat(const double *eul, double *quat)
00272 {
00273         double heading  = PI*eul[0]/180.0;
00274         double attitude = PI*eul[1]/180.0;
00275         double bank     = PI*eul[2]/180.0;
00276 
00277         double c1 = cos(heading/2.0);
00278         double s1 = sin(heading/2.0);
00279         double c2 = cos(attitude/2.0);
00280         double s2 = sin(attitude/2.0);
00281         double c3 = cos(bank/2.0);
00282         double s3 = sin(bank/2.0);
00283         double c1c2 = c1*c2;
00284         double s1s2 = s1*s2;
00285 
00286         quat[0] = c1c2*c3  - s1s2*s3;
00287         quat[1] = c1c2*s3  + s1s2*c3;
00288         quat[2] = s1*c2*c3 + c1*s2*s3;
00289         quat[3] = c1*s2*c3 - s1*c2*s3;
00290 
00291         QuatNorm(quat);
00292 }
00293 
00294 void Rotation::SetQuaternion(CvMat *mat)
00295 {
00296         cvCopy(mat, &quaternion_mat);
00297         QuatNorm(quaternion);
00298 }
00299 
00300 void Rotation::SetQuaternion(const double *quat)
00301 {
00302         quaternion[0] = quat[0];
00303         quaternion[1] = quat[1];
00304         quaternion[2] = quat[2];
00305         quaternion[3] = quat[3];
00306         QuatNorm(quaternion);
00307 }
00308 
00309 void Rotation::SetEuler(const CvMat *mat)
00310 {
00311         EulToQuat(mat->data.db, quaternion);
00312 }
00313 
00314 void Rotation::SetRodriques(const CvMat *mat)
00315 {
00316         double tmp[9];
00317         RodToMat9(mat->data.db, tmp);
00318         Mat9ToQuat(tmp, quaternion);
00319 }
00320 
00321 void Rotation::SetMatrix(const CvMat *mat)
00322 {
00323         double tmp[9];
00324         for(int i = 0; i < 3; ++i)
00325                 for(int j = 0; j < 3; ++j)
00326                         tmp[i*3+j] = cvmGet(mat, i, j);
00327 
00328         Mat9ToQuat(tmp, quaternion);
00329 }
00330 
00331 void Rotation::GetMatrix(CvMat *mat) const
00332 {
00333         if (mat->width == 3) {
00334                 QuatToMat9(quaternion, mat->data.db);
00335         } else if (mat->width == 4) {
00336                 cvSetIdentity(mat);
00337                 QuatToMat16(quaternion, mat->data.db);
00338         }
00339 }
00340 
00341 void Rotation::GetRodriques(CvMat *mat) const
00342 {
00343         double tmp[9];
00344         QuatToMat9(quaternion, tmp);
00345         Mat9ToRod(tmp, mat->data.db);
00346 }
00347 
00348 void Rotation::GetEuler(CvMat *mat) const
00349 {
00350         QuatToEul(quaternion, mat->data.db);
00351 }
00352 
00353 void Rotation::GetQuaternion(CvMat *mat) const
00354 {
00355         cvCopy(&quaternion_mat, mat);
00356 }
00357 
00358 // TODO: This is not needed???
00359 inline Rotation& Rotation::operator = (const Rotation& r)
00360 {
00361         memcpy(quaternion, r.quaternion, 4*sizeof(double));
00362         return *this;
00363 }
00364 
00365 inline Rotation& Rotation::operator += (const Rotation& r)
00366 {
00367         Rotation res;
00368         QuatMul(quaternion, r.quaternion, res.quaternion);
00369         memcpy(quaternion, res.quaternion, 4*sizeof(double));
00370         //x += v.x;
00371         //y += v.y;
00372         //z += v.z;
00373 
00374         return *this;
00375 }
00376 
00377 inline Rotation operator + (const Rotation& r1, const Rotation& r2)
00378 {
00379         Rotation ret = r1;
00380         ret += r2;
00381 
00382         return ret;
00383 }
00384 
00385 } // namespace alvar


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Feb 16 2017 03:23:02