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

Fix test cases

release/4.3a0
Frank Dellaert 2024-12-15 23:42:59 -05:00
parent a32bb7bf09
commit bcfb7d8444
1 changed files with 24 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
gtsam/geometry/tests/testSO3.cpp
View File

@ -304,26 +304,25 @@ TEST(SO3, JacobianLogmap) {
} }
namespace test_cases { namespace test_cases {
std::vector<Vector3> nearZeros{ std::vector<Vector3> small{{0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}};
{0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}}; std::vector<Vector3> large{
std::vector<Vector3> omegas{
{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}}; {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}};
auto omegas = [](bool nearZero) { return nearZero ? small : large; };
std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}}; std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}};
} // namespace test_cases } // namespace test_cases
//****************************************************************************** //******************************************************************************
TEST(SO3, Cross) { TEST(SO3, CrossB) {
Matrix aH1; Matrix aH1;
for (bool nearZero : {true, false}) { for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f = std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) { [=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).cross(v); return so3::DexpFunctor(omega, nearZero).crossB(v);
}; };
const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas; for (const Vector3& omega : test_cases::omegas(nearZero)) {
for (Vector3 omega : omegas) {
so3::DexpFunctor local(omega, nearZero); so3::DexpFunctor local(omega, nearZero);
for (Vector3 v : test_cases::vs) { for (const Vector3& v : test_cases::vs) {
local.cross(v, aH1); local.crossB(v, aH1);
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
} }
} }
@ -331,18 +330,17 @@ TEST(SO3, Cross) {
} }
//****************************************************************************** //******************************************************************************
TEST(SO3, DoubleCross) { TEST(SO3, DoubleCrossC) {
Matrix aH1; Matrix aH1;
for (bool nearZero : {true, false}) { for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f = std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) { [=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).doubleCross(v); return so3::DexpFunctor(omega, nearZero).doubleCrossC(v);
}; };
const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas; for (const Vector3& omega : test_cases::omegas(nearZero)) {
for (Vector3 omega : omegas) {
so3::DexpFunctor local(omega, nearZero); so3::DexpFunctor local(omega, nearZero);
for (Vector3 v : test_cases::vs) { for (const Vector3& v : test_cases::vs) {
local.doubleCross(v, aH1); local.doubleCrossC(v, aH1);
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
} }
} }
@ -357,9 +355,9 @@ TEST(SO3, ApplyDexp) {
[=](const Vector3& omega, const Vector3& v) { [=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyDexp(v); return so3::DexpFunctor(omega, nearZero).applyDexp(v);
}; };
for (Vector3 omega : test_cases::omegas) { for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero); so3::DexpFunctor local(omega, nearZero);
for (Vector3 v : test_cases::vs) { for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(local.dexp() * v), EXPECT(assert_equal(Vector3(local.dexp() * v),
local.applyDexp(v, aH1, aH2))); local.applyDexp(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
@ -378,14 +376,14 @@ TEST(SO3, ApplyLeftJacobian) {
[=](const Vector3& omega, const Vector3& v) { [=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v); return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v);
}; };
for (Vector3 omega : test_cases::omegas) { for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero); so3::DexpFunctor local(omega, nearZero);
for (Vector3 v : test_cases::vs) { for (const Vector3& v : test_cases::vs) {
CHECK(assert_equal(Vector3(local.leftJacobian() * v), EXPECT(assert_equal(Vector3(local.leftJacobian() * v),
local.applyLeftJacobian(v, aH1, aH2))); local.applyLeftJacobian(v, aH1, aH2)));
CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2)); EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
CHECK(assert_equal(local.leftJacobian(), aH2)); EXPECT(assert_equal(local.leftJacobian(), aH2));
} }
} }
} }
@ -399,10 +397,10 @@ TEST(SO3, ApplyInvDexp) {
[=](const Vector3& omega, const Vector3& v) { [=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
}; };
for (Vector3 omega : test_cases::omegas) { for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero); so3::DexpFunctor local(omega, nearZero);
Matrix invDexp = local.dexp().inverse(); Matrix invDexp = local.dexp().inverse();
for (Vector3 v : test_cases::vs) { for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(invDexp * v), EXPECT(assert_equal(Vector3(invDexp * v),
local.applyInvDexp(v, aH1, aH2))); local.applyInvDexp(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));