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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
gtsam/geometry/SO3.cpp
View File

@ -68,16 +68,13 @@ SO3 ExpmapFunctor::expmap() const {
DexpFunctor::DexpFunctor(const Vector3& omega)
: ExpmapFunctor(omega), omega(omega) {
if (nearZero) return;
a = one_minus_cos / theta;
b = 1.0 - sin_theta / theta;
}
SO3 DexpFunctor::dexp() const {
if (nearZero)
return I_3x3 - 0.5 * W;
else
return I_3x3 - a * K + b * KK;
dexp_ = I_3x3 - 0.5 * W;
else {
a = one_minus_cos / theta;
b = 1.0 - sin_theta / theta;
dexp_ = I_3x3 - a * K + b * KK;
}
}
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;
return v - 0.5 * omega.cross(v);
}
const Vector3 Kv = omega.cross(v / theta);
const Vector3 KKv = omega.cross(Kv / theta);
if (H1) {
// 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 double Da = (sin_theta - 2.0 * a) / theta2;
const double Db = (one_minus_cos - 3.0 * b) / theta2;
*H1 = (-Da * Kv + Db * KKv) * omega.transpose() + a * T -
skewSymmetric(Kv * b / theta) - b * K * T;
}
if (H2) *H2 = dexp();
return v - a * Kv + b * KKv;
if (H2) *H2 = dexp_;
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
@ -121,12 +131,6 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
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) {
using std::sqrt;

20
gtsam/geometry/SO3.h
View File

@ -102,11 +102,6 @@ public:
/// Derivative of Expmap
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
* \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
// 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 {
/// Functor implementing Exponential map
@ -156,7 +152,7 @@ class ExpmapFunctor {
/// Constructor with axis-angle
ExpmapFunctor(const Vector3& axis, double angle);
/// Rodrgues formula
/// Rodrigues formula
SO3 expmap() const;
};
@ -164,6 +160,7 @@ class ExpmapFunctor {
class DexpFunctor : public ExpmapFunctor {
const Vector3 omega;
double a, b;
Matrix3 dexp_;
public:
/// 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)
// This maps a perturbation v in the tangent space to
// a perturbation on the manifold Expmap(dexp * v) */
SO3 dexp() const;
const Matrix3& dexp() const { return dexp_; }
/// Multiplies with dexp(), with optional derivatives
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const;
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
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

74
gtsam/geometry/tests/testSO3.cpp
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;
boost::function<Vector3(const Vector3&, const Vector3&)> f =
boost::bind(SO3::ApplyExpmapDerivative, _1, _2, boost::none, boost::none);
for (Vector3 omega : {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 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));
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
so3::DexpFunctor local(omega);
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
Vector3(0.4, 0.3, 0.2)}) {
EXPECT(assert_equal(Vector3(local.dexp() * v),
local.applyDexp(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(apply, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(apply, omega, v), aH2));
EXPECT(assert_equal(local.dexp(), aH2));
}
}
}
/* ************************************************************************* */
TEST(SO3, ApplyExpmapDerivative2) {
Matrix aH1, aH2;
boost::function<Vector3(const Vector3&, const Vector3&)> f =
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));
}
Vector3 applyInv(const Vector3& omega, const Vector3& v) {
so3::DexpFunctor local(omega);
return local.applyInvDexp(v);
}
/* ************************************************************************* */
TEST(SO3, ApplyExpmapDerivative3) {
TEST(SO3, ApplyInvDexp) {
Matrix aH1, aH2;
boost::function<Vector3(const Vector3&, const Vector3&)> f =
boost::bind(SO3::ApplyExpmapDerivative, _1, _2, boost::none, boost::none);
const Vector3 omega(0.1, 0.2, 0.3), v(0.4, 0.3, 0.2);
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));
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
so3::DexpFunctor local(omega);
Matrix invDexp = local.dexp().inverse();
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
Vector3(0.4, 0.3, 0.2)}) {
EXPECT(
assert_equal(Vector3(invDexp * v), local.applyInvDexp(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(applyInv, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(applyInv, omega, v), aH2));
EXPECT(assert_equal(invDexp, aH2));
}
}
}
//******************************************************************************