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

Add nearZeroApprox flag and make sure those cases are covered in tests

release/4.3a0
Frank Dellaert 2016-02-01 23:51:33 -08:00
parent 6da59d73bd
commit 4ebd3f48c3
3 changed files with 57 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
gtsam/geometry/SO3.cpp
View File

@ -27,8 +27,8 @@ namespace gtsam {
namespace so3 {
void ExpmapFunctor::init() {
nearZero = (theta2 <= std::numeric_limits<double>::epsilon());
void ExpmapFunctor::init(bool nearZeroApprox) {
nearZero = nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
if (nearZero) return;
theta = std::sqrt(theta2); // rotation angle
sin_theta = std::sin(theta);
@ -36,10 +36,11 @@ void ExpmapFunctor::init() {
one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
}
ExpmapFunctor::ExpmapFunctor(const Vector3& omega) : theta2(omega.dot(omega)) {
ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
: theta2(omega.dot(omega)) {
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();
init(nearZeroApprox);
if (!nearZero) {
theta = std::sqrt(theta2);
K = W / theta;
@ -47,12 +48,12 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega) : theta2(omega.dot(omega)) {
}
}
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle)
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox)
: theta2(angle * 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;
init();
init(nearZeroApprox);
if (!nearZero) {
theta = angle;
KK = K * K;
@ -66,8 +67,8 @@ SO3 ExpmapFunctor::expmap() const {
return I_3x3 + sin_theta * K + one_minus_cos * KK;
}
DexpFunctor::DexpFunctor(const Vector3& omega)
: ExpmapFunctor(omega), omega(omega) {
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
if (nearZero)
dexp_ = I_3x3 - 0.5 * W;
else {
@ -79,19 +80,18 @@ DexpFunctor::DexpFunctor(const Vector3& omega)
Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
if (nearZero) {
if (H1) *H1 = 0.5 * skewSymmetric(v);
if (H2) *H2 = I_3x3;
return v - 0.5 * omega.cross(v);
}
if (H1) {
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
const Vector3 Kv = K * v;
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() -
skewSymmetric(Kv * b / theta) +
(a * I_3x3 - b * K) * skewSymmetric(v / theta);
if (nearZero) {
*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;
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() -
skewSymmetric(Kv * b / theta) +
(a * I_3x3 - b * K) * skewSymmetric(v / theta);
}
}
if (H2) *H2 = dexp_;
return dexp_ * v;

8
gtsam/geometry/SO3.h
View File

@ -143,14 +143,14 @@ class ExpmapFunctor {
bool nearZero;
double theta, sin_theta, one_minus_cos; // only defined if !nearZero
void init();
void init(bool nearZeroApprox = false);
public:
/// Constructor with element of Lie algebra so(3)
ExpmapFunctor(const Vector3& omega);
ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
/// Constructor with axis-angle
ExpmapFunctor(const Vector3& axis, double angle);
ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
/// Rodrigues formula
SO3 expmap() const;
@ -164,7 +164,7 @@ class DexpFunctor : public ExpmapFunctor {
public:
/// Constructor with element of Lie algebra so(3)
DexpFunctor(const Vector3& omega);
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,

66
gtsam/geometry/tests/testSO3.cpp
View File

@ -203,49 +203,49 @@ TEST(SO3, JacobianLogmap) {
EXPECT(assert_equal(Jexpected, Jactual));
}
/* ************************************************************************* */
Vector3 apply(const Vector3& omega, const Vector3& v) {
so3::DexpFunctor local(omega);
return local.applyDexp(v);
}
/* ************************************************************************* */
TEST(SO3, ApplyDexp) {
Matrix aH1, 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));
for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
};
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, nearZeroApprox);
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(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(local.dexp(), aH2));
}
}
}
}
/* ************************************************************************* */
Vector3 applyInv(const Vector3& omega, const Vector3& v) {
so3::DexpFunctor local(omega);
return local.applyInvDexp(v);
}
/* ************************************************************************* */
TEST(SO3, ApplyInvDexp) {
Matrix aH1, 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));
for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
};
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, nearZeroApprox);
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(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(invDexp, aH2));
}
}
}
}