diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 722087934..fe527ab2e 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -149,16 +149,17 @@ TEST(Pose3, Adjoint_full) // Check Adjoint numerical derivatives TEST(Pose3, Adjoint_jacobians) { + Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + + // Check evaluation sanity check + EQUALITY(static_cast(T.AdjointMap() * xi), T.Adjoint(xi)); + EQUALITY(static_cast(T2.AdjointMap() * xi), T2.Adjoint(xi)); + EQUALITY(static_cast(T3.AdjointMap() * xi), T3.Adjoint(xi)); + + // Check jacobians Matrix6 actualH1, actualH2, expectedH1, expectedH2; std::function Adjoint_proxy = - [&](const Pose3& T, const Vector6& xi) { - Vector6 res1 = T.AdjointMap() * xi, res2 = T.Adjoint(xi); - EXPECT(assert_equal(res1, res2)); - return res1; - }; - - gtsam::Vector6 xi = - (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + [&](const Pose3& T, const Vector6& xi) { return T.AdjointMap() * xi; }; T.Adjoint(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(Adjoint_proxy, T, xi); @@ -183,18 +184,23 @@ TEST(Pose3, Adjoint_jacobians) // Check AdjointTranspose and jacobians TEST(Pose3, AdjointTranspose) { + Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + + // Check evaluation + EQUALITY(static_cast(T.AdjointMap().transpose() * xi), + T.AdjointTranspose(xi)); + EQUALITY(static_cast(T2.AdjointMap().transpose() * xi), + T2.AdjointTranspose(xi)); + EQUALITY(static_cast(T3.AdjointMap().transpose() * xi), + T3.AdjointTranspose(xi)); + + // Check jacobians Matrix6 actualH1, actualH2, expectedH1, expectedH2; std::function AdjointTranspose_proxy = [&](const Pose3& T, const Vector6& xi) { - Vector6 res1 = T.AdjointMap().transpose() * xi, - res2 = T.AdjointTranspose(xi); - EXPECT(assert_equal(res1, res2)); - return res1; + return T.AdjointMap().transpose() * xi; }; - gtsam::Vector6 xi = - (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); - T.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi);