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

test computeQforExpmapDerivative

release/4.3a0
JIanzhu Huai 2020-09-04 17:19:58 +08:00
parent 915702116f
commit 7d91540865
3 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Pose3.cpp
View File

@ -198,7 +198,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
* (see Chirikjian11book2, pg 44, eq 10.95. * (see Chirikjian11book2, pg 44, eq 10.95.
* The closed-form formula is similar to formula 102 in Barfoot14tro) * 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 w = xi.head<3>();
const auto v = xi.tail<3>(); const auto v = xi.tail<3>();
const Matrix3 V = skewSymmetric(v); const Matrix3 V = skewSymmetric(v);
@ -220,7 +220,7 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
#else #else
// The closed-form formula in Barfoot14tro eq. (102) // The closed-form formula in Barfoot14tro eq. (102)
double phi = w.norm(); 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 sinPhi = sin(phi), cosPhi = cos(phi);
const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * 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 // Invert the sign of odd-order terms to have the right Jacobian

3
gtsam/geometry/Pose3.h
View File

@ -181,6 +181,9 @@ public:
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none); static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
}; };
static Matrix3 computeQforExpmapDerivative(
const Vector6& xi, double nearZeroThreshold = 1e-5);
using LieGroup<Pose3, 6>::inverse; // version with derivative using LieGroup<Pose3, 6>::inverse; // version with derivative
/** /**

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

@ -818,6 +818,15 @@ TEST( Pose3, LogmapDerivative) {
EXPECT(assert_equal(expectedH, actualH)); 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) { Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
return Pose3::adjointMap(xi) * v; return Pose3::adjointMap(xi) * v;