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

Fix check.geometry

release/4.3a0
Frank Dellaert 2022-02-16 08:25:10 -05:00
parent 315df6c315
commit 3a49c79ee8
8 changed files with 145 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/serializationTestHelpers.h
View File

@ -42,7 +42,7 @@ T create() {
}
// Creates or empties a folder in the build folder and returns the relative path
boost::filesystem::path resetFilesystem(
inline boost::filesystem::path resetFilesystem(
boost::filesystem::path folder = "actual") {
boost::filesystem::remove_all(folder);
boost::filesystem::create_directory(folder);

2
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -160,7 +160,7 @@ TEST(Cal3Bundler, retract) {
}
/* ************************************************************************* */
TEST(Cal3_S2, Print) {
TEST(Cal3Bundler, Print) {
Cal3Bundler cal(1, 2, 3, 4, 5);
std::stringstream os;
os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()

49
gtsam/geometry/tests/testPose2.cpp
View File

@ -796,44 +796,45 @@ TEST(Pose2, align_4) {
}
//******************************************************************************
namespace pose2_example {
Pose2 id;
Pose2 T1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5)));
Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0));
} // namespace pose2_example
//******************************************************************************
TEST(Pose2 , Invariants) {
Pose2 id;
TEST(Pose2, Invariants) {
using namespace pose2_example;
EXPECT(check_group_invariants(id,id));
EXPECT(check_group_invariants(id,T1));
EXPECT(check_group_invariants(T2,id));
EXPECT(check_group_invariants(T2,T1));
EXPECT(check_manifold_invariants(id,id));
EXPECT(check_manifold_invariants(id,T1));
EXPECT(check_manifold_invariants(T2,id));
EXPECT(check_manifold_invariants(T2,T1));
EXPECT(check_group_invariants(id, id));
EXPECT(check_group_invariants(id, T1));
EXPECT(check_group_invariants(T2, id));
EXPECT(check_group_invariants(T2, T1));
EXPECT(check_manifold_invariants(id, id));
EXPECT(check_manifold_invariants(id, T1));
EXPECT(check_manifold_invariants(T2, id));
EXPECT(check_manifold_invariants(T2, T1));
}
//******************************************************************************
TEST(Pose2 , LieGroupDerivatives) {
Pose2 id;
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
TEST(Pose2, LieGroupDerivatives) {
using namespace pose2_example;
CHECK_LIE_GROUP_DERIVATIVES(id, id);
CHECK_LIE_GROUP_DERIVATIVES(id, T2);
CHECK_LIE_GROUP_DERIVATIVES(T2, id);
CHECK_LIE_GROUP_DERIVATIVES(T2, T1);
}
//******************************************************************************
TEST(Pose2 , ChartDerivatives) {
Pose2 id;
TEST(Pose2, ChartDerivatives) {
using namespace pose2_example;
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T1);
CHECK_CHART_DERIVATIVES(id, id);
CHECK_CHART_DERIVATIVES(id, T2);
CHECK_CHART_DERIVATIVES(T2, id);
CHECK_CHART_DERIVATIVES(T2, T1);
}
//******************************************************************************

26
gtsam/geometry/tests/testQuaternion.cpp
View File

@ -80,12 +80,6 @@ TEST(Quaternion , Compose) {
EXPECT(traits<Q>::Equals(expected, actual));
}
//******************************************************************************
Vector3 Q_z_axis(0, 0, 1);
Q id(Eigen::AngleAxisd(0, Q_z_axis));
Q R1(Eigen::AngleAxisd(1, Q_z_axis));
Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
//******************************************************************************
TEST(Quaternion , Between) {
Vector3 z_axis(0, 0, 1);
@ -108,7 +102,17 @@ TEST(Quaternion , Inverse) {
}
//******************************************************************************
TEST(Quaternion , Invariants) {
namespace q_example {
Vector3 Q_z_axis(0, 0, 1);
Q id(Eigen::AngleAxisd(0, Q_z_axis));
Q R1(Eigen::AngleAxisd(1, Q_z_axis));
Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
} // namespace q_example
//******************************************************************************
TEST(Quaternion, Invariants) {
using namespace q_example;
EXPECT(check_group_invariants(id, id));
EXPECT(check_group_invariants(id, R1));
EXPECT(check_group_invariants(R2, id));
@ -121,7 +125,9 @@ TEST(Quaternion , Invariants) {
}
//******************************************************************************
TEST(Quaternion , LieGroupDerivatives) {
TEST(Quaternion, LieGroupDerivatives) {
using namespace q_example;
CHECK_LIE_GROUP_DERIVATIVES(id, id);
CHECK_LIE_GROUP_DERIVATIVES(id, R2);
CHECK_LIE_GROUP_DERIVATIVES(R2, id);
@ -129,7 +135,9 @@ TEST(Quaternion , LieGroupDerivatives) {
}
//******************************************************************************
TEST(Quaternion , ChartDerivatives) {
TEST(Quaternion, ChartDerivatives) {
using namespace q_example;
CHECK_CHART_DERIVATIVES(id, id);
CHECK_CHART_DERIVATIVES(id, R2);
CHECK_CHART_DERIVATIVES(R2, id);

52
gtsam/geometry/tests/testRot2.cpp
View File

@ -156,44 +156,42 @@ TEST( Rot2, relativeBearing )
}
//******************************************************************************
namespace rot2_example {
Rot2 id;
Rot2 T1(0.1);
Rot2 T2(0.2);
} // namespace rot2_example
//******************************************************************************
TEST(Rot2 , Invariants) {
Rot2 id;
EXPECT(check_group_invariants(id,id));
EXPECT(check_group_invariants(id,T1));
EXPECT(check_group_invariants(T2,id));
EXPECT(check_group_invariants(T2,T1));
EXPECT(check_manifold_invariants(id,id));
EXPECT(check_manifold_invariants(id,T1));
EXPECT(check_manifold_invariants(T2,id));
EXPECT(check_manifold_invariants(T2,T1));
TEST(Rot2, Invariants) {
using namespace rot2_example;
EXPECT(check_group_invariants(id, id));
EXPECT(check_group_invariants(id, T1));
EXPECT(check_group_invariants(T2, id));
EXPECT(check_group_invariants(T2, T1));
EXPECT(check_manifold_invariants(id, id));
EXPECT(check_manifold_invariants(id, T1));
EXPECT(check_manifold_invariants(T2, id));
EXPECT(check_manifold_invariants(T2, T1));
}
//******************************************************************************
TEST(Rot2 , LieGroupDerivatives) {
Rot2 id;
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
TEST(Rot2, LieGroupDerivatives) {
using namespace rot2_example;
CHECK_LIE_GROUP_DERIVATIVES(id, id);
CHECK_LIE_GROUP_DERIVATIVES(id, T2);
CHECK_LIE_GROUP_DERIVATIVES(T2, id);
CHECK_LIE_GROUP_DERIVATIVES(T2, T1);
}
//******************************************************************************
TEST(Rot2 , ChartDerivatives) {
Rot2 id;
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T1);
TEST(Rot2, ChartDerivatives) {
using namespace rot2_example;
CHECK_CHART_DERIVATIVES(id, id);
CHECK_CHART_DERIVATIVES(id, T2);
CHECK_CHART_DERIVATIVES(T2, id);
CHECK_CHART_DERIVATIVES(T2, T1);
}
/* ************************************************************************* */

56
gtsam/geometry/tests/testRot3.cpp
View File

@ -640,46 +640,50 @@ TEST( Rot3, slerp)
}
//******************************************************************************
namespace rot3_example {
Rot3 id;
Rot3 T1(Rot3::AxisAngle(Vector3(0, 0, 1), 1));
Rot3 T2(Rot3::AxisAngle(Vector3(0, 1, 0), 2));
} // namespace rot3_example
//******************************************************************************
TEST(Rot3 , Invariants) {
Rot3 id;
TEST(Rot3, Invariants) {
using namespace rot3_example;
EXPECT(check_group_invariants(id,id));
EXPECT(check_group_invariants(id,T1));
EXPECT(check_group_invariants(T2,id));
EXPECT(check_group_invariants(T2,T1));
EXPECT(check_group_invariants(T1,T2));
EXPECT(check_group_invariants(id, id));
EXPECT(check_group_invariants(id, T1));
EXPECT(check_group_invariants(T2, id));
EXPECT(check_group_invariants(T2, T1));
EXPECT(check_group_invariants(T1, T2));
EXPECT(check_manifold_invariants(id,id));
EXPECT(check_manifold_invariants(id,T1));
EXPECT(check_manifold_invariants(T2,id));
EXPECT(check_manifold_invariants(T2,T1));
EXPECT(check_manifold_invariants(T1,T2));
EXPECT(check_manifold_invariants(id, id));
EXPECT(check_manifold_invariants(id, T1));
EXPECT(check_manifold_invariants(T2, id));
EXPECT(check_manifold_invariants(T2, T1));
EXPECT(check_manifold_invariants(T1, T2));
}
//******************************************************************************
TEST(Rot3 , LieGroupDerivatives) {
Rot3 id;
TEST(Rot3, LieGroupDerivatives) {
using namespace rot3_example;
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T1,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
CHECK_LIE_GROUP_DERIVATIVES(id, id);
CHECK_LIE_GROUP_DERIVATIVES(id, T2);
CHECK_LIE_GROUP_DERIVATIVES(T2, id);
CHECK_LIE_GROUP_DERIVATIVES(T1, T2);
CHECK_LIE_GROUP_DERIVATIVES(T2, T1);
}
//******************************************************************************
TEST(Rot3 , ChartDerivatives) {
Rot3 id;
TEST(Rot3, ChartDerivatives) {
using namespace rot3_example;
if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T1,T2);
CHECK_CHART_DERIVATIVES(T2,T1);
CHECK_CHART_DERIVATIVES(id, id);
CHECK_CHART_DERIVATIVES(id, T2);
CHECK_CHART_DERIVATIVES(T2, id);
CHECK_CHART_DERIVATIVES(T1, T2);
CHECK_CHART_DERIVATIVES(T2, T1);
}
}

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

@ -67,28 +67,33 @@ TEST(SO3, ClosestTo) {
}
//******************************************************************************
namespace so3_example {
SO3 id;
Vector3 z_axis(0, 0, 1), v2(1, 2, 0), v3(1, 2, 3);
SO3 R1(Eigen::AngleAxisd(0.1, z_axis));
SO3 R2(Eigen::AngleAxisd(0.2, z_axis));
} // namespace so3_example
/* ************************************************************************* */
TEST(SO3, ChordalMean) {
using namespace so3_example;
std::vector<SO3> rotations = {R1, R1.inverse()};
EXPECT(assert_equal(SO3(), SO3::ChordalMean(rotations)));
}
//******************************************************************************
// Check that Hat specialization is equal to dynamic version
TEST(SO3, Hat) {
// Check that Hat specialization is equal to dynamic version
using namespace so3_example;
EXPECT(assert_equal(SO3::Hat(z_axis), SOn::Hat(z_axis)));
EXPECT(assert_equal(SO3::Hat(v2), SOn::Hat(v2)));
EXPECT(assert_equal(SO3::Hat(v3), SOn::Hat(v3)));
}
//******************************************************************************
// Check that Hat specialization is equal to dynamic version
TEST(SO3, Vee) {
// Check that Hat specialization is equal to dynamic version
using namespace so3_example;
auto X1 = SOn::Hat(z_axis), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
EXPECT(assert_equal(SO3::Vee(X1), SOn::Vee(X1)));
EXPECT(assert_equal(SO3::Vee(X2), SOn::Vee(X2)));
@ -97,6 +102,7 @@ TEST(SO3, Vee) {
//******************************************************************************
TEST(SO3, Local) {
using namespace so3_example;
Vector3 expected(0, 0, 0.1);
Vector3 actual = traits<SO3>::Local(R1, R2);
EXPECT(assert_equal((Vector)expected, actual));
@ -104,6 +110,7 @@ TEST(SO3, Local) {
//******************************************************************************
TEST(SO3, Retract) {
using namespace so3_example;
Vector3 v(0, 0, 0.1);
SO3 actual = traits<SO3>::Retract(R1, v);
EXPECT(assert_equal(R2, actual));
@ -111,6 +118,7 @@ TEST(SO3, Retract) {
//******************************************************************************
TEST(SO3, Logmap) {
using namespace so3_example;
Vector3 expected(0, 0, 0.1);
Vector3 actual = SO3::Logmap(R1.between(R2));
EXPECT(assert_equal((Vector)expected, actual));
@ -118,6 +126,7 @@ TEST(SO3, Logmap) {
//******************************************************************************
TEST(SO3, Expmap) {
using namespace so3_example;
Vector3 v(0, 0, 0.1);
SO3 actual = R1 * SO3::Expmap(v);
EXPECT(assert_equal(R2, actual));
@ -125,6 +134,8 @@ TEST(SO3, Expmap) {
//******************************************************************************
TEST(SO3, Invariants) {
using namespace so3_example;
EXPECT(check_group_invariants(id, id));
EXPECT(check_group_invariants(id, R1));
EXPECT(check_group_invariants(R2, id));
@ -138,6 +149,7 @@ TEST(SO3, Invariants) {
//******************************************************************************
TEST(SO3, LieGroupDerivatives) {
using namespace so3_example;
CHECK_LIE_GROUP_DERIVATIVES(id, id);
CHECK_LIE_GROUP_DERIVATIVES(id, R2);
CHECK_LIE_GROUP_DERIVATIVES(R2, id);
@ -146,6 +158,7 @@ TEST(SO3, LieGroupDerivatives) {
//******************************************************************************
TEST(SO3, ChartDerivatives) {
using namespace so3_example;
CHECK_CHART_DERIVATIVES(id, id);
CHECK_CHART_DERIVATIVES(id, R2);
CHECK_CHART_DERIVATIVES(R2, id);
@ -350,6 +363,7 @@ TEST(SO3, ApplyInvDexp) {
//******************************************************************************
TEST(SO3, vec) {
using namespace so3_example;
const Vector9 expected = Eigen::Map<const Vector9>(R2.matrix().data());
Matrix actualH;
const Vector9 actual = R2.vec(actualH);

42
gtsam/geometry/tests/testSO4.cpp
View File

@ -48,6 +48,14 @@ TEST(SO4, Concept) {
}
//******************************************************************************
TEST(SO4, Random) {
std::mt19937 rng(42);
auto Q = SO4::Random(rng);
EXPECT_LONGS_EQUAL(4, Q.matrix().rows());
}
//******************************************************************************
namespace so4_example {
SO4 id;
Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
SO4 Q1 = SO4::Expmap(v1);
@ -55,15 +63,12 @@ Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished();
SO4 Q2 = SO4::Expmap(v2);
Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished();
SO4 Q3 = SO4::Expmap(v3);
} // namespace so4_example
//******************************************************************************
TEST(SO4, Random) {
std::mt19937 rng(42);
auto Q = SO4::Random(rng);
EXPECT_LONGS_EQUAL(4, Q.matrix().rows());
}
//******************************************************************************
TEST(SO4, Expmap) {
using namespace so4_example;
// If we do exponential map in SO(3) subgroup, topleft should be equal to R1.
auto R1 = SO3::Expmap(v1.tail<3>()).matrix();
EXPECT((Q1.matrix().topLeftCorner<3, 3>().isApprox(R1)));
@ -84,16 +89,18 @@ TEST(SO4, Expmap) {
}
//******************************************************************************
// Check that Hat specialization is equal to dynamic version
TEST(SO4, Hat) {
// Check that Hat specialization is equal to dynamic version
using namespace so4_example;
EXPECT(assert_equal(SO4::Hat(v1), SOn::Hat(v1)));
EXPECT(assert_equal(SO4::Hat(v2), SOn::Hat(v2)));
EXPECT(assert_equal(SO4::Hat(v3), SOn::Hat(v3)));
}
//******************************************************************************
// Check that Hat specialization is equal to dynamic version
TEST(SO4, Vee) {
// Check that Hat specialization is equal to dynamic version
using namespace so4_example;
auto X1 = SOn::Hat(v1), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
EXPECT(assert_equal(SO4::Vee(X1), SOn::Vee(X1)));
EXPECT(assert_equal(SO4::Vee(X2), SOn::Vee(X2)));
@ -102,6 +109,8 @@ TEST(SO4, Vee) {
//******************************************************************************
TEST(SO4, Retract) {
using namespace so4_example;
// Check that Cayley yields the same as Expmap for small values
EXPECT(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100)));
EXPECT(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100)));
@ -116,8 +125,9 @@ TEST(SO4, Retract) {
}
//******************************************************************************
// Check that Cayley is identical to dynamic version
TEST(SO4, Local) {
// Check that Cayley is identical to dynamic version
using namespace so4_example;
EXPECT(
assert_equal(id.localCoordinates(Q1), SOn(4).localCoordinates(SOn(Q1))));
EXPECT(
@ -130,6 +140,8 @@ TEST(SO4, Local) {
//******************************************************************************
TEST(SO4, Invariants) {
using namespace so4_example;
EXPECT(check_group_invariants(id, id));
EXPECT(check_group_invariants(id, Q1));
EXPECT(check_group_invariants(Q2, id));
@ -145,6 +157,8 @@ TEST(SO4, Invariants) {
//******************************************************************************
TEST(SO4, compose) {
using namespace so4_example;
SO4 expected = Q1 * Q2;
Matrix actualH1, actualH2;
SO4 actual = Q1.compose(Q2, actualH1, actualH2);
@ -161,20 +175,22 @@ TEST(SO4, compose) {
//******************************************************************************
TEST(SO4, vec) {
using namespace so4_example;
using Vector16 = SO4::VectorN2;
const Vector16 expected = Eigen::Map<const Vector16>(Q2.matrix().data());
Matrix actualH;
const Vector16 actual = Q2.vec(actualH);
EXPECT(assert_equal(expected, actual));
std::function<Vector16(const SO4&)> f = [](const SO4& Q) {
return Q.vec();
};
std::function<Vector16(const SO4&)> f = [](const SO4& Q) { return Q.vec(); };
const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5);
EXPECT(assert_equal(numericalH, actualH));
}
//******************************************************************************
TEST(SO4, topLeft) {
using namespace so4_example;
const Matrix3 expected = Q3.matrix().topLeftCorner<3, 3>();
Matrix actualH;
const Matrix3 actual = topLeft(Q3, actualH);
@ -188,6 +204,8 @@ TEST(SO4, topLeft) {
//******************************************************************************
TEST(SO4, stiefel) {
using namespace so4_example;
const Matrix43 expected = Q3.matrix().leftCols<3>();
Matrix actualH;
const Matrix43 actual = stiefel(Q3, actualH);