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

fix notations

release/4.3a0
John Lambert 2021-03-05 14:17:00 -05:00
parent 96cce19f79
commit 4d079e1905
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/Similarity3.cpp
View File

@ -177,10 +177,10 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
Point3Pairs abPointPairs; Point3Pairs abPointPairs;
rotations.reserve(n); rotations.reserve(n);
abPointPairs.reserve(n); abPointPairs.reserve(n);
Pose3 wTa, wTb; Pose3 aTi, bTi;
for (const Pose3Pair &abPair : abPosePairs) { for (const Pose3Pair &abPair : abPosePairs) {
std::tie(wTa, wTb) = abPair; std::tie(aTi, bTi) = abPair;
Rot3 aRb = wTa.rotation().between(wTb.rotation()); Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse());
rotations.emplace_back(aRb); rotations.emplace_back(aRb);
abPointPairs.emplace_back(wTa.translation(), wTb.translation()); abPointPairs.emplace_back(wTa.translation(), wTb.translation());
} }