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);
// calculate scale
double x = 0;
double y = 0;
double x = 0, y = 0;
Point3 aPoint, bPoint;
for (const Point3Pair& abPair : abPointPairs) {
std::tie(aPoint, bPoint) = abPair;
@ -121,6 +120,24 @@ Similarity3 Similarity3::AlignGivenR(const std::vector<Point3Pair>& abPointPairs
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
Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
const size_t n = abPointPairs.size();
@ -133,17 +150,19 @@ Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
// Add to form H matrix
Matrix3 H = Z_3x3;
Point3 aPoint, bPoint;
std::vector<Point3Pair> d_abPointPairs;
for (const Point3Pair& abPair : abPointPairs) {
std::tie(aPoint, bPoint) = abPair;
Point3 da = aPoint - aCentroid;
Point3 db = bPoint - bCentroid;
d_abPointPairs.emplace_back(da, db);
H += da * db.transpose();
}
// ClosestTo finds rotation matrix closest to H in Frobenius sense
Rot3 aRb = Rot3::ClosestTo(H);
return AlignGivenR(abPointPairs, aRb);
return AlignGivenR(d_abPointPairs, aRb, aCentroid, bCentroid);
}
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.
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);
/// 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);
/// @}
};