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

Change getXY to calculateScale.

release/4.3a0
alexma3312 2020-09-26 12:03:10 -04:00
parent 463b634328
commit ffd0d5e6b9
1 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -37,8 +37,8 @@ namespace{
return d_abPointPairs; return d_abPointPairs;
} }
/// Form inner products x and y. /// Form inner products x and y and calculate scale.
static std::pair<double, double> getXY(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb) { static const double calculateScale(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb) {
double x = 0, y = 0; double x = 0, y = 0;
Point3 da, db; Point3 da, db;
for (const Point3Pair& d_abPair : d_abPointPairs) { for (const Point3Pair& d_abPair : d_abPointPairs) {
@ -47,7 +47,8 @@ namespace{
y += da.transpose() * Rdb; y += da.transpose() * Rdb;
x += Rdb.transpose() * Rdb; x += Rdb.transpose() * Rdb;
} }
return std::make_pair(x, y); const double s = y / x;
return s;
} }
/// Form outer product H. /// Form outer product H.
@ -63,9 +64,7 @@ namespace{
/// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids.
static Similarity3 align(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid) { static Similarity3 align(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid) {
double x, y; const double s = calculateScale(d_abPointPairs, aRb);
std::tie(x, y) = getXY(d_abPointPairs, aRb);
const double s = y / x;
// calculate translation // calculate translation
const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s;
return Similarity3(aRb, aTb, s); return Similarity3(aRb, aTb, s);