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);
}
// 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) {
// calculate centroids
Point3 aCentroid, bCentroid;
std::tie(aCentroid, bCentroid) = mean(abPointPairs);
// calculate scale
double x = 0, y = 0;
std::vector<Point3Pair> Similarity3::SubtractCentroids(const std::vector<Point3Pair>& abPointPairs, const Point3& aCentroid, const Point3& bCentroid) {
std::vector<Point3Pair> d_abPointPairs;
Point3 aPoint, bPoint;
for (const Point3Pair& abPair : abPointPairs) {
std::tie(aPoint, bPoint) = abPair;
const Point3 da = aPoint - aCentroid;
const Point3 db = bPoint - bCentroid;
Vector3 Rdb = aRb * db;
y += da.transpose() * Rdb;
x += Rdb.transpose() * Rdb;
Point3 da = aPoint - aCentroid;
Point3 db = bPoint - bCentroid;
d_abPointPairs.emplace_back(da, db);
}
const double s = y / x;
// calculate translation
const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s;
return Similarity3(aRb, aTb, s);
return d_abPointPairs;
}
// 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
std::pair<double, double> Similarity3::GetXY(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb) {
double x = 0, y = 0;
Point3 da, db;
for (const Point3Pair& d_abPair : d_abPointPairs) {
@ -131,38 +117,52 @@ Similarity3 Similarity3::AlignGivenR(const std::vector<Point3Pair>& d_abPointPai
y += da.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
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();
if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs
Similarity3 Similarity3::AlignGivenR(const std::vector<Point3Pair>& abPointPairs, const Rot3& aRb) {
// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
// calculate centroids
Point3 aCentroid, bCentroid;
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
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();
}
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
Point3 aCentroid, bCentroid;
std::tie(aCentroid, bCentroid) = mean(abPointPairs);
// substract centroids
std::vector<Point3Pair> d_abPointPairs = SubtractCentroids(abPointPairs, aCentroid, bCentroid);
// form H matrix
Matrix3 H = GetH(d_abPointPairs);
// ClosestTo finds rotation matrix closest to H in Frobenius sense
Rot3 aRb = Rot3::ClosestTo(H);
return AlignGivenR(d_abPointPairs, aRb, aCentroid, bCentroid);
return AlignGivenRandCentroids(d_abPointPairs, aRb, aCentroid, bCentroid);
}
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.
static Matrix3 GetV(Vector3 w, double lambda);
/// 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);
/// Subtract centroids from point pairs.
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.
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);
/// @}
};