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

add const on Rot3

release/4.3a0
John Lambert 2021-03-10 09:45:19 -05:00
parent 490c14b95e
commit bddd7e68eb
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/geometry/Similarity3.cpp
View File

@ -180,10 +180,11 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
Point3Pairs abPointPairs; Point3Pairs abPointPairs;
rotations.reserve(n); rotations.reserve(n);
abPointPairs.reserve(n); abPointPairs.reserve(n);
// note that frame "i" is the i'th object/camera/etc body frame
Pose3 aTi, bTi; Pose3 aTi, bTi;
for (const Pose3Pair &abPair : abPosePairs) { for (const Pose3Pair &abPair : abPosePairs) {
std::tie(aTi, bTi) = abPair; std::tie(aTi, bTi) = abPair;
Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse()); const Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse());
rotations.emplace_back(aRb); rotations.emplace_back(aRb);
abPointPairs.emplace_back(aTi.translation(), bTi.translation()); abPointPairs.emplace_back(aTi.translation(), bTi.translation());
} }