1 #include "PoseParametrizationCovariance.hh"
2 #include <Base/Debug/Exception.hh>
3 #include <Base/Common/CompareFloatingPoint.hh>
4 #include <Base/Geometry/PoseParametrization.hh>
5 #include <Base/Math/Matrix4x4.hh>
6 #include <Base/Geometry/RMatrixBase.hh>
13 :
Matrix<POSECOVARIANCE_TYPE>(7,7)
25 for (
int c=0; c<3; c++){
26 for (
int r=0; r<3; r++){
27 covC[r][c] = (*this)[r][c];
37 for (
int c=0; c<4; c++){
38 for (
int r=0; r<4; r++){
39 covQ[r][c] = (*this)[r+3][c+3];
155 if ( !
Equal(T[3][0], 0.0) || !
Equal(T[3][1], 0.0) ||
156 !
Equal(T[3][2], 0.0) || !
Equal(T[3][3], 1.0) ) {
157 BEXCEPTION(
"unsupported transformation format (only affine transformations"
158 " supported): "<<T<<
"\n"<<transf);
173 unsigned int row = 0;
174 unsigned int column = 0;
175 for( row = 0; row<3; row++) {
176 for(column = 0; column<3; column++) {
177 J[row][column] = rotAlone[row][column] = T[row][column];
180 for(column = 0; column < 3; column++) {
187 for( row = 0; row<4; row++) {
188 for(column = 0; column<4; column++) {
189 J[row+3][column+3] = qM[row][column];
Matrix3x3< POSECOVARIANCE_TYPE > GetCovC() const
void SetColumn(const unsigned int col, const Vector3< T > &c)
Matrix< T > Transpose() const
transpose function, storing data destination matrix
void GetColumn(const unsigned int col, Vector3< T > &r) const
extract one column ('Spalte') from this matrix (for convenience)
Matrix4x4< POSECOVARIANCE_TYPE > GetCovQ() const
Matrix4x4< QUAT_TYPE > GetQuaternionMultMatrixLeft() const
Returns matrix which expresses (left) quaternion multiplication.
int GetQuaternion(Quaternion< ROTATION_MATRIX_TYPE > &quat) const
Calculates quaternion representation for this rotation matrix.
PoseParametrizationCovariance()
~PoseParametrizationCovariance()
is a 'fixed size' quadratic matrix of dim.
bool Equal(const T left, const T right, const T eps)
comparison function for floating point values See http://www.boost.org/libs/test/doc/components/test_...
matrix class with arbitrary size, indexing is row major.
Implements a 3D rotation matrix.
is a 'fixed size' quadratic matrix of dim.
void Transform(const Matrix4x4< double > &transf)
Given a 6D pose (position and euler angles) and its associated covariance, the function transforms th...
Vector3< T > & Normalize()
normalize this vector to length 1