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

Simplified Jacobians with ABC

release/4.3a0
Frank Dellaert 2024-12-14 15:48:55 -05:00
parent 7f1e101c6b
commit d1634c0335
2 changed files with 27 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
gtsam/geometry/Pose3.cpp
View File

@ -246,33 +246,34 @@ class ExpmapFunctor : public so3::DexpFunctor {
static constexpr double one_one_hundred_twentieth = 1.0 / 120.0;
public:
ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
: so3::DexpFunctor(omega, nearZeroApprox) {}
ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false,
bool includeHigherOrder = false)
: so3::DexpFunctor(omega, nearZeroApprox),
includeHigherOrder(includeHigherOrder) {}
// Compute the Jacobian Q with respect to w
Matrix3 computeQ(const Vector3& v) const {
const Matrix3 V = skewSymmetric(v);
const Matrix3 WVW = W * V * W;
if (!nearZero) {
const double theta3 = theta2 * theta;
const double theta4 = theta2 * theta2;
const double theta5 = theta4 * theta;
const double s = sin_theta;
const double a = one_minus_cos - theta2 / 2;
// The closed-form formula in Barfoot14tro eq. (102)
return -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) +
a / theta4 * (WW * V + V * WW - 3 * WVW) -
0.5 *
(a / theta4 - 3 * (theta - s - theta3 * one_sixth) / theta5) *
(WVW * W + W * WVW);
// Simplified from closed-form formula in Barfoot14tro eq. (102)
// Note dexp = I_3x3 - B * W + C * WW and t = dexp * v
return -0.5 * V + C * (W * V + V * W - WVW) +
(B - 0.5) / theta2 * (WW * V + V * WW - 3 * WVW) -
0.5 * (B - 3 * C) / theta2 * (WVW * W + W * WVW);
} else {
return -0.5 * V + one_sixth * (W * V + V * W - WVW) -
one_twenty_fourth * (WW * V + V * WW - 3 * WVW) +
one_one_hundred_twentieth * (WVW * W + W * WVW);
Matrix3 Q = -0.5 * V + one_sixth * (W * V + V * W - WVW);
Q -= one_twenty_fourth * (WW * V + V * WW - 3 * WVW);
if (includeHigherOrder) {
Q += one_one_hundred_twentieth * (WVW * W + W * WVW);
}
return Q;
}
}
private:
bool includeHigherOrder;
};
} // namespace pose3

19
gtsam/geometry/SO3.cpp
View File

@ -85,7 +85,7 @@ SO3 ExpmapFunctor::expmap() const {
Matrix3 DexpFunctor::leftJacobian() const {
if (nearZero) {
return I_3x3 + 0.5 * W + one_sixth * WW;
return I_3x3 + 0.5 * W; // + one_sixth * WW;
} else {
return I_3x3 + B * W + C * WW;
}
@ -94,7 +94,7 @@ Matrix3 DexpFunctor::leftJacobian() const {
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
if (nearZero) {
rightJacobian_ = I_3x3 - 0.5 * W + one_sixth * WW;
rightJacobian_ = I_3x3 - 0.5 * W; // + one_sixth * WW;
} else {
C = (1 - A) / theta2;
rightJacobian_ = I_3x3 - B * W + C * WW;
@ -104,18 +104,15 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
if (H1) {
const Matrix3 V = skewSymmetric(v);
if (nearZero) {
*H1 = 0.5 * skewSymmetric(v);
*H1 = 0.5 * V;
} else {
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
double a = B * theta;
double b = C * theta2;
const Vector3 Kv = W * v / theta;
const double Da = (sin_theta - 2.0 * a) / theta2;
const double Db = (one_minus_cos - 3.0 * b) / theta2;
*H1 = (Db * W / theta - Da * I_3x3) * Kv * omega.transpose() -
skewSymmetric(Kv * b / theta) +
(a * I_3x3 - b * W / theta) * skewSymmetric(v / theta);
const double Da = (A - 2.0 * B) / theta2;
const double Db = (B - 3.0 * C) / theta2;
*H1 = (Db * WW - Da * W) * v * omega.transpose() -
C * skewSymmetric(W * v) + B * V - C * W * V;
}
}
if (H2) *H2 = rightJacobian_;