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

review comment: move check outside lambda

release/4.3a0
Gerry Chen 2021-10-04 11:19:19 -04:00
parent ab1b926dcd
commit f7fed8b5f5
1 changed files with 21 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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<gtsam::Vector>(T.AdjointMap() * xi), T.Adjoint(xi));
EQUALITY(static_cast<gtsam::Vector>(T2.AdjointMap() * xi), T2.Adjoint(xi));
EQUALITY(static_cast<gtsam::Vector>(T3.AdjointMap() * xi), T3.Adjoint(xi));
// Check jacobians
Matrix6 actualH1, actualH2, expectedH1, expectedH2;
std::function<Vector6(const Pose3&, const Vector6&)> 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<Vector>(T.AdjointMap().transpose() * xi),
T.AdjointTranspose(xi));
EQUALITY(static_cast<Vector>(T2.AdjointMap().transpose() * xi),
T2.AdjointTranspose(xi));
EQUALITY(static_cast<Vector>(T3.AdjointMap().transpose() * xi),
T3.AdjointTranspose(xi));
// Check jacobians
Matrix6 actualH1, actualH2, expectedH1, expectedH2;
std::function<Vector6(const Pose3&, const Vector6&)> 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);