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

fix bugs in Karcher mean

release/4.3a0
John Lambert 2021-03-03 11:25:55 -05:00
parent 943879c6cc
commit 3121604f63
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/geometry/Similarity3.cpp
View File

@ -179,12 +179,13 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
Pose3 wTa, wTb;
for (const Pose3Pair &abPair : abPosePairs) {
std::tie(wTa, wTb) = abPair;
rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse()));
Rot3 aRb = wTa.rotation().between(wTb.rotation());
rotations.emplace_back(aRb);
abPointPairs.emplace_back(wTa.translation(), wTb.translation());
}
const Rot3 aRb = FindKarcherMean<Rot3>(rotations);
const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
return alignGivenR(abPointPairs, aRb);
return alignGivenR(abPointPairs, aRb_estimate);
}
Matrix4 Similarity3::wedge(const Vector7 &xi) {