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

Update doc.

release/4.3a0
ss 2020-08-10 08:37:39 -04:00
parent 8dd9ff5c52
commit 7cfcbff4db
1 changed files with 3 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -95,6 +95,7 @@ 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
Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) { Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
const size_t n = abPointPairs.size(); 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 if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs
@ -138,6 +139,8 @@ Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
return Similarity3(aRb, aTb, s); return Similarity3(aRb, aTb, s);
} }
// Use the geodesic L2 mean to solve the mean of rotations,
// Refer to: http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf (on page 18)
Rot3 Similarity3::rotationAveraging(const std::vector<Rot3>& rotations, double error) { Rot3 Similarity3::rotationAveraging(const std::vector<Rot3>& rotations, double error) {
Rot3 R = rotations[0]; Rot3 R = rotations[0];
const size_t n = rotations.size(); const size_t n = rotations.size();