Simplify functor according to Eade
parent
bb7b6b39c7
commit
440c3ea64b
|
@ -48,12 +48,15 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9,
|
|||
}
|
||||
|
||||
void ExpmapFunctor::init(bool nearZeroApprox) {
|
||||
WW = W * W;
|
||||
nearZero =
|
||||
nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
|
||||
if (!nearZero) {
|
||||
sin_theta = std::sin(theta);
|
||||
const double s2 = std::sin(theta / 2.0);
|
||||
one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
|
||||
A = sin_theta / theta;
|
||||
B = one_minus_cos / theta2;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -62,39 +65,39 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
|
|||
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(nearZeroApprox);
|
||||
if (!nearZero) {
|
||||
K = W / theta;
|
||||
KK = K * K;
|
||||
}
|
||||
}
|
||||
|
||||
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
|
||||
bool nearZeroApprox)
|
||||
: theta2(angle * angle), theta(angle) {
|
||||
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;
|
||||
W << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
|
||||
W *= angle;
|
||||
init(nearZeroApprox);
|
||||
if (!nearZero) {
|
||||
KK = K * K;
|
||||
}
|
||||
}
|
||||
|
||||
SO3 ExpmapFunctor::expmap() const {
|
||||
if (nearZero)
|
||||
return SO3(I_3x3 + W);
|
||||
return SO3(I_3x3 + W + 0.5 * WW);
|
||||
else
|
||||
return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
|
||||
return SO3(I_3x3 + A * W + B * WW);
|
||||
}
|
||||
|
||||
Matrix3 DexpFunctor::leftJacobian() const {
|
||||
if (nearZero) {
|
||||
return I_3x3 + 0.5 * W + one_sixth * WW;
|
||||
} else {
|
||||
return I_3x3 + B * W + C * WW;
|
||||
}
|
||||
}
|
||||
|
||||
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
|
||||
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
|
||||
if (nearZero) {
|
||||
dexp_ = I_3x3 - 0.5 * W;
|
||||
rightJacobian_ = I_3x3 - 0.5 * W + one_sixth * WW;
|
||||
} else {
|
||||
a = one_minus_cos / theta;
|
||||
b = 1.0 - sin_theta / theta;
|
||||
dexp_ = I_3x3 - a * K + b * KK;
|
||||
C = (1 - A) / theta2;
|
||||
rightJacobian_ = I_3x3 - B * W + C * WW;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -105,21 +108,23 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
|
|||
*H1 = 0.5 * skewSymmetric(v);
|
||||
} else {
|
||||
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
|
||||
const Vector3 Kv = K * v;
|
||||
double a = B * theta;
|
||||
double b = C * theta2;
|
||||
const Vector3 Kv = W * v / theta;
|
||||
const double Da = (sin_theta - 2.0 * a) / theta2;
|
||||
const double Db = (one_minus_cos - 3.0 * b) / theta2;
|
||||
*H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
|
||||
*H1 = (Db * W / theta - Da * I_3x3) * Kv * omega.transpose() -
|
||||
skewSymmetric(Kv * b / theta) +
|
||||
(a * I_3x3 - b * K) * skewSymmetric(v / theta);
|
||||
(a * I_3x3 - b * W / theta) * skewSymmetric(v / theta);
|
||||
}
|
||||
}
|
||||
if (H2) *H2 = dexp_;
|
||||
return dexp_ * v;
|
||||
if (H2) *H2 = rightJacobian_;
|
||||
return rightJacobian_ * v;
|
||||
}
|
||||
|
||||
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
|
||||
OptionalJacobian<3, 3> H2) const {
|
||||
const Matrix3 invDexp = dexp_.inverse();
|
||||
const Matrix3 invDexp = rightJacobian_.inverse();
|
||||
const Vector3 c = invDexp * v;
|
||||
if (H1) {
|
||||
Matrix3 D_dexpv_omega;
|
||||
|
|
|
@ -136,7 +136,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
|
|||
class GTSAM_EXPORT ExpmapFunctor {
|
||||
protected:
|
||||
const double theta2;
|
||||
Matrix3 W, K, KK;
|
||||
Matrix3 W, WW;
|
||||
double A, B; // Ethan Eade's constants
|
||||
bool nearZero;
|
||||
double theta, sin_theta, one_minus_cos; // only defined if !nearZero
|
||||
|
||||
|
@ -155,13 +156,16 @@ class GTSAM_EXPORT ExpmapFunctor {
|
|||
|
||||
/// Functor that implements Exponential map *and* its derivatives
|
||||
class DexpFunctor : public ExpmapFunctor {
|
||||
protected:
|
||||
static constexpr double one_sixth = 1.0 / 6.0;
|
||||
const Vector3 omega;
|
||||
double a, b;
|
||||
Matrix3 dexp_;
|
||||
double C; // Ethan Eade's C constant
|
||||
Matrix3 rightJacobian_;
|
||||
|
||||
public:
|
||||
/// Constructor with element of Lie algebra so(3)
|
||||
GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
|
||||
GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega,
|
||||
bool nearZeroApprox = false);
|
||||
|
||||
// NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
|
||||
// (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
|
||||
|
@ -169,16 +173,21 @@ class DexpFunctor : public ExpmapFunctor {
|
|||
// expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
|
||||
// This maps a perturbation v in the tangent space to
|
||||
// a perturbation on the manifold Expmap(dexp * v) */
|
||||
const Matrix3& dexp() const { return dexp_; }
|
||||
const Matrix3& dexp() const { return rightJacobian_; }
|
||||
|
||||
/// Multiplies with dexp(), with optional derivatives
|
||||
GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
|
||||
OptionalJacobian<3, 3> H2 = {}) const;
|
||||
GTSAM_EXPORT Vector3 applyDexp(const Vector3& v,
|
||||
OptionalJacobian<3, 3> H1 = {},
|
||||
OptionalJacobian<3, 3> H2 = {}) const;
|
||||
|
||||
/// Multiplies with dexp().inverse(), with optional derivatives
|
||||
GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
|
||||
OptionalJacobian<3, 3> H1 = {},
|
||||
OptionalJacobian<3, 3> H2 = {}) const;
|
||||
OptionalJacobian<3, 3> H1 = {},
|
||||
OptionalJacobian<3, 3> H2 = {}) const;
|
||||
|
||||
// Compute the left Jacobian for Exponential map in SO(3)
|
||||
// Note precomputed, as not used as much
|
||||
Matrix3 leftJacobian() const;
|
||||
};
|
||||
} // namespace so3
|
||||
|
||||
|
|
Loading…
Reference in New Issue