ExampleCovTransformPose.cpp

Example for pose covariance transformation

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 ExampleCovTransformPose.cpp
   @relates CovTransformPose
   @brief Example for pose covariance transformation 
   @ingroup g_examples
   @author MIP
*/

#include <Base/Common/W32Compat.hh>
#include <Geometry/CovTransformPose.hh>

#include <Base/Math/Vector3.hh>
#include <Base/Math/Matrix.hh>
#include <Geometry/RMatrix.hh>
#include <Base/Geometry/Quaternion.hh>
#include <MathAlgo/SVD.hh>

using namespace BIAS;
using namespace std;

int main() 
{
  //create test-data...
  Vector3<double> angles(0.2, 1.2, 1.01);
  Vector3<double> posAngles(1.0, 3.0, 0.0);
  Matrix<double> covAngles(6,6);
  //...and covariance
  covAngles.SetIdentity();
  covAngles *= 0.3;
  covAngles[3][3] *= 0.1;
  covAngles[4][4] *= 0.1;
  covAngles[5][5] *= 0.1;

  cout << "Pos. for angles: " << posAngles << endl;
  cout << "Angles: " << angles << endl;
  cout << "Cov for pos./angles: " << covAngles << endl << endl;
  cout << "==============================================================="
       << endl;

  //get transformed values for Euler-angles to quaternions
  Quaternion<double> quat;
  Vector3<double> posQuat;
  Matrix<double> covQuat;

  CovTransformPose trans;
  if (trans.PoseEulerZYXToQuat(posAngles, angles, covAngles,
                               posQuat, quat, covQuat) !=0)
    exit(-1);

  cout << "Pos. for quat: " << posQuat << endl;
  cout << "Quaternion: " << quat << endl;
  cout << "Cov for pos./quat: " << covQuat << endl << endl;
  cout << "==============================================================="
       << endl;

  //get back-transformed values for quaternions to Euler-angles
  if (trans.PoseQuatToEulerZYX(posQuat, quat, covQuat,
                               posAngles, angles, covAngles) !=0)
    exit(-1);

  cout << "Re-Pos. for angles: " << posAngles << endl;
  cout << "Re-Angles: " << angles << endl;
  cout << "Re-Cov for pos./angles: " << covAngles << endl;

  return 0;
}