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

Switch to Skew[axis] = Skew[omega/angle] for simpler forms

release/4.3a0
Frank Dellaert 2016-02-01 00:48:47 -08:00
parent f054a00457
commit c838d7c133
1 changed files with 44 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
gtsam/geometry/SO3.cpp
View File

@ -30,40 +30,55 @@ namespace gtsam {
struct ExpmapImpl { struct ExpmapImpl {
const Vector3 omega; const Vector3 omega;
const double theta2; const double theta2;
Matrix3 W; Matrix3 W, K, KK;
bool nearZero; bool nearZero;
double theta, sin_over_theta, one_minus_cos; double theta, sin_theta, sin_over_theta, one_minus_cos, a, b;
void Initialize() { void init() {
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; // Skew[omega]
nearZero = (theta2 <= std::numeric_limits<double>::epsilon()); nearZero = (theta2 <= std::numeric_limits<double>::epsilon());
if (!nearZero) { if (!nearZero) {
theta = std::sqrt(theta2); // rotation angle theta = std::sqrt(theta2); // rotation angle
sin_over_theta = std::sin(theta) / theta; sin_theta = std::sin(theta);
sin_over_theta = sin_theta / theta;
const double s2 = std::sin(theta / 2.0); const double s2 = std::sin(theta / 2.0);
one_minus_cos = one_minus_cos =
2.0 * s2 * s2; // numerically better behaved than [1 - cos(theta)] 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
a = one_minus_cos / theta;
b = 1.0 - sin_over_theta;
} }
} }
// Constructor with element of Lie algebra so(3): W = omega^, normalized by // Constructor with element of Lie algebra so(3): W = omega^, normalized by
// normx // normx
ExpmapImpl(const Vector3& omega) : omega(omega), theta2(omega.dot(omega)) { ExpmapImpl(const Vector3& omega) : omega(omega), theta2(omega.dot(omega)) {
Initialize(); const double wx = omega.x(), wy = omega.y(), wz = omega.z();
W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
init();
if (!nearZero) {
theta = std::sqrt(theta2);
K = W / theta;
KK = K * K;
}
} }
// Constructor with axis-angle // Constructor with axis-angle
ExpmapImpl(const Vector3& axis, double theta) ExpmapImpl(const Vector3& axis, double angle)
: omega(axis * theta), theta2(theta * theta) { : omega(axis * angle), theta2(angle * angle) {
Initialize(); const double ax = axis.x(), ay = axis.y(), az = axis.z();
K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
W = K * angle;
init();
if (!nearZero) {
theta = angle;
KK = K * K;
}
} }
SO3 operator()() const { SO3 operator()() const {
if (nearZero) if (nearZero)
return I_3x3 + W; return I_3x3 + W;
else else
return I_3x3 + sin_over_theta * W + one_minus_cos * W * W / theta2; return I_3x3 + sin_theta * K + one_minus_cos * K * K;
} }
// NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
@ -73,39 +88,32 @@ struct ExpmapImpl {
// This maps a perturbation v in the tangent space to // This maps a perturbation v in the tangent space to
// a perturbation on the manifold Expmap(dexp * v) */ // a perturbation on the manifold Expmap(dexp * v) */
SO3 dexp() const { SO3 dexp() const {
if (nearZero) { if (nearZero)
return I_3x3 - 0.5 * W; return I_3x3 - 0.5 * W;
} else { else
const double a = one_minus_cos / theta2; return I_3x3 - a * K + b * KK;
const double b = (1.0 - sin_over_theta) / theta2;
return I_3x3 - a * W + b * W * W;
}
} }
// Just multiplies with dexp() // Just multiplies with dexp()
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2 = boost::none) const { OptionalJacobian<3, 3> H2) const {
if (nearZero) { if (nearZero) {
if (H1) *H1 = 0.5 * skewSymmetric(v); if (H1) *H1 = 0.5 * skewSymmetric(v);
if (H2) *H2 = I_3x3; if (H2) *H2 = I_3x3;
return v; return v;
} else {
const double a = one_minus_cos / theta2;
const double b = (1.0 - sin_over_theta) / theta2;
Matrix3 dexp = I_3x3 - a * W + b * W * W;
if (H1) {
const Vector3 Wv = omega.cross(v);
const Vector3 WWv = omega.cross(Wv);
const Matrix3 T = skewSymmetric(v);
const double Da = (sin_over_theta - 2.0 * a) / theta2;
const double Db =
(3.0 * sin_over_theta - std::cos(theta) - 2.0) / theta2 / theta2;
*H1 = (-Da * Wv + Db * WWv) * omega.transpose() + a * T -
b * skewSymmetric(Wv) - b * W * T;
}
if (H2) *H2 = dexp;
return dexp * v;
} }
Matrix3 dexp = I_3x3 - a * K + b * KK;
if (H1) {
const Vector3 Kv = omega.cross(v / theta);
const Vector3 KKv = omega.cross(Kv / theta);
const Matrix3 T = skewSymmetric(v / theta);
const double Da = (sin_over_theta - 2.0 * a / theta) / theta;
const double Db = (3.0 * sin_over_theta - std::cos(theta) - 2.0) / theta2;
*H1 = (-Da * Kv + Db * KKv) * omega.transpose() + a * T -
skewSymmetric(Kv * b / theta) - b * K * T;
}
if (H2) *H2 = dexp;
return dexp * v;
} }
}; };