Basic Image AlgorithmS Library  2.8.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
PoseParametrizationCovariance.cpp
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>
7 
8 using namespace BIAS;
9 using namespace std;
10 
11 PoseParametrizationCovariance::
12 PoseParametrizationCovariance()
13  : Matrix<POSECOVARIANCE_TYPE>(7,7)
14 {}
15 
16 PoseParametrizationCovariance::
17 ~PoseParametrizationCovariance()
18 {}
19 
20 
21 Matrix3x3<POSECOVARIANCE_TYPE> PoseParametrizationCovariance::
22 GetCovC() const
23 {
24  Matrix3x3<double> covC;
25  for (int c=0; c<3; c++){
26  for (int r=0; r<3; r++){
27  covC[r][c] = (*this)[r][c];
28  }
29  }
30  return covC;
31 }
32 
33 Matrix4x4<POSECOVARIANCE_TYPE> PoseParametrizationCovariance::
34 GetCovQ() const
35 {
36  Matrix4x4<double> covQ;
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];
40  }
41  }
42  return covQ;
43 
44 }
45 
46 
47 // this pice of code is erroneous and replaced by the
48 // unscented transfrom in
49 // void PoseParametrizationCovariance::
50 // SetFromEulerAngles(const Matrix<double>& cov_pose,
51 // const Vector<double>& pose)
52 // {
53 // if (cov_pose.num_cols() != 6 || cov_pose.num_rows() != 6){
54 // BEXCEPTION("invalid argument "<<cov_pose);
55 // }
56 // if (pose.size() != 6 ){
57 // BEXCEPTION("invalid argument "<<pose);
58 // }
59 // //BIASERR("this is only correct when the trace of the rotation matrix is "
60 // // "greater than zero");
61 // RMatrixBase R;
62 // R.SetXYZ(pose[3], pose[4], pose[5]);
63 // if (R.Trace()<0.0){
64 // BEXCEPTION("trace of rotation matrix must > 0.0");
65 // }
66 
67 // Matrix<double> J(7, 6, MatrixIdentity);
68 
69 // /** matlab code:
70 // alpha = sym('alpha');
71 // beta = sym('beta');
72 // gamma = sym('gamma');
73 // % compute rotation matrix from euler angles
74 // R_gamma = [ cos(gamma) -sin(gamma) 0
75 // sin(gamma) cos(gamma) 0
76 // 0 0 1];
77 // R_beta = [ cos(beta) 0 sin(beta)
78 // 0 1 0
79 // -sin(beta) 0 cos(beta) ];
80 // R_alpha = [ 1 0 0
81 // 0 cos(alpha) -sin(alpha)
82 // 0 sin(alpha) cos(alpha) ];
83 // R = R_gamma * R_beta * R_alpha;
84 // % compute quaternion q from rotation matrix
85 // q = [ 1/2 * sqrt(R(1,1) + R(2,2) + R(3,3) + 1)
86 // (R(3,2)-R(2,3))/(2*sqrt(R(1,1) + R(2,2) + R(3,3) + 1))
87 // (R(1,3)-R(3,1))/(2*sqrt(R(1,1) + R(2,2) + R(3,3) + 1))
88 // (R(2,1)-R(1,2))/(2*sqrt(R(1,1) + R(2,2) + R(3,3) + 1)) ];
89 // % compute jacobian
90 // j = jacobian(q, [alpha, beta, gamma]);
91 // disp('J[0][0] = ');
92 // disp(j(1,1));
93 // disp('J[1][0] = ');
94 // disp(j(2,1));
95 // disp('J[2][0] = ');
96 // disp(j(3,1));
97 // disp('J[3][0] = ');
98 // disp(j(4,1));
99 // disp('J[0][1] = ');
100 // disp(j(1,2));
101 // disp('J[1][1] = ');
102 // disp(j(2,2));
103 // disp('J[2][1] = ');
104 // disp(j(3,2));
105 // disp('J[3][1] = ');
106 // disp(j(4,2));
107 // disp('J[0][2] = ');
108 // disp(j(1,3));
109 // disp('J[1][2] = ');
110 // disp(j(2,3));
111 // disp('J[2][2] = ');
112 // disp(j(3,3));
113 // disp('J[3][2] = ');
114 // disp(j(4,3)); */
115 // double alpha = pose[3], beta = pose[4], gamma = pose[5];
116 // // I guess this should be optimized
117 // J[0][0] =
118 // 1/4/sqrt(cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1)*(-cos(gamma)*sin(alpha)+sin(gamma)*sin(beta)*cos(alpha)-cos(beta)*sin(alpha));
119 // J[1][0] =
120 // 1/2*(cos(beta)*cos(alpha)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha))/sqrt(cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1)-1/4*(cos(beta)*sin(alpha)+cos(gamma)*sin(alpha)-sin(gamma)*sin(beta)*cos(alpha))/pow((cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1),(3.0/2.0))*(-cos(gamma)*sin(alpha)+sin(gamma)*sin(beta)*cos(alpha)-cos(beta)*sin(alpha));
121 // J[2][0] =
122 // 1/2*(sin(gamma)*cos(alpha)-cos(gamma)*sin(beta)*sin(alpha))/sqrt(cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1)-1/4*(sin(gamma)*sin(alpha)+cos(gamma)*sin(beta)*cos(alpha)+sin(beta))/pow((cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1),(3.0/2.0))*(-cos(gamma)*sin(alpha)+sin(gamma)*sin(beta)*cos(alpha)-cos(beta)*sin(alpha));
123 // J[3][0] =
124 // 1/2*(-sin(gamma)*sin(alpha)-cos(gamma)*sin(beta)*cos(alpha))/sqrt(cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1)-1/4*(sin(gamma)*cos(beta)+sin(gamma)*cos(alpha)-cos(gamma)*sin(beta)*sin(alpha))/pow((cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1),(3.0/2.0))*(-cos(gamma)*sin(alpha)+sin(gamma)*sin(beta)*cos(alpha)-cos(beta)*sin(alpha));
125 // J[0][1] =
126 // 1/4/sqrt(cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1)*(-cos(gamma)*sin(beta)+sin(gamma)*cos(beta)*sin(alpha)-sin(beta)*cos(alpha));
127 // J[1][1] =
128 // 1/2*(-sin(beta)*sin(alpha)-sin(gamma)*cos(beta)*cos(alpha))/sqrt(cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1)-1/4*(cos(beta)*sin(alpha)+cos(gamma)*sin(alpha)-sin(gamma)*sin(beta)*cos(alpha))/pow((cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1),(3.0/2.0))*(-cos(gamma)*sin(beta)+sin(gamma)*cos(beta)*sin(alpha)-sin(beta)*cos(alpha));
129 // J[2][1] =
130 // 1/2*(cos(gamma)*cos(beta)*cos(alpha)+cos(beta))/sqrt(cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1)-1/4*(sin(gamma)*sin(alpha)+cos(gamma)*sin(beta)*cos(alpha)+sin(beta))/pow((cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1), (3.0/2.0))*(-cos(gamma)*sin(beta)+sin(gamma)*cos(beta)*sin(alpha)-sin(beta)*cos(alpha));
131 // J[3][1] =
132 // 1/2*(-sin(gamma)*sin(beta)-cos(gamma)*cos(beta)*sin(alpha))/sqrt(cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1)-1/4*(sin(gamma)*cos(beta)+sin(gamma)*cos(alpha)-cos(gamma)*sin(beta)*sin(alpha))/pow((cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1), (3.0/2.0))*(-cos(gamma)*sin(beta)+sin(gamma)*cos(beta)*sin(alpha)-sin(beta)*cos(alpha));
133 // J[0][2] =
134 // 1/4/sqrt(cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1)*(-sin(gamma)*cos(beta)-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha));
135 // J[1][2] =
136 // 1/2*(-sin(gamma)*sin(alpha)-cos(gamma)*sin(beta)*cos(alpha))/sqrt(cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1)-1/4*(cos(beta)*sin(alpha)+cos(gamma)*sin(alpha)-sin(gamma)*sin(beta)*cos(alpha))/pow((cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1),(3.0/2.0))*(-sin(gamma)*cos(beta)-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha));
137 // J[2][2] =
138 // 1/2*(cos(gamma)*sin(alpha)-sin(gamma)*sin(beta)*cos(alpha))/sqrt(cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1)-1/4*(sin(gamma)*sin(alpha)+cos(gamma)*sin(beta)*cos(alpha)+sin(beta))/pow((cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1),(3.0/2.0))*(-sin(gamma)*cos(beta)-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha));
139 // J[3][2] =
140 // 1/2*(cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha))/sqrt(cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1)-1/4*(sin(gamma)*cos(beta)+sin(gamma)*cos(alpha)-cos(gamma)*sin(beta)*sin(alpha))/pow((cos(gamma)*cos(beta)+cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha)+cos(beta)*cos(alpha)+1),(3.0/2.0))*(-sin(gamma)*cos(beta)-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha));
141 
142 // cout << "J: "<<J<<endl;
143 
144 // *this = J * cov_pose * J.Transpose();
145 
146 // }
147 
148 
149 void PoseParametrizationCovariance::
150 Transform(const Matrix4x4<double>& transf)
151 {
152  Matrix4x4<double> T = transf;
153  // normalize the transformation such that the last row reads 0 0 0 1
154  //if ( !Equal(T[3][3], 1.0) ) T /= T[3][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);
159  }
160 
161  //first of all extract rotation from affine transformation.
162  // It can be shown that the translation does not influence the covariances.
163  // It is therefore neglected. Scale and rotation are found in the upper
164  // left 3x3 matrix and can be keept together for purpose of covC
165  // transformation. Alone the rotation influences the covariance of Q
166  // and must be transformed into a quaternion multiplication matrix.
167 
168 
169  RMatrixBase rotAlone;
170  Quaternion<double> qRot;
171  Matrix4x4<double> qM;
172  Matrix<double> J(7, 7, MatrixZero);
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];
178  }
179  }
180  for(column = 0; column < 3; column++) {
181  Vector3<double> currentCol = rotAlone.GetColumn(column);
182  currentCol.Normalize();
183  rotAlone.SetColumn(column, currentCol);
184  }
185  rotAlone.GetQuaternion(qRot);
186  qM = qRot.GetQuaternionMultMatrixLeft();
187  for( row = 0; row<4; row++) {
188  for(column = 0; column<4; column++) {
189  J[row+3][column+3] = qM[row][column];
190  }
191  }
192  *this = J * (*this) * J.Transpose();
193 
194 
195  // /** The seven dimensional pose p is transformed nonlinearily as follows
196 // f_(0..2) (p) = T[0..2][0..2] * p[0..2]
197 // f_3 (p) = 0
198 // f_(4..6) (p) = T[0..2][0..2] * 1/sin(acos(p[3])) * p[4..6]
199 // explanation:
200 // The pose is composed of the position (p[0..2]) and the orientation
201 // parametrized as a quaternion (p[3..6]). The transformation of the
202 // position is straight forward, i.e. by multiplying with T. The Jacobian of
203 // this transformation is aequivalent to the upper left 3x3 matrix of T.
204 // The union quaternion representing a rotation about the axis [x, y, z]'
205 // with angle a can be interpreted as
206 // q = [ cos(a/2), x* sin(*a/2), y*sin(a/2), z*sin(a/2)]'
207 // and the transformation of the quaternion is aequivalent to the transf.
208 // of the axis [x, y, z] = 1/(sin(acos(q[0])) * q[1..3])
209 // hint: d/dx (1/(sin(acos(x))) = x/(1-x^2)^3/2 */
210 // Matrix<double> J(7, 7, MatrixZero);
211 // double q0 = pose.GetOrientation()[0];
212 // for (int y=0; y<3; y++){
213 // for (int x=0; x<3; x++){
214 // J[y][x] = J[y+4][x+4] = T[y][x];
215 // }
216 // J[y+4][3] = q0 / pow( (1.0 - q0*q0), 1.5);
217 // }
218 // *this = J * *this * J.Transpose();
219 }
BIAS::PoseParametrizationCovariance::GetCovC
Matrix3x3< POSECOVARIANCE_TYPE > GetCovC() const
Definition: PoseParametrizationCovariance.cpp:22
BIAS::Matrix3x3::SetColumn
void SetColumn(const unsigned int col, const Vector3< T > &c)
Definition: Matrix3x3.cpp:323
BIAS::Matrix::Transpose
Matrix< T > Transpose() const
transpose function, storing data destination matrix
Definition: Matrix.hh:823
BIAS::Matrix3x3::GetColumn
void GetColumn(const unsigned int col, Vector3< T > &r) const
extract one column (&#39;Spalte&#39;) from this matrix (for convenience)
Definition: Matrix3x3.cpp:299
BIAS::PoseParametrizationCovariance::GetCovQ
Matrix4x4< POSECOVARIANCE_TYPE > GetCovQ() const
Definition: PoseParametrizationCovariance.cpp:34
BIAS::Quaternion::GetQuaternionMultMatrixLeft
Matrix4x4< QUAT_TYPE > GetQuaternionMultMatrixLeft() const
Returns matrix which expresses (left) quaternion multiplication.
BIAS::RMatrixBase::GetQuaternion
int GetQuaternion(Quaternion< ROTATION_MATRIX_TYPE > &quat) const
Calculates quaternion representation for this rotation matrix.
Definition: RMatrixBase.cpp:345
BIAS::Matrix3x3
is a &#39;fixed size&#39; quadratic matrix of dim.
Definition: Matrix.hh:54
BIAS::Equal
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_...
Definition: CompareFloatingPoint.cpp:23
BIAS::Matrix
matrix class with arbitrary size, indexing is row major.
Definition: GenerateGauss.hh:31
BIAS::RMatrixBase
Implements a 3D rotation matrix.
Definition: RMatrixBase.hh:44
BIAS::Matrix4x4
is a &#39;fixed size&#39; quadratic matrix of dim.
Definition: Matrix4x4.hh:54
BIAS::PoseParametrizationCovariance::Transform
void Transform(const Matrix4x4< double > &transf)
Given a 6D pose (position and euler angles) and its associated covariance, the function transforms th...
Definition: PoseParametrizationCovariance.cpp:150
BIAS::Vector3::Normalize
Vector3< T > & Normalize()
normalize this vector to length 1
Definition: Vector3.hh:663
Generated on Tue Feb 6 2018 02:01:31 for Basic Image AlgorithmS Library by   doxygen 1.8.5