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

Refactor code to increase speed.

release/4.3a0
alexma3312 2020-09-26 14:09:37 -04:00
parent e12d3ba197
commit 8236d69fa1
1 changed files with 54 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -23,61 +23,56 @@
namespace gtsam { namespace gtsam {
namespace{ namespace {
/// Subtract centroids from point pairs. /// Subtract centroids from point pairs.
static std::vector<Point3Pair> subtractCentroids(const std::vector<Point3Pair>& abPointPairs, const Point3Pair& centroids) { static std::vector<Point3Pair> subtractCentroids(const std::vector<Point3Pair>& abPointPairs, const Point3Pair& centroids) {
std::vector<Point3Pair> d_abPointPairs; std::vector<Point3Pair> d_abPointPairs;
for (const Point3Pair& abPair : abPointPairs) { for (const Point3Pair& abPair : abPointPairs) {
Point3 da = abPair.first - centroids.first; Point3 da = abPair.first - centroids.first;
Point3 db = abPair.second - centroids.second; Point3 db = abPair.second - centroids.second;
d_abPointPairs.emplace_back(da, db); d_abPointPairs.emplace_back(da, db);
}
return d_abPointPairs;
}
/// Form inner products x and y and calculate scale.
static const double calculateScale(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb) {
double x = 0, y = 0;
Point3 da, db;
for (const Point3Pair& d_abPair : d_abPointPairs) {
std::tie(da, db) = d_abPair;
const Vector3 da_prime = aRb * db;
y += da.transpose() * da_prime;
x += da_prime.transpose() * da_prime;
}
const double s = y / x;
return s;
}
/// Form outer product H.
static Matrix3 calculateH(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;
}
/// 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 Point3Pair& centroids) {
const double s = calculateScale(d_abPointPairs, aRb);
// calculate translation
const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
return Similarity3(aRb, aTb, s);
}
/// 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) {
// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
auto centroids = mean(abPointPairs);
// substract centroids
std::vector<Point3Pair> d_abPointPairs = subtractCentroids(abPointPairs, centroids);
return align(d_abPointPairs, aRb, centroids);
} }
return d_abPointPairs;
} }
/// Form inner products x and y and calculate scale.
static const double calculateScale(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb) {
double x = 0, y = 0;
Point3 da, db;
for (const Point3Pair& d_abPair : d_abPointPairs) {
std::tie(da, db) = d_abPair;
const Vector3 da_prime = aRb * db;
y += da.transpose() * da_prime;
x += da_prime.transpose() * da_prime;
}
const double s = y / x;
return s;
}
/// Form outer product H.
static Matrix3 calculateH(const std::vector<Point3Pair>& d_abPointPairs) {
Matrix3 H = Z_3x3;
for (const Point3Pair& d_abPair : d_abPointPairs) {
H += d_abPair.first * d_abPair.second.transpose();
}
return H;
}
/// 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 Point3Pair& centroids) {
const double s = calculateScale(d_abPointPairs, aRb);
const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
return Similarity3(aRb, aTb, s);
}
/// This method estimates the similarity transform from point pairs, given a known or estimated rotation.
// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
static Similarity3 alignGivenR(const std::vector<Point3Pair>& abPointPairs, const Rot3& aRb) {
auto centroids = mean(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
return align(d_abPointPairs, aRb, centroids);
}
} // namespace
Similarity3::Similarity3() : Similarity3::Similarity3() :
t_(0,0,0), s_(1) { t_(0,0,0), s_(1) {
@ -154,13 +149,9 @@ Point3 Similarity3::operator*(const Point3& p) const {
Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) { Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
// 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
const size_t n = abPointPairs.size(); if (abPointPairs.size() < 3) throw std::runtime_error("input should have at least 3 pairs of points");
if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs
// calculate centroids
auto centroids = mean(abPointPairs); auto centroids = mean(abPointPairs);
// substract centroids auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
std::vector<Point3Pair> d_abPointPairs = subtractCentroids(abPointPairs, centroids);
// form H matrix
Matrix3 H = calculateH(d_abPointPairs); Matrix3 H = calculateH(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);
@ -169,19 +160,20 @@ Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) { Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) {
const size_t n = abPosePairs.size(); const size_t n = abPosePairs.size();
if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses"); // we need at least two pairs if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses");
// calculate rotation // calculate rotation
vector<Rot3> rotationList; vector<Rot3> rotations;
vector<Point3Pair> abPointPairs; vector<Point3Pair> abPointPairs;
rotations.reserve(n);
abPointPairs.reserve(n); abPointPairs.reserve(n);
Pose3 wTa, wTb; Pose3 wTa, wTb;
for (const Pose3Pair& abPair : abPosePairs) { for (const Pose3Pair& abPair : abPosePairs) {
std::tie(wTa, wTb) = abPair; std::tie(wTa, wTb) = abPair;
rotationList.emplace_back(wTa.rotation().compose(wTb.rotation().inverse())); rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse()));
abPointPairs.emplace_back(wTa.translation(), wTb.translation()); abPointPairs.emplace_back(wTa.translation(), wTb.translation());
} }
const Rot3 aRb = FindKarcherMean<Rot3>(rotationList); const Rot3 aRb = FindKarcherMean<Rot3>(rotations);
return alignGivenR(abPointPairs, aRb); return alignGivenR(abPointPairs, aRb);
} }