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

Correct variable names and refactor code.

release/4.3a0
alexma3312 2020-08-17 17:58:45 -04:00
parent 58ec261cd7
commit 6f33d00654
2 changed files with 36 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -106,17 +106,18 @@ Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
aCentroid += abPair.first;
bCentroid += abPair.second;
}
double f = 1.0 / n;
const double f = 1.0 / n;
aCentroid *= f;
bCentroid *= f;
// Add to form H matrix
Matrix3 H = Z_3x3;
vector<Point3Pair> d_abPairs;
d_abPairs.reserve(n);
for (const Point3Pair& abPair : abPointPairs) {
Point3 da = abPair.first - aCentroid;
Point3 db = abPair.second - bCentroid;
d_abPairs.push_back(make_pair(da,db));
d_abPairs.emplace_back(da, db);
H += da * db.transpose();
}
@ -135,7 +136,7 @@ Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
}
double s = y / x;
Point3 aTb = (Point3(aCentroid) - s * (aRb * Point3(bCentroid))) / s;
Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s;
return Similarity3(aRb, aTb, s);
}
@ -166,9 +167,9 @@ Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) {
for (const Pose3Pair& abPair : abPosePairs) {
aCentroid += abPair.first.translation();
bCentroid += abPair.second.translation();
rotationList.push_back(abPair.first.rotation().compose(abPair.second.rotation().inverse()));
rotationList.emplace_back(abPair.first.rotation().compose(abPair.second.rotation().inverse()));
}
double f = 1.0 / n;
const double f = 1.0 / n;
aCentroid *= f;
bCentroid *= f;
Rot3 aRb = Similarity3::rotationAveraging(rotationList);
@ -185,7 +186,7 @@ Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) {
}
double s = y / x;
Point3 aTb = (Point3(aCentroid) - s * (aRb * Point3(bCentroid))) / s;
Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s;
return Similarity3(aRb, aTb, s);
}

64
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -260,71 +260,65 @@ TEST(Similarity3, GroupAction) {
//******************************************************************************
// Group action on Pose3
TEST(Similarity3, GroupActionPose3) {
Similarity3 bTa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
// Create source poses
Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0));
Pose3 Ta2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0));
// Create destination poses
Pose3 expectedTb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 expectedTb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10));
Pose3 expected_Tb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 expected_Tb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10));
EXPECT(assert_equal(expectedTb1, bTa.transformFrom(Ta1)));
EXPECT(assert_equal(expectedTb2, bTa.transformFrom(Ta2)));
EXPECT(assert_equal(expected_Tb1, bSa.transformFrom(Ta1)));
EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2)));
}
//******************************************************************************
// Align with Point3 Pairs
TEST(Similarity3, AlignPoint3_1) {
Similarity3 expected(Rot3::Rz(-90 * degree), Point3(3, 4, 5), 2.0);
Similarity3 expected_aSb(Rot3::Rz(-90 * degree), Point3(3, 4, 5), 2.0);
Point3 p1 = Point3(0, 0, 0);
Point3 p2 = Point3(3, 0, 0);
Point3 p3 = Point3(3, 0, 4);
Point3 b1(0, 0, 0), b2(3, 0, 0), b3(3, 0, 4);
Point3Pair ab1(make_pair(expected.transformFrom(p1), p1));
Point3Pair ab2(make_pair(expected.transformFrom(p2), p2));
Point3Pair ab3(make_pair(expected.transformFrom(p3), p3));
Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));
vector<Point3Pair> correspondences{ab1, ab2, ab3};
Similarity3 actual = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected, actual));
Similarity3 actual_aSb = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_aSb, actual_aSb));
}
TEST(Similarity3, AlignPoint3_2) {
Similarity3 expected(Rot3(), Point3(10, 10, 0), 1.0);
Similarity3 expected_aSb(Rot3(), Point3(10, 10, 0), 1.0);
Point3 p1 = Point3(0, 0, 0);
Point3 p2 = Point3(20, 10, 0);
Point3 p3 = Point3(10, 20, 0);
Point3 b1(0, 0, 0), b2(20, 10, 0), b3(10, 20, 0);
Point3Pair ab1(make_pair(expected.transformFrom(p1), p1));
Point3Pair ab2(make_pair(expected.transformFrom(p2), p2));
Point3Pair ab3(make_pair(expected.transformFrom(p3), p3));
Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));
vector<Point3Pair> correspondences{ab1, ab2, ab3};
Similarity3 actual = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected, actual));
Similarity3 actual_aSb = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_aSb, actual_aSb));
}
TEST(Similarity3, AlignPoint3_3) {
Similarity3 expected(Rot3::RzRyRx(0.3, 0.2, 0.1), Point3(20, 10, 5), 1.0);
Similarity3 expected_aSb(Rot3::RzRyRx(0.3, 0.2, 0.1), Point3(20, 10, 5), 1.0);
Point3 p1 = Point3(0, 0, 1);
Point3 p2 = Point3(10, 0, 2);
Point3 p3 = Point3(20, -10, 30);
Point3 b1(0, 0, 1), b2(10, 0, 2), b3(20, -10, 30);
Point3Pair ab1(make_pair(expected.transformFrom(p1), p1));
Point3Pair ab2(make_pair(expected.transformFrom(p2), p2));
Point3Pair ab3(make_pair(expected.transformFrom(p3), p3));
Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));
vector<Point3Pair> correspondences{ab1, ab2, ab3};
Similarity3 actual = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected, actual));
Similarity3 actual_aSb = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_aSb, actual_aSb));
}
//******************************************************************************
@ -339,7 +333,7 @@ TEST(Similarity3, RotationAveraging) {
//******************************************************************************
// Align with Pose3 Pairs
TEST(Similarity3, AlignPose3) {
Similarity3 expected(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
Similarity3 expected_aSb(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
// Create source poses
Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0));
@ -354,8 +348,8 @@ TEST(Similarity3, AlignPose3) {
vector<Pose3Pair> correspondences{bTa1, bTa2};
Similarity3 actual = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected, actual));
Similarity3 actual_aSb = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_aSb, actual_aSb));
}
//******************************************************************************