From 19d1ac42b95664530b276744f2ad19da34207793 Mon Sep 17 00:00:00 2001 From: JIanzhu Huai Date: Thu, 3 Sep 2020 15:21:15 +0800 Subject: [PATCH 1/4] correct coefficients of approximated SE3 Q_r --- gtsam/geometry/Pose3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0a56e2ef5..ecc0598eb 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -230,8 +230,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 From 7d91540865bb9a7db74d5800bf81c3204105b178 Mon Sep 17 00:00:00 2001 From: JIanzhu Huai Date: Fri, 4 Sep 2020 17:19:58 +0800 Subject: [PATCH 2/4] test computeQforExpmapDerivative --- gtsam/geometry/Pose3.cpp | 4 ++-- gtsam/geometry/Pose3.h | 3 +++ gtsam/geometry/tests/testPose3.cpp | 9 +++++++++ 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0a56e2ef5..e505e8dff 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -198,7 +198,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { * (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 +220,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 diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 159fd2927..575fbaa5c 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -181,6 +181,9 @@ public: static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none); }; + static Matrix3 computeQforExpmapDerivative( + const Vector6& xi, double nearZeroThreshold = 1e-5); + using LieGroup::inverse; // version with derivative /** diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 8586f35a0..55720abca 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -818,6 +818,15 @@ 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; From 52fc9cf4ba86582d96d4aec3b7a17fece655bd9f Mon Sep 17 00:00:00 2001 From: JIanzhu Huai Date: Fri, 25 Sep 2020 19:47:07 +0800 Subject: [PATCH 3/4] test Qr with old codebase fails --- gtsam/geometry/Pose3.cpp | 18 +++++------------- gtsam/geometry/Pose3.h | 11 ++++++++++- gtsam/geometry/tests/testPose3.cpp | 20 +++++++++++--------- 3 files changed, 26 insertions(+), 23 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2e6a2136e..12f628731 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -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; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 575fbaa5c..2f0802cab 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -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::inverse; // version with derivative diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 55720abca..9639ffbcf 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -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::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; From 2550bf76a493ac5148297c09f5adc45f5cf872b9 Mon Sep 17 00:00:00 2001 From: JIanzhu Huai Date: Fri, 25 Sep 2020 20:26:24 +0800 Subject: [PATCH 4/4] correct Qr coefficients in approximation --- gtsam/geometry/Pose3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 12f628731..ea822b796 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -222,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) - - 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