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

Refactor Align with short functions.

release/4.3a0
alexma3312 2020-09-20 11:45:30 -04:00
parent 470862e42c
commit bf0651b626
2 changed files with 52 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -96,33 +96,19 @@ Point3 Similarity3::operator*(const Point3& p) const {
return transformFrom(p); return transformFrom(p);
} }
// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 std::vector<Point3Pair> Similarity3::SubtractCentroids(const std::vector<Point3Pair>& abPointPairs, const Point3& aCentroid, const Point3& bCentroid) {
Similarity3 Similarity3::AlignGivenR(const std::vector<Point3Pair>& abPointPairs, const Rot3& aRb) { std::vector<Point3Pair> d_abPointPairs;
// calculate centroids
Point3 aCentroid, bCentroid;
std::tie(aCentroid, bCentroid) = mean(abPointPairs);
// calculate scale
double x = 0, 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;
const Point3 da = aPoint - aCentroid; Point3 da = aPoint - aCentroid;
const Point3 db = bPoint - bCentroid; Point3 db = bPoint - bCentroid;
Vector3 Rdb = aRb * db; d_abPointPairs.emplace_back(da, db);
y += da.transpose() * Rdb;
x += Rdb.transpose() * Rdb;
} }
const double s = y / x; return d_abPointPairs;
// 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 std::pair<double, double> Similarity3::GetXY(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb) {
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; 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) {
@ -131,38 +117,52 @@ Similarity3 Similarity3::AlignGivenR(const std::vector<Point3Pair>& d_abPointPai
y += da.transpose() * Rdb; y += da.transpose() * Rdb;
x += Rdb.transpose() * Rdb; x += Rdb.transpose() * Rdb;
} }
const double s = y / x; return std::make_pair(x, y);
}
Matrix3 Similarity3::GetH(const std::vector<Point3Pair>& d_abPointPairs) {
Matrix3 H = Z_3x3;
Point3 da, db;
for (const Point3Pair& d_abPair : d_abPointPairs) {
std::tie(da, db) = d_abPair;
H += da * db.transpose();
}
return H;
}
Similarity3 Similarity3::AlignGivenRandCentroids(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid) {
double x, y;
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);
} }
// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 Similarity3 Similarity3::AlignGivenR(const std::vector<Point3Pair>& abPointPairs, const Rot3& aRb) {
Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) { // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
const size_t n = abPointPairs.size();
if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs
// calculate centroids // calculate centroids
Point3 aCentroid, bCentroid; Point3 aCentroid, bCentroid;
std::tie(aCentroid, bCentroid) = mean(abPointPairs); std::tie(aCentroid, bCentroid) = mean(abPointPairs);
// substract centroids
std::vector<Point3Pair> d_abPointPairs = SubtractCentroids(abPointPairs, aCentroid, bCentroid);
return AlignGivenRandCentroids(d_abPointPairs, aRb, aCentroid, bCentroid);
}
// Add to form H matrix Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
Matrix3 H = Z_3x3; // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
Point3 aPoint, bPoint; const size_t n = abPointPairs.size();
std::vector<Point3Pair> d_abPointPairs; if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs
for (const Point3Pair& abPair : abPointPairs) { // calculate centroids
std::tie(aPoint, bPoint) = abPair; Point3 aCentroid, bCentroid;
Point3 da = aPoint - aCentroid; std::tie(aCentroid, bCentroid) = mean(abPointPairs);
Point3 db = bPoint - bCentroid; // substract centroids
d_abPointPairs.emplace_back(da, db); std::vector<Point3Pair> d_abPointPairs = SubtractCentroids(abPointPairs, aCentroid, bCentroid);
H += da * db.transpose(); // form H matrix
} Matrix3 H = GetH(d_abPointPairs);
// 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 AlignGivenRandCentroids(d_abPointPairs, aRb, aCentroid, bCentroid);
return AlignGivenR(d_abPointPairs, aRb, aCentroid, bCentroid);
} }
Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) { Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) {

14
gtsam_unstable/geometry/Similarity3.h
View File

@ -211,12 +211,20 @@ 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 method estimates the similarity transform from point pairs, given a known or estimated rotation. /// Subtract centroids from point pairs.
static Similarity3 AlignGivenR(const std::vector<Point3Pair>& abPointPairs, const Rot3& aRb); static std::vector<Point3Pair> SubtractCentroids(const std::vector<Point3Pair>& abPointPairs, const Point3& aCentroid, const Point3& bCentroid);
/// Form inner products x and y.
static std::pair<double, double> GetXY(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb);
/// Form outer product H.
static Matrix3 GetH(const std::vector<Point3Pair>& d_abPointPairs);
/// 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 AlignGivenR(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid); static Similarity3 AlignGivenRandCentroids(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid);
/// 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);
/// @} /// @}
}; };