From 82bc6d8931e60d56b251565681a594bacdee2879 Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Thu, 27 Feb 2025 16:11:21 +1100 Subject: [PATCH] adds jacobians to Pose3::InterpolateRt --- gtsam/geometry/Pose3.cpp | 22 ++++++- gtsam/geometry/Pose3.h | 5 +- gtsam/geometry/tests/testPose3.cpp | 97 +++++++++++++++++++++++++++--- 3 files changed, 114 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f6d0c43cb..337037676 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -171,9 +171,29 @@ bool Pose3::equals(const Pose3& pose, double tol) const { } /* ************************************************************************* */ -Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { +Pose3 Pose3::interpolateRt(const Pose3& T, double t, + OptionalJacobian<6, 6> Hself, + OptionalJacobian<6, 6> Harg, + OptionalJacobian<6, 1> Ht) const { + if(Hself || Harg || Ht){ + typename MakeJacobian::type HselfRot, HargRot; + typename MakeJacobian::type HtRot; + typename MakeJacobian::type HselfPoint, HargPoint; + typename MakeJacobian::type HtPoint; + + Rot3 Rint = interpolate(R_, T.R_, t, HselfRot, HargRot, HtRot); + Point3 Pint = interpolate(t_, T.t_, t, HselfPoint, HargPoint, HtPoint); + Pose3 result = Pose3(Rint, Pint); + + if(Hself) *Hself << HselfRot, Z_3x3, Z_3x3, Rint.transpose() * R_.matrix() * HselfPoint; + if(Harg) *Harg << HargRot, Z_3x3, Z_3x3, Rint.transpose() * T.R_.matrix() * HargPoint; + if(Ht) *Ht << HtRot, Rint.transpose() * HtPoint; + + return result; + } return Pose3(interpolate(R_, T.R_, t), interpolate(t_, T.t_, t)); + } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index af14ac82c..dd2030567 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -132,7 +132,10 @@ public: * @param T End point of interpolation. * @param t A value in [0, 1]. */ - Pose3 interpolateRt(const Pose3& T, double t) const; + Pose3 interpolateRt(const Pose3& T, double t, + OptionalJacobian<6, 6> Hself = {}, + OptionalJacobian<6, 6> Harg = {}, + OptionalJacobian<6, 1> Ht = {}) const; /// @} /// @name Lie Group diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index b04fb1982..f5ebf29af 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1167,56 +1167,137 @@ TEST(Pose3, interpolateJacobians) { Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0)); double t = 0.5; Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation - Matrix actualJacobianX, actualJacobianY; - EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + Matrix actualJacobianX, actualJacobianY, actualJacobianT; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT), 1e-5)); Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6)); } { Pose3 X = Pose3::Identity(); Pose3 Y(Rot3::Identity(), Point3(1, 0, 0)); double t = 0.3; Pose3 expectedPoseInterp(Rot3::Identity(), Point3(0.3, 0, 0)); - Matrix actualJacobianX, actualJacobianY; - EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + Matrix actualJacobianX, actualJacobianY, actualJacobianT; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT), 1e-5)); Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6)); } { Pose3 X = Pose3::Identity(); Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0)); double t = 0.5; Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0)); - Matrix actualJacobianX, actualJacobianY; - EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + Matrix actualJacobianX, actualJacobianY, actualJacobianT; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT), 1e-5)); Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6)); } { Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2)); Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1)); double t = 0.3; Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0)); - Matrix actualJacobianX, actualJacobianY; - interpolate(X, Y, t, actualJacobianX, actualJacobianY); + Matrix actualJacobianX, actualJacobianY, actualJacobianT; + interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT); Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6)); + } +} +/* ************************************************************************* */ +Pose3 testing_interpolate_rt(const Pose3& t1, const Pose3& t2, double gamma) { return t1.interpolateRt(t2, gamma); } + +TEST(Pose3, interpolateRtJacobians) { + { + Pose3 X = Pose3::Identity(); + Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0)); + double t = 0.5; + Matrix actualJacobianX, actualJacobianY, actualJacobianT; + X.interpolateRt(Y, t, actualJacobianX, actualJacobianY, actualJacobianT); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6)); + } + { + Pose3 X = Pose3::Identity(); + Pose3 Y(Rot3::Identity(), Point3(1, 0, 0)); + double t = 0.3; + Matrix actualJacobianX, actualJacobianY, actualJacobianT; + X.interpolateRt(Y, t, actualJacobianX, actualJacobianY, actualJacobianT); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6)); + } + { + Pose3 X = Pose3::Identity(); + Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0)); + double t = 0.5; + Matrix actualJacobianX, actualJacobianY, actualJacobianT; + X.interpolateRt(Y, t, actualJacobianX, actualJacobianY, actualJacobianT); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6)); + } + { + Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2)); + Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1)); + double t = 0.3; + Matrix actualJacobianX, actualJacobianY, actualJacobianT; + X.interpolateRt(Y, t, actualJacobianX, actualJacobianY, actualJacobianT); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6)); } }