Basic Image AlgorithmS Library  2.8.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
ExampleRMatrix.cpp

Example for rotation matrix usage and conversion.

Author
MIP
/* This file is part of the BIAS library (Basic ImageAlgorithmS).
Copyright (C) 2003-2009 (see file CONTACT for details)
Multimediale Systeme der Informationsverarbeitung
Institut fuer Informatik
Christian-Albrechts-Universitaet Kiel
BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.
BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
@example ExampleRMatrix.cpp
@relates RMatrixBase
@brief Example for rotation matrix usage and conversion.
@ingroup g_examples
@author MIP
*/
#include <Base/Geometry/RMatrixBase.hh>
#include <Base/Geometry/Quaternion.hh>
#ifdef WIN32
# ifndef strcasecmp
# define strcasecmp _strcmpi
# endif
#endif
#include <string.h>
#include <stdio.h>
#include <iostream>
#define ROT_PARAM_Invalid -1
#define ROT_PARAM_Rodrigues 0
#define ROT_PARAM_Euler 1
#define ROT_PARAM_AxisAngle 2
#define ROT_PARAM_Quaternion 3
#define ROT_ORDER_XYZ 0
#define ROT_ORDER_XZY 1
#define ROT_ORDER_YXZ 2
#define ROT_ORDER_YZX 3
#define ROT_ORDER_ZXY 4
#define ROT_ORDER_ZYX 5
using namespace BIAS;
using namespace std;
int main(int argc, char *argv[])
{
int res = 0;
bool showUsage = (argc < 5);
int paramType = ROT_PARAM_Invalid, angleOrder = ROT_ORDER_XYZ;
if (argc > 1) {
if (strcasecmp(argv[1], "Rodrigues") == 0)
paramType = ROT_PARAM_Rodrigues;
else if (strcasecmp(argv[1], "Euler") == 0)
paramType = ROT_PARAM_Euler;
else if (strcasecmp(argv[1], "EulerXYZ") == 0) {
paramType = ROT_PARAM_Euler; angleOrder = ROT_ORDER_XYZ;
} else if (strcasecmp(argv[1], "EulerXZY") == 0) {
paramType = ROT_PARAM_Euler; angleOrder = ROT_ORDER_XZY;
} else if (strcasecmp(argv[1], "EulerYXZ") == 0) {
paramType = ROT_PARAM_Euler; angleOrder = ROT_ORDER_YXZ;
} else if (strcasecmp(argv[1], "EulerYZX") == 0) {
paramType = ROT_PARAM_Euler; angleOrder = ROT_ORDER_YZX;
} else if (strcasecmp(argv[1], "EulerZXY") == 0) {
paramType = ROT_PARAM_Euler; angleOrder = ROT_ORDER_ZXY;
} else if (strcasecmp(argv[1], "EulerZYX") == 0) {
paramType = ROT_PARAM_Euler; angleOrder = ROT_ORDER_ZYX;
}
else if (strcasecmp(argv[1], "AxisAngle") == 0)
paramType = ROT_PARAM_AxisAngle;
else if (strcasecmp(argv[1], "Quaternion") == 0)
paramType = ROT_PARAM_Quaternion;
}
if (paramType == ROT_PARAM_Invalid)
showUsage = true;
else if ((paramType == ROT_PARAM_AxisAngle ||
paramType == ROT_PARAM_Quaternion) && argc < 6)
showUsage = true;
if (showUsage) {
cout << "USAGE : ExampleRMatrix Rodrigues <wx> <wy> <wz> [<degree?>]" << endl
<< " or ExampleRMatrix Euler[<order>] <ax> <ay> <az> [<degree?>]" << endl
<< " or ExampleRMatrix AxisAngle <rx> <ry> <rz> <a> [<degree?>]" << endl
<< " or ExampleRMatrix Quaternion <qx> <qy> <qz> <qw>" << endl << endl
<< "Note that rotation angles and the length of the Rodrigues vector" << endl
<< "are given in radians by default. Euler must be followed by order" << endl
<< "of rotation angles (e.g. EulerYZX), otherwise order XYZ is used." << endl
<< "Use parameter degree = 1 to provide angles in degree instead." << endl << endl;
return 0;
}
bool useDegree = false;
if (argc > ((paramType > 1) ? 6 : 5))
useDegree = (atoi(argv[(paramType > 1) ? 6 : 5]) != 0);
double factor = useDegree ? (M_PI / 180.0) : 1.0;
RMatrixBase R;
switch (paramType)
{
case ROT_PARAM_Rodrigues:
{
BIAS::Vector3<double> w(atof(argv[2]), atof(argv[3]), atof(argv[4]));
R.SetFromAxisAngle(w * factor);
break;
}
case ROT_PARAM_Euler:
{
double ax = atof(argv[2]) * factor;
double ay = atof(argv[3]) * factor;
double az = atof(argv[4]) * factor;
switch (angleOrder)
{
case ROT_ORDER_XYZ: R.SetXYZ(ax, ay, az); break;
case ROT_ORDER_XZY: R.SetXZY(ax, ay, az); break;
case ROT_ORDER_YXZ: R.SetYXZ(ax, ay, az); break;
case ROT_ORDER_YZX: R.SetYZX(ax, ay, az); break;
case ROT_ORDER_ZXY: R.SetZXY(ax, ay, az); break;
case ROT_ORDER_ZYX: R.SetZYX(ax, ay, az); break;
default:
cerr << "ERROR : Invalid Euler angle order!" << endl;
res = -1;
}
break;
}
case ROT_PARAM_AxisAngle:
{
BIAS::Vector3<double> r(atof(argv[2]), atof(argv[3]), atof(argv[4]));
r.Normalize();
double a = atof(argv[5]) * factor;
R.SetFromAxisAngle(r * a);
break;
}
case ROT_PARAM_Quaternion:
{
BIAS::Quaternion<double> q(atof(argv[2]), atof(argv[3]), atof(argv[4]), atof(argv[5]));
q.Normalize();
R.SetFromQuaternion(q);
break;
}
default:
cerr << "ERROR : Invalid rotation parameter type!" << endl;
res = -2;
}
if (res < 0) return res;
cout << "Rotation matrix is " << R << endl;
double ax, ay, az;
if (R.GetRotationAnglesXYZ(ax, ay, az) == 0) {
cout << "Rotation angles in order XYZ are ax = " << ax * 180 / M_PI << ", ay = "
<< ay * 180 / M_PI << ", az = " << az * 180 / M_PI << " degree" << endl;
} else {
cerr << "ERROR : Failed to get rotation angles in order XYZ!" << endl;
res = -3;
}
if (R.GetRotationAnglesZYX(ax, ay, az) == 0) {
cout << "Rotation angles in order ZYX are ax = " << ax * 180 / M_PI << ", ay = "
<< ay * 180 / M_PI << ", az = " << az * 180 / M_PI << " degree" << endl;
} else {
cerr << "ERROR : Failed to get rotation angles in order ZYX!" << endl;
res = -4;
}
if (R.GetRotationAnglesYXZ(ax, ay, az) == 0) {
cout << "Rotation angles in order YXZ are ax = " << ax * 180 / M_PI << ", ay = "
<< ay * 180 / M_PI << ", az = " << az * 180 / M_PI << " degree" << endl;
} else {
cerr << "ERROR : Failed to get rotation angles in order YXZ!" << endl;
res = -5;
}
if (R.GetRotationAnglesZXY(ax, ay, az) == 0) {
cout << "Rotation angles in order ZXY are ax = " << ax * 180 / M_PI << ", ay = "
<< ay * 180 / M_PI << ", az = " << az * 180 / M_PI << " degree" << endl;
} else {
cerr << "ERROR : Failed to get rotation angles in order ZXY!" << endl;
res = -6;
}
if (R.GetRotationAnglesYZX(ax, ay, az) == 0) {
cout << "Rotation angles in order YZX are ax = " << ax * 180 / M_PI << ", ay = "
<< ay * 180 / M_PI << ", az = " << az * 180 / M_PI << " degree" << endl;
} else {
cerr << "ERROR : Failed to get rotation angles in order YZX!" << endl;
res = -7;
}
if (R.GetRotationAnglesXZY(ax, ay, az) == 0) {
cout << "Rotation angles in order XZY are ax = " << ax * 180 / M_PI << ", ay = "
<< ay * 180 / M_PI << ", az = " << az * 180 / M_PI << " degree" << endl;
} else {
cerr << "ERROR : Failed to get rotation angles in order XZY!" << endl;
res = -8;
}
BIAS::Quaternion<double> q;
if (R.GetQuaternion(q) == 0) {
cout << "Unit quaternion is " << q << endl;
} else {
cerr << "ERROR : Failed to get unit quaternion!" << endl;
res = -9;
}
BIAS::Vector3<double> r;
double a;
if (R.GetRotationAxisAngle(r, a) == 0) {
cout << "Rotation axis is " << r << endl
<< "Rotation angle is " << a * 180.0 / M_PI << " degree" << endl;
} else {
cerr << "ERROR : Failed to get rotation axis/angle!" << endl;
res = -10;
}
BIAS::Vector3<double> w;
if (R.GetRotationAxisAngle(w) == 0) {
cout << "Rodrigues vector is " << w << endl;
} else {
cerr << "ERROR : Failed to get Rodrigues vector!" << endl;
res = -11;
}
return res;
}
Generated on Tue Feb 6 2018 02:01:30 for Basic Image AlgorithmS Library by   doxygen 1.8.5