applyInvDexp
parent
8c6383c711
commit
5e8ff450ee
|
@ -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;
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
|
|
|
@ -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));
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
|
|
Loading…
Reference in New Issue