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

test Qr with old codebase fails

release/4.3a0
JIanzhu Huai 2020-09-25 19:47:07 +08:00
parent c4ebab5e44
commit 52fc9cf4ba
3 changed files with 26 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
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)
*/
Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) {
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);
@ -230,8 +222,8 @@ Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi, double nearZeroThr
}
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)
+ 1./120.*(W*V*W*W + 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);
}
#endif
@ -242,7 +234,7 @@ Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi, double nearZeroThr
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;

11
gtsam/geometry/Pose3.h
View File

@ -181,7 +181,16 @@ public:
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
};
static Matrix3 computeQforExpmapDerivative(
/**
* 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

20
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;
@ -818,15 +829,6 @@ TEST( Pose3, LogmapDerivative) {
EXPECT(assert_equal(expectedH, actualH));
}
TEST( Pose3, computeQforExpmapDerivative) {
Vector6 w = Vector6::Random();
w.head<3>().normalize();
w.head<3>() = w.head<3>() * 0.09;
Matrix3 Qexact = Pose3::computeQforExpmapDerivative(w);
Matrix3 Qapprox = Pose3::computeQforExpmapDerivative(w, 0.1);
EXPECT(assert_equal(Qapprox, Qexact, 1e-5));
}
/* ************************************************************************* */
Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
return Pose3::adjointMap(xi) * v;