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

New constants for cross and doubleCross

release/4.3a0
Frank Dellaert 2024-12-15 17:54:30 -05:00
parent fa1249bf14
commit e96d8487e4
3 changed files with 108 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
gtsam/geometry/SO3.cpp
View File

@ -51,49 +51,51 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
}
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)]
const double sin_theta = std::sin(theta);
A = sin_theta / theta;
const double s2 = std::sin(theta / 2.0);
const double one_minus_cos =
2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
B = one_minus_cos / theta2;
} else {
// Limits as theta -> 0:
A = 1.0;
B = 0.5;
}
}
ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
: theta2(omega.dot(omega)), theta(std::sqrt(theta2)) {
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
: theta2(omega.dot(omega)),
theta(std::sqrt(theta2)),
W(skewSymmetric(omega)),
WW(W * W) {
init(nearZeroApprox);
}
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();
W << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
W *= angle;
: theta2(angle * angle),
theta(angle),
W(skewSymmetric(axis * angle)),
WW(W * W) {
init(nearZeroApprox);
}
SO3 ExpmapFunctor::expmap() const {
if (nearZero)
return SO3(I_3x3 + W + 0.5 * WW);
else
return SO3(I_3x3 + A * W + B * WW);
}
SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
C = (1 - A) / theta2;
C = nearZero ? one_sixth : (1 - A) / theta2;
D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2;
E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2;
}
Matrix3 DexpFunctor::rightJacobian() const {
if (nearZero) {
return I_3x3 - 0.5 * W; // + one_sixth * WW;
return I_3x3 - B * W; // + C * WW;
} else {
return I_3x3 - B * W + C * WW;
}
@ -103,10 +105,10 @@ Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const {
// Wv = omega x * v
const Vector3 Wv = gtsam::cross(omega, v);
if (H) {
// 1x3 Jacobian of B with respect to omega
const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose();
// Apply product rule:
*H = Wv * HB - B * skewSymmetric(v);
// D * omega.transpose() is 1x3 Jacobian of B with respect to omega
// - skewSymmetric(v) is 3x3 Jacobian of B gtsam::cross(omega, v)
*H = Wv * D * omega.transpose() - B * skewSymmetric(v);
}
return B * Wv;
}
@ -118,10 +120,10 @@ Vector3 DexpFunctor::doubleCross(const Vector3& v,
const Vector3 WWv =
gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr);
if (H) {
// 1x3 Jacobian of C with respect to omega
const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose();
// Apply product rule:
*H = WWv * HC + C * doubleCrossJacobian;
// E * omega.transpose() is 1x3 Jacobian of C with respect to omega
// doubleCrossJacobian is 3x3 Jacobian of C gtsam::doubleCross(omega, v)
*H = WWv * E * omega.transpose() + C * doubleCrossJacobian;
}
return C * WWv;
}
@ -134,12 +136,12 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
if (H2) *H2 = I_3x3 - 0.5 * W;
return v - 0.5 * gtsam::cross(omega, v);
} else {
Matrix3 D_BWv_omega, D_CWWv_omega;
Matrix3 D_BWv_omega, D_CWWv_omega;
const Vector3 BWv = cross(v, D_BWv_omega);
const Vector3 CWWv = doubleCross(v, D_CWWv_omega);
if (H1) *H1 = - D_BWv_omega + D_CWWv_omega;
if (H2) *H2 = rightJacobian();
return v - BWv + CWWv;
if (H2) *H2 = rightJacobian();
return v - BWv + CWWv;
}
}
@ -219,12 +221,7 @@ SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
template <>
GTSAM_EXPORT
Matrix3 SO3::Hat(const Vector3& xi) {
// skew symmetric matrix X = xi^
Matrix3 Y = Z_3x3;
Y(0, 1) = -xi(2);
Y(0, 2) = +xi(1);
Y(1, 2) = -xi(0);
return Y - Y.transpose();
return skewSymmetric(xi);
}
//******************************************************************************

22
gtsam/geometry/SO3.h
View File

@ -134,11 +134,12 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
/// Functor implementing Exponential map
class GTSAM_EXPORT ExpmapFunctor {
protected:
const double theta2;
Matrix3 W, WW;
double A, B; // Ethan Eade's constants
const double theta2, theta;
const Matrix3 W, WW;
bool nearZero;
double theta, sin_theta, one_minus_cos; // only defined if !nearZero
// Ethan Eade's constants:
double A; // A = sin(theta) / theta or 1 for theta->0
double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0
void init(bool nearZeroApprox = false);
@ -156,17 +157,19 @@ 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 C; // Ethan Eade's C constant
double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0
// Constants used in cross and doubleCross
double D; // (A - 2.0 * B) / theta2 or -1/12 for theta->0
double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0
public:
/// Computes B * (omega x v).
Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
/// Computes C * (omega x (omega x v)).
Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
public:
/// Constructor with element of Lie algebra so(3)
GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega,
bool nearZeroApprox = false);
@ -199,6 +202,11 @@ class DexpFunctor : public ExpmapFunctor {
GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
protected:
static constexpr double one_sixth = 1.0 / 6.0;
static constexpr double _one_twelfth = -1.0 / 12.0;
static constexpr double _one_sixtieth = -1.0 / 60.0;
};
} // namespace so3

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

@ -303,19 +303,63 @@ TEST(SO3, JacobianLogmap) {
EXPECT(assert_equal(Jexpected, Jactual));
}
namespace test_cases {
std::vector<Vector3> nearZeros{
{0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}};
std::vector<Vector3> omegas{
{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}};
std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}};
} // namespace test_cases
//******************************************************************************
TEST(SO3, Cross) {
Matrix aH1;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).cross(v);
};
const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas;
for (Vector3 omega : omegas) {
so3::DexpFunctor local(omega, nearZero);
for (Vector3 v : test_cases::vs) {
local.cross(v, aH1);
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
}
}
}
}
//******************************************************************************
TEST(SO3, DoubleCross) {
Matrix aH1;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).doubleCross(v);
};
const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas;
for (Vector3 omega : omegas) {
so3::DexpFunctor local(omega, nearZero);
for (Vector3 v : test_cases::vs) {
local.doubleCross(v, aH1);
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
}
}
}
}
//******************************************************************************
TEST(SO3, ApplyDexp) {
Matrix aH1, aH2;
for (bool nearZeroApprox : {true, false}) {
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
return so3::DexpFunctor(omega, nearZero).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)}) {
for (Vector3 omega : test_cases::omegas) {
so3::DexpFunctor local(omega, nearZero);
for (Vector3 v : test_cases::vs) {
EXPECT(assert_equal(Vector3(local.dexp() * v),
local.applyDexp(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
@ -329,18 +373,16 @@ TEST(SO3, ApplyDexp) {
//******************************************************************************
TEST(SO3, ApplyLeftJacobian) {
Matrix aH1, aH2;
for (bool nearZeroApprox : {false, true}) {
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyLeftJacobian(v);
return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(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)}) {
for (Vector3 omega : test_cases::omegas) {
so3::DexpFunctor local(omega, nearZero);
for (Vector3 v : test_cases::vs) {
CHECK(assert_equal(Vector3(local.leftJacobian() * v),
local.applyLeftJacobian(v, aH1, aH2)));
local.applyLeftJacobian(v, aH1, aH2)));
CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1));
CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2));
CHECK(assert_equal(local.leftJacobian(), aH2));
@ -352,17 +394,15 @@ TEST(SO3, ApplyLeftJacobian) {
//******************************************************************************
TEST(SO3, ApplyInvDexp) {
Matrix aH1, aH2;
for (bool nearZeroApprox : {true, false}) {
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
return so3::DexpFunctor(omega, nearZero).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);
for (Vector3 omega : test_cases::omegas) {
so3::DexpFunctor local(omega, nearZero);
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)}) {
for (Vector3 v : test_cases::vs) {
EXPECT(assert_equal(Vector3(invDexp * v),
local.applyInvDexp(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));