applyInvDexp

release/4.3a0
Frank Dellaert 2016-02-01 12:43:05 -08:00
parent 8c6383c711
commit 5e8ff450ee
3 changed files with 68 additions and 68 deletions

View File

@ -68,16 +68,13 @@ SO3 ExpmapFunctor::expmap() const {
DexpFunctor::DexpFunctor(const Vector3& omega) DexpFunctor::DexpFunctor(const Vector3& omega)
: ExpmapFunctor(omega), omega(omega) { : ExpmapFunctor(omega), omega(omega) {
if (nearZero) return; if (nearZero)
dexp_ = I_3x3 - 0.5 * W;
else {
a = one_minus_cos / theta; a = one_minus_cos / theta;
b = 1.0 - sin_theta / theta; b = 1.0 - sin_theta / theta;
dexp_ = I_3x3 - a * K + b * KK;
} }
SO3 DexpFunctor::dexp() const {
if (nearZero)
return I_3x3 - 0.5 * W;
else
return I_3x3 - a * K + b * KK;
} }
Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
@ -87,18 +84,31 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
if (H2) *H2 = I_3x3; if (H2) *H2 = I_3x3;
return v - 0.5 * omega.cross(v); return v - 0.5 * omega.cross(v);
} }
const Vector3 Kv = omega.cross(v / theta);
const Vector3 KKv = omega.cross(Kv / theta);
if (H1) { if (H1) {
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
const Vector3 Kv = omega.cross(v / theta);
const Vector3 KKv = omega.cross(Kv / theta);
const Matrix3 T = skewSymmetric(v / theta); const Matrix3 T = skewSymmetric(v / theta);
const double Da = (sin_theta - 2.0 * a) / theta2; const double Da = (sin_theta - 2.0 * a) / theta2;
const double Db = (one_minus_cos - 3.0 * b) / theta2; const double Db = (one_minus_cos - 3.0 * b) / theta2;
*H1 = (-Da * Kv + Db * KKv) * omega.transpose() + a * T - *H1 = (-Da * Kv + Db * KKv) * omega.transpose() + a * T -
skewSymmetric(Kv * b / theta) - b * K * T; skewSymmetric(Kv * b / theta) - b * K * T;
} }
if (H2) *H2 = dexp(); if (H2) *H2 = dexp_;
return v - a * Kv + b * KKv; return dexp_ * v;
}
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 invDexp = dexp_.inverse();
const Vector3 c = invDexp * v;
if (H1) {
Matrix3 D_dexpv_omega;
applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping
*H1 = -invDexp* D_dexpv_omega;
}
if (H2) *H2 = invDexp;
return c;
} }
} // namespace so3 } // namespace so3
@ -121,12 +131,6 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
return so3::DexpFunctor(omega).dexp(); return so3::DexpFunctor(omega).dexp();
} }
Vector3 SO3::ApplyExpmapDerivative(const Vector3& omega, const Vector3& v,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) {
return so3::DexpFunctor(omega).applyDexp(v, H1, H2);
}
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
using std::sqrt; using std::sqrt;

View File

@ -102,11 +102,6 @@ public:
/// Derivative of Expmap /// Derivative of Expmap
static Matrix3 ExpmapDerivative(const Vector3& omega); static Matrix3 ExpmapDerivative(const Vector3& omega);
/// Implement ExpmapDerivative(omega) * v, with derivatives
static Vector3 ApplyExpmapDerivative(const Vector3& omega, const Vector3& v,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none);
/** /**
* Log map at identity - returns the canonical coordinates * Log map at identity - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation * \f$ [R_x,R_y,R_z] \f$ of this rotation
@ -137,6 +132,7 @@ public:
// This namespace exposes two functors that allow for saving computation when // This namespace exposes two functors that allow for saving computation when
// exponential map and its derivatives are needed at the same location in so<3> // exponential map and its derivatives are needed at the same location in so<3>
// The second functor also implements dedicated methods to apply dexp and/or inv(dexp)
namespace so3 { namespace so3 {
/// Functor implementing Exponential map /// Functor implementing Exponential map
@ -156,7 +152,7 @@ class ExpmapFunctor {
/// Constructor with axis-angle /// Constructor with axis-angle
ExpmapFunctor(const Vector3& axis, double angle); ExpmapFunctor(const Vector3& axis, double angle);
/// Rodrgues formula /// Rodrigues formula
SO3 expmap() const; SO3 expmap() const;
}; };
@ -164,6 +160,7 @@ class ExpmapFunctor {
class DexpFunctor : public ExpmapFunctor { class DexpFunctor : public ExpmapFunctor {
const Vector3 omega; const Vector3 omega;
double a, b; double a, b;
Matrix3 dexp_;
public: public:
/// Constructor with element of Lie algebra so(3) /// Constructor with element of Lie algebra so(3)
@ -175,11 +172,16 @@ class DexpFunctor : public ExpmapFunctor {
// expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
// 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; const Matrix3& dexp() const { return dexp_; }
/// Multiplies with dexp(), with optional derivatives /// Multiplies with dexp(), with optional derivatives
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2) const; OptionalJacobian<3, 3> H2 = boost::none) const;
/// Multiplies with dexp().inverse(), with optional derivatives
Vector3 applyInvDexp(const Vector3& v,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
}; };
} // namespace so3 } // namespace so3

View File

@ -204,56 +204,50 @@ TEST(SO3, JacobianLogmap) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SO3, ApplyExpmapDerivative1) { Vector3 apply(const Vector3& omega, const Vector3& v) {
so3::DexpFunctor local(omega);
return local.applyDexp(v);
}
/* ************************************************************************* */
TEST(SO3, ApplyDexp) {
Matrix aH1, aH2; Matrix aH1, aH2;
boost::function<Vector3(const Vector3&, const Vector3&)> f = for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
boost::bind(SO3::ApplyExpmapDerivative, _1, _2, boost::none, boost::none); Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
for (Vector3 omega : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) { so3::DexpFunctor local(omega);
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) { for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
Matrix3 H = SO3::ExpmapDerivative(omega); Vector3(0.4, 0.3, 0.2)}) {
Vector3 expected = H * v; EXPECT(assert_equal(Vector3(local.dexp() * v),
EXPECT(assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v))); local.applyDexp(v, aH1, aH2)));
EXPECT(assert_equal(expected, EXPECT(assert_equal(numericalDerivative21(apply, omega, v), aH1));
SO3::ApplyExpmapDerivative(omega, v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative22(apply, omega, v), aH2));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(local.dexp(), aH2));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(H, aH2));
} }
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SO3, ApplyExpmapDerivative2) { Vector3 applyInv(const Vector3& omega, const Vector3& v) {
Matrix aH1, aH2; so3::DexpFunctor local(omega);
boost::function<Vector3(const Vector3&, const Vector3&)> f = return local.applyInvDexp(v);
boost::bind(SO3::ApplyExpmapDerivative, _1, _2, boost::none, boost::none);
const Vector3 omega(0, 0, 0);
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) {
Matrix3 H = SO3::ExpmapDerivative(omega);
Vector3 expected = H * v;
EXPECT(assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v)));
EXPECT(
assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(H, aH2));
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SO3, ApplyExpmapDerivative3) { TEST(SO3, ApplyInvDexp) {
Matrix aH1, aH2; Matrix aH1, aH2;
boost::function<Vector3(const Vector3&, const Vector3&)> f = for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
boost::bind(SO3::ApplyExpmapDerivative, _1, _2, boost::none, boost::none); Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
const Vector3 omega(0.1, 0.2, 0.3), v(0.4, 0.3, 0.2); so3::DexpFunctor local(omega);
Matrix3 H = SO3::ExpmapDerivative(omega); Matrix invDexp = local.dexp().inverse();
Vector3 expected = H * v; for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
EXPECT(assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v))); Vector3(0.4, 0.3, 0.2)}) {
EXPECT( EXPECT(
assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v, aH1, aH2))); assert_equal(Vector3(invDexp * v), local.applyInvDexp(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative21(applyInv, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); EXPECT(assert_equal(numericalDerivative22(applyInv, omega, v), aH2));
EXPECT(assert_equal(H, aH2)); EXPECT(assert_equal(invDexp, aH2));
}
}
} }
//****************************************************************************** //******************************************************************************