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

Merge pull request #507 from JzHuai0108/develop 

correct coefficients of approximated SE3 Q_r
release/4.3a0
Frank Dellaert 2020-09-26 11:56:16 -04:00 committed by GitHub
parent b5014b1403 2550bf76a4
commit 79d6d5fa2f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 29 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/geometry/Pose3.cpp
View File

@ -190,15 +190,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
}
/* ************************************************************************* */
/**
* Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix
* J(xi) = [J_(w) Z_3x3;
* Q J_(w)]
* where J_(w) is the SO3 Expmap derivative.
* (see Chirikjian11book2, pg 44, eq 10.95.
* The closed-form formula is similar to formula 102 in Barfoot14tro)
*/
static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) {
const auto w = xi.head<3>();
const auto v = xi.tail<3>();
const Matrix3 V = skewSymmetric(v);
@ -220,7 +212,7 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
#else
// The closed-form formula in Barfoot14tro eq. (102)
double phi = w.norm();
if (std::abs(phi)>1e-5) {
if (std::abs(phi)>nearZeroThreshold) {
const double sinPhi = sin(phi), cosPhi = cos(phi);
const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
// Invert the sign of odd-order terms to have the right Jacobian
@ -230,8 +222,8 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
}
else {
Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W)
+ 1./24.*(W*W*V + V*W*W - 3*W*V*W)
- 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W);
- 1./24.*(W*W*V + V*W*W - 3*W*V*W)
+ 1./120.*(W*V*W*W + W*W*V*W);
}
#endif
@ -242,7 +234,7 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
const Vector3 w = xi.head<3>();
const Matrix3 Jw = Rot3::ExpmapDerivative(w);
const Matrix3 Q = computeQforExpmapDerivative(xi);
const Matrix3 Q = ComputeQforExpmapDerivative(xi);
Matrix6 J;
J << Jw, Z_3x3, Q, Jw;
return J;
@ -253,7 +245,7 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
const Vector6 xi = Logmap(pose);
const Vector3 w = xi.head<3>();
const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Q = computeQforExpmapDerivative(xi);
const Matrix3 Q = ComputeQforExpmapDerivative(xi);
const Matrix3 Q2 = -Jw*Q*Jw;
Matrix6 J;
J << Jw, Z_3x3, Q2, Jw;

12
gtsam/geometry/Pose3.h
View File

@ -181,6 +181,18 @@ public:
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
};
/**
* Compute the 3x3 bottom-left block Q of SE3 Expmap right derivative matrix
* J_r(xi) = [J_(w) Z_3x3;
* Q_r J_(w)]
* where J_(w) is the SO3 Expmap right derivative.
* (see Chirikjian11book2, pg 44, eq 10.95.
* The closed-form formula is identical to formula 102 in Barfoot14tro where
* Q_l of the SE3 Expmap left derivative matrix is given.
*/
static Matrix3 ComputeQforExpmapDerivative(
const Vector6& xi, double nearZeroThreshold = 1e-5);
using LieGroup<Pose3, 6>::inverse; // version with derivative
/**

11
gtsam/geometry/tests/testPose3.cpp
View File

@ -807,6 +807,17 @@ TEST(Pose3, ExpmapDerivative2) {
}
}
TEST( Pose3, ExpmapDerivativeQr) {
Vector6 w = Vector6::Random();
w.head<3>().normalize();
w.head<3>() = w.head<3>() * 0.9e-2;
Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none);
Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
}
/* ************************************************************************* */
TEST( Pose3, LogmapDerivative) {
Matrix6 actualH;