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

Simplified applyDexp

release/4.3a0
Frank Dellaert 2024-12-14 23:53:51 -05:00
parent 8040a0a31e
commit 78f17aabc4
2 changed files with 66 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
gtsam/geometry/SO3.cpp
View File

@ -18,13 +18,14 @@
* @date December 2014
*/
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/concepts.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/SO3.h>
#include <Eigen/SVD>
#include <cmath>
#include <iostream>
#include <limits>
namespace gtsam {
@ -41,7 +42,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
return H;
}
GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
OptionalJacobian<9, 9> H) {
Matrix3 MR = M * R.matrix();
if (H) *H = Dcompose(R);
return MR;
@ -83,45 +85,54 @@ SO3 ExpmapFunctor::expmap() const {
return SO3(I_3x3 + A * W + B * WW);
}
Matrix3 DexpFunctor::leftJacobian() const {
if (nearZero) {
return I_3x3 + 0.5 * W; // + one_sixth * WW;
} else {
return I_3x3 + B * W + C * WW;
}
}
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
C = (1 - A) / theta2;
}
Matrix3 DexpFunctor::rightJacobian() const {
if (nearZero) {
rightJacobian_ = I_3x3 - 0.5 * W; // + one_sixth * WW;
return I_3x3 - 0.5 * W; // + one_sixth * WW;
} else {
C = (1 - A) / theta2;
rightJacobian_ = I_3x3 - B * W + C * WW;
return I_3x3 - B * W + C * WW;
}
}
// Derivative of w x w x v in w:
static Matrix3 doubleCrossJacobian(const Vector3& w, const Vector3& v) {
return v.dot(w) * I_3x3 + w * v.transpose() - 2 * v * w.transpose();
}
// Multiplies v with left Jacobian through vector operations only.
// When Jacobian H1 wrt omega is requested, product rule is invoked twice, once
// for (B * Wv) and (C * WWv). The Jacobian H2 wrt v is just the right Jacobian.
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 * V;
} else {
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
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;
// Wv = omega x * v, with Jacobian -V in w
const Vector3 Wv = gtsam::cross(omega, v);
if (nearZero) {
if (H1) *H1 = 0.5 * skewSymmetric(v);
if (H2) *H2 = I_3x3 - 0.5 * W;
return v - 0.5 * Wv;
} else {
// WWv = omega x (omega x * v), with Jacobian doubleCrossJacobian
const Vector3 WWv = gtsam::cross(omega, Wv);
if (H1) {
// 1x3 Jacobians of B and C with respect to theta
const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose();
const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose();
*H1 = -Wv * HB + B * skewSymmetric(v) +
C * doubleCrossJacobian(omega, v) + WWv * HC;
}
if (H2) *H2 = rightJacobian();
return v - B * Wv + C * WWv;
}
if (H2) *H2 = rightJacobian_;
return rightJacobian_ * v;
}
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 invDexp = rightJacobian_.inverse();
const Matrix3 invDexp = rightJacobian().inverse();
const Vector3 c = invDexp * v;
if (H1) {
Matrix3 D_dexpv_omega;
@ -132,6 +143,23 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
return c;
}
Matrix3 DexpFunctor::leftJacobian() const {
if (nearZero) {
return I_3x3 + 0.5 * W; // + one_sixth * WW;
} else {
return I_3x3 + B * W + C * WW;
}
}
Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 Jw = leftJacobian();
if (H1) H1->setZero();
if (H2) *H2 = Jw;
return Jw * v;
}
} // namespace so3
//******************************************************************************

17
gtsam/geometry/SO3.h
View File

@ -26,7 +26,6 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/dllexport.h>
#include <cmath>
#include <vector>
namespace gtsam {
@ -160,7 +159,6 @@ class DexpFunctor : public ExpmapFunctor {
static constexpr double one_sixth = 1.0 / 6.0;
const Vector3 omega;
double C; // Ethan Eade's C constant
Matrix3 rightJacobian_;
public:
/// Constructor with element of Lie algebra so(3)
@ -172,8 +170,11 @@ class DexpFunctor : public ExpmapFunctor {
// Information Theory, and Lie Groups", Volume 2, 2008.
// 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) */
const Matrix3& dexp() const { return rightJacobian_; }
// a perturbation on the manifold Expmap(dexp * v)
GTSAM_EXPORT Matrix3 rightJacobian() const;
/// differential of expmap == right Jacobian
GTSAM_EXPORT Matrix3 dexp() const { return rightJacobian(); }
/// Multiplies with dexp(), with optional derivatives
GTSAM_EXPORT Vector3 applyDexp(const Vector3& v,
@ -186,8 +187,12 @@ class DexpFunctor : public ExpmapFunctor {
OptionalJacobian<3, 3> H2 = {}) const;
// Compute the left Jacobian for Exponential map in SO(3)
// Note precomputed, as not used as much
Matrix3 leftJacobian() const;
GTSAM_EXPORT Matrix3 leftJacobian() const;
/// Multiplies with leftJacobian(), with optional derivatives
GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
};
} // namespace so3