33 #include <Base/Common/W32Compat.hh>
34 #include <Geometry/CovTransformPose.hh>
36 #include <Base/Math/Vector3.hh>
37 #include <Base/Math/Matrix.hh>
38 #include <Geometry/RMatrix.hh>
39 #include <Base/Geometry/Quaternion.hh>
40 #include <MathAlgo/SVD.hh>
52 covAngles.SetIdentity();
54 covAngles[3][3] *= 0.1;
55 covAngles[4][4] *= 0.1;
56 covAngles[5][5] *= 0.1;
58 cout <<
"Pos. for angles: " << posAngles << endl;
59 cout <<
"Angles: " << angles << endl;
60 cout <<
"Cov for pos./angles: " << covAngles << endl << endl;
61 cout <<
"==============================================================="
71 posQuat, quat, covQuat) !=0)
74 cout <<
"Pos. for quat: " << posQuat << endl;
75 cout <<
"Quaternion: " << quat << endl;
76 cout <<
"Cov for pos./quat: " << covQuat << endl << endl;
77 cout <<
"==============================================================="
82 posAngles, angles, covAngles) !=0)
85 cout <<
"Re-Pos. for angles: " << posAngles << endl;
86 cout <<
"Re-Angles: " << angles << endl;
87 cout <<
"Re-Cov for pos./angles: " << covAngles << endl;