1 #include "RotationAveraging.hh"
3 #include <Base/Math/Matrix.hh>
4 #include <Base/Math/Matrix3x3.hh>
5 #include <Base/Math/Vector.hh>
6 #include <Base/Common/CompareFloatingPoint.hh>
7 #include <Base/Geometry/Quaternion.hh>
8 #include <Geometry/RMatrix.hh>
9 #include <MathAlgo/SVD3x3.hh>
14 RotationAveraging::RotationAveraging() : tolerance_(1e-10), maxIters_(10)
23 BIASASSERT(TestRotations(R));
24 const unsigned int n = R.size();
30 BIASERR(
"Initialization of mean rotation via geodesic L2-mean failed!");
34 for (
unsigned int iter = 0; iter < maxIters_; iter++)
39 for (
unsigned int i = 0; i < n; i++)
45 scale += 1.0 / resW.
NormL2();
52 updateAngle = meanResW.
NormL2();
53 BCDOUT(ROTATIONAVERGAGE_ITERS,
"Geodesic L1-mean : Iteration " << iter
54 <<
" : update angle = " << (updateAngle * 180.0 / M_PI) <<
" degree" << endl);
56 if (updateAngle < tolerance_)
61 meanResR.
Set(meanResW, updateAngle);
62 meanR = meanResR * meanR;
71 BIASASSERT(TestRotations(R));
72 const unsigned int n = R.size();
77 for (
unsigned int iter = 0; iter < maxIters_; iter++)
81 for (
unsigned int i = 0; i < n; i++)
87 meanResW /= (double)n;
89 updateAngle = meanResW.
NormL2();
90 BCDOUT(ROTATIONAVERGAGE_ITERS,
"Geodesic L2-mean : Iteration " << iter
91 <<
" : update angle = " << (updateAngle * 180.0 / M_PI) <<
" degree" << endl);
93 if (updateAngle < tolerance_)
98 meanResR.
Set(meanResW, updateAngle);
109 std::vector<BIAS::Quaternion<double> > Q;
111 for (
unsigned int i = 0; i < R.size(); i++) {
112 R[i].GetQuaternion(q);
126 BIASASSERT(TestRotations(Q));
128 for (
unsigned int i = 0; i < Q.size(); i++) {
142 BIASASSERT(TestRotations(R));
144 for (
unsigned int i = 0; i < R.size(); i++) {
145 R[i].GetRotationAxisAngle(w);
157 BIASASSERT(TestRotations(R));
159 for (
unsigned int i = 0; i < R.size(); i++)
161 S /= (double)R.size();
174 BIASASSERT(TestRotations(Q));
177 for (
unsigned int i = 0; i < Q.size(); i++)
178 for (
unsigned int j = 0; j < 4; j++)
179 for (
unsigned int k = 0; k < 4; k++)
180 A[j][k] += Q[i][j]*Q[i][k];
187 bool RotationAveraging::
188 TestRotations(
const std::vector<BIAS::RMatrix> &R)
191 BIASERR(
"No input rotations given!");
194 for (
unsigned int i = 0; i < R.size(); i++) {
196 BIASERR(
"Input rotation angle larger than 180 degree!");
203 bool RotationAveraging::
207 BIASERR(
"No input rotations given!");
210 for (
unsigned int i = 0; i < Q.size(); i++) {
212 BIASERR(
"Input rotation angle is larger than 180 degree!");
virtual void EnforceConstraints()
Enforce orthonormality constraints and right-handed rotation with SVD.
computes and holds the singular value decomposition of a rectangular (not necessarily quadratic) Matr...
int ChordalL2Mean(const std::vector< BIAS::RMatrix > &R, BIAS::RMatrix &meanR)
Compute chordal L2-mean of rotations.
void SetZero()
set all values to 0
void SetZero()
set all values to 0
int LogarithmicL2Mean(const std::vector< BIAS::RMatrix > &R, BIAS::RMatrix &meanR)
Compute matrix logarithm L2-mean of rotations.
Vector< T > GetCol(const int &col) const
return a copy of column "col", zero based counting
int SetFromQuaternion(const Quaternion< ROTATION_MATRIX_TYPE > &q)
Set rotation matrix from a quaternion.
void Normalize()
Scales quaternion to unit length, i.e.
void Set(const Vector3< ROTATION_MATRIX_TYPE > &w, const ROTATION_MATRIX_TYPE phi)
Set from rotation axis w and angle phi (in rad)
bool GreaterEqual(const T left, const T right, const T eps=std::numeric_limits< T >::epsilon())
comparison function for floating point values
Matrix< double > GetV() const
return V
int GeodesicL1Mean(const std::vector< BIAS::RMatrix > &R, BIAS::RMatrix &meanR)
Compute geodesic L1-mean (geodesic median) of rotations.
long int NewDebugLevel(const std::string &name)
creates a new debuglevel
void MultiplyIP(const T &scalar)
Multiplication (in place) of an scalar.
void SetFromAxisAngle(Vector3< ROTATION_MATRIX_TYPE > w)
Set from rotation axis * angle (modified Rodrigues vector)
int GeodesicL2Mean(const std::vector< BIAS::RMatrix > &R, BIAS::RMatrix &meanR)
Compute geodesic L2-mean (Karcher/geometric mean) of rotations.
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_...
Matrix3x3< T > Transpose() const
returns transposed matrix tested 12.06.2002
void MultiplyIP(const T &scalar)
Multiplication (in place) of an scalar.
int GetRotationAxisAngle(Vector3< ROTATION_MATRIX_TYPE > &axis, ROTATION_MATRIX_TYPE &angle) const
Calculates angle and rotation axis representation for this rotation matrix.
int QuaternionL2Mean(const std::vector< BIAS::RMatrix > &R, BIAS::RMatrix &meanR)
Compute quaternion L2-mean of rotations.
Vector3< T > & Normalize()
normalize this vector to length 1
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)