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

30
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -52,10 +52,8 @@ namespace{
/// Form outer product H. /// Form outer product H.
static Matrix3 calculateH(const std::vector<Point3Pair>& d_abPointPairs) { static Matrix3 calculateH(const std::vector<Point3Pair>& d_abPointPairs) {
Matrix3 H = Z_3x3; Matrix3 H = Z_3x3;
Point3 da, db;
for (const Point3Pair& d_abPair : d_abPointPairs) { for (const Point3Pair& d_abPair : d_abPointPairs) {
std::tie(da, db) = d_abPair; H += d_abPair.first * d_abPair.second.transpose();
H += da * db.transpose();
} }
return H; return H;
} }
@ -63,21 +61,18 @@ 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 Point3Pair& centroids) { static Similarity3 align(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb, const Point3Pair& centroids) {
const double s = calculateScale(d_abPointPairs, aRb); const double s = calculateScale(d_abPointPairs, aRb);
// calculate translation
const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s; const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
return Similarity3(aRb, aTb, s); return Similarity3(aRb, aTb, s);
} }
/// This method 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) {
// 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
static Similarity3 alignGivenR(const std::vector<Point3Pair>& abPointPairs, const Rot3& aRb) {
auto centroids = mean(abPointPairs); auto centroids = mean(abPointPairs);
// substract centroids auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
std::vector<Point3Pair> d_abPointPairs = subtractCentroids(abPointPairs, centroids);
return align(d_abPointPairs, aRb, 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);
} }