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

Fix double computation.

release/4.3a0
alexma3312 2020-09-14 00:20:50 -04:00
parent 41921c3173
commit 66c9a63919
2 changed files with 27 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -103,8 +103,7 @@ Similarity3 Similarity3::AlignGivenR(const std::vector<Point3Pair>& abPointPairs
std::tie(aCentroid, bCentroid) = mean(abPointPairs); std::tie(aCentroid, bCentroid) = mean(abPointPairs);
// calculate scale // calculate scale
double x = 0; double x = 0, y = 0;
double y = 0;
Point3 aPoint, bPoint; Point3 aPoint, bPoint;
for (const Point3Pair& abPair : abPointPairs) { for (const Point3Pair& abPair : abPointPairs) {
std::tie(aPoint, bPoint) = abPair; std::tie(aPoint, bPoint) = abPair;
@ -121,6 +120,24 @@ Similarity3 Similarity3::AlignGivenR(const std::vector<Point3Pair>& abPointPairs
return Similarity3(aRb, aTb, s); return Similarity3(aRb, aTb, s);
} }
// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
Similarity3 Similarity3::AlignGivenR(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid) {
// calculate scale
double x = 0, y = 0;
Point3 da, db;
for (const Point3Pair& d_abPair : d_abPointPairs) {
std::tie(da, db) = d_abPair;
Vector3 Rdb = aRb * db;
y += da.transpose() * Rdb;
x += Rdb.transpose() * Rdb;
}
const double s = y / x;
// calculate translation
const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s;
return Similarity3(aRb, aTb, s);
}
// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) { Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
const size_t n = abPointPairs.size(); const size_t n = abPointPairs.size();
@ -133,17 +150,19 @@ Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
// Add to form H matrix // Add to form H matrix
Matrix3 H = Z_3x3; Matrix3 H = Z_3x3;
Point3 aPoint, bPoint; Point3 aPoint, bPoint;
std::vector<Point3Pair> d_abPointPairs;
for (const Point3Pair& abPair : abPointPairs) { for (const Point3Pair& abPair : abPointPairs) {
std::tie(aPoint, bPoint) = abPair; std::tie(aPoint, bPoint) = abPair;
Point3 da = aPoint - aCentroid; Point3 da = aPoint - aCentroid;
Point3 db = bPoint - bCentroid; Point3 db = bPoint - bCentroid;
d_abPointPairs.emplace_back(da, db);
H += da * db.transpose(); H += da * db.transpose();
} }
// ClosestTo finds rotation matrix closest to H in Frobenius sense // ClosestTo finds rotation matrix closest to H in Frobenius sense
Rot3 aRb = Rot3::ClosestTo(H); Rot3 aRb = Rot3::ClosestTo(H);
return AlignGivenR(abPointPairs, aRb); return AlignGivenR(d_abPointPairs, aRb, aCentroid, bCentroid);
} }
Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) { Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) {

6
gtsam_unstable/geometry/Similarity3.h
View File

@ -211,9 +211,13 @@ private:
/// Calculate expmap and logmap coefficients. /// Calculate expmap and logmap coefficients.
static Matrix3 GetV(Vector3 w, double lambda); static Matrix3 GetV(Vector3 w, double lambda);
/// This methods estimates the similarity transform from point pairs, given a known or estimated rotation. /// This method estimates the similarity transform from point pairs, given a known or estimated rotation.
static Similarity3 AlignGivenR(const std::vector<Point3Pair>& abPointPairs, const Rot3& aRb); static Similarity3 AlignGivenR(const std::vector<Point3Pair>& abPointPairs, const Rot3& aRb);
/// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids.
static Similarity3 AlignGivenR(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid);
/// @} /// @}
}; };