12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Mathematica closed form optimization (procrastination?) gone wild

release/4.3a0
Frank Dellaert 2012-01-06 20:44:06 +00:00
parent ad0212354c
commit 0ba97cc39a
1 changed files with 19 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
gtsam/geometry/Rot3M.cpp
View File

@ -243,12 +243,12 @@ Rot3 Rot3::retract(const Vector& omega) const {
#else
#ifdef SLOW_CAYLEY
Matrix Omega = skewSymmetric(omega);
return (*this)*Cayley(-Omega/2);
return (*this)*Cayley<3>(-Omega/2);
#else
double x = omega(0), y = omega(1), z = omega(2);
double x2 = x*x, y2 = y*y, z2 = z*z;
double xy = x*y, xz = x*z, yz = y*z;
double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0*f;
const double x = omega(0), y = omega(1), z = omega(2);
const double x2 = x*x, y2 = y*y, z2 = z*z;
const double xy = x*y, xz = x*z, yz = y*z;
const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0*f;
return (*this)* Rot3(
(4+x2-y2-z2)*f, (xy - 2*z)*_2f, (xz + 2*y)*_2f,
(xy + 2*z)*_2f, (4-x2+y2-z2)*f, (yz - 2*x)*_2f,
@ -263,19 +263,25 @@ Vector Rot3::localCoordinates(const Rot3& T) const {
#ifdef CORRECT_ROT3_EXMAP
return Logmap(between(T));
#else
// original
// Matrix Omega = Cayley(between(T).matrix());
// Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix());
#ifdef SLOW_CAYLEY
// using templated version of Cayley
Matrix Omega = Cayley<3>(A);
// completely inlined Cayley from template
// Eigen::Matrix3d Omega = (Eigen::Matrix3d::Identity() - A)*(Eigen::Matrix3d::Identity() + A).inverse();
return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0));
#else
// Mathematica closed form optimization (procrastination?) gone wild:
const double a=A(0,0),b=A(0,1),c=A(0,2);
const double d=A(1,0),e=A(1,1),f=A(1,2);
const double g=A(2,0),h=A(2,1),i=A(2,2);
const double di = d*i, ce = c*e, cd = c*d, fg=f*g;
const double M = 1 + e - f*h + i + e*i;
const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg));
const double x = (a * f - cd + f) * K;
const double y = (b * f - ce - c) * K;
const double z = (fg - di - d) * K;
return -2 * Vector_(3, x, y, z);
#endif
#endif
}