#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()
{
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;
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;
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;
}