ExampleRMatrix.cpp

/* 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;
}