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

clean up Sim(3) notations

release/4.3a0
John Lambert 2021-03-05 14:17:29 -05:00
parent 4d079e1905
commit 063a41a3f8
1 changed files with 27 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/geometry/tests/testSimilarity3.cpp
View File

@ -260,18 +260,22 @@ TEST(Similarity3, GroupAction) {
//****************************************************************************** //******************************************************************************
// Group action on Pose3 // Group action on Pose3
TEST(Similarity3, GroupActionPose3) { TEST(Similarity3, GroupActionPose3) {
Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); // Suppose we know the pose of the egovehicle in the world frame
Similarity3 wSe(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
// Create source poses // Create source poses (two objects o1 and o2 living in the egovehicle "e" frame)
Pose3 Ta1(Rot3(), Point3(0, 0, 0)); // Suppose they are 3d cuboids detected by onboard sensor, in the egovehicle frame
Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); Pose3 eTo1(Rot3(), Point3(0, 0, 0));
Pose3 eTo2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
// Create destination poses // Create destination poses (two objects now living in the world/city "w" frame)
Pose3 expected_Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); // Desired to place the objects into the world frame for tracking
Pose3 expected_Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10)); Pose3 expected_wTo1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 expected_wTo2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
EXPECT(assert_equal(expected_Tb1, bSa.transformFrom(Ta1))); // objects now live in the world frame, instead of in the egovehicle frame
EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2))); EXPECT(assert_equal(expected_wTo1, wSe.transformFrom(eTo1)));
EXPECT(assert_equal(expected_wTo2, wSe.transformFrom(eTo2)));
} }
// Test left group action compatibility. // Test left group action compatibility.
@ -346,26 +350,28 @@ TEST(Similarity3, AlignPoint3_3) {
//****************************************************************************** //******************************************************************************
// Align with Pose3 Pairs // Align with Pose3 Pairs
TEST(Similarity3, AlignPose3) { TEST(Similarity3, AlignPose3) {
Similarity3 expected_bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); Similarity3 expected_wSe(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
// Create source poses // Create source poses (two objects o1 and o2 living in the egovehicle "e" frame)
Pose3 Ta1(Rot3(), Point3(0, 0, 0)); // Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); Pose3 eTo1(Rot3(), Point3(0, 0, 0));
Pose3 eTo2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
// Create destination poses // Create destination poses (same two objects, but instead living in the world/city "w" frame)
Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); Pose3 wTo1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10)); Pose3 wTo2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
Pose3Pair bTa1(make_pair(Tb1, Ta1)); Pose3Pair wTe1(make_pair(wTo1, eTo1));
Pose3Pair bTa2(make_pair(Tb2, Ta2)); Pose3Pair wTe2(make_pair(wTo2, eTo2));
vector<Pose3Pair> correspondences{bTa1, bTa2}; vector<Pose3Pair> correspondences{wTe1, wTe2};
// Cayley transform cannot accommodate 180 degree rotations, // Cayley transform cannot accommodate 180 degree rotations,
// hence we only test for Expmap // hence we only test for Expmap
#ifdef GTSAM_ROT3_EXPMAP #ifdef GTSAM_ROT3_EXPMAP
Similarity3 actual_bSa = Similarity3::Align(correspondences); // Recover the transformation wSe (i.e. world_S_egovehicle)
EXPECT(assert_equal(expected_bSa, actual_bSa)); Similarity3 actual_wSe = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_wSe, actual_wSe));
#endif #endif
} }