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

Merge pull request #459 from Alexma3312/fix/pose2-align 

Fix pose2 align bug.
release/4.3a0
Varun Agrawal 2020-08-11 14:25:37 -04:00 committed by GitHub
parent ac0150ba5d ccdd1471ed
commit 13bdc9a7df
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/Pose2.cpp
View File

@ -322,10 +322,10 @@ boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {
// calculate cos and sin // calculate cos and sin
double c=0,s=0; double c=0,s=0;
for(const Point2Pair& pair: pairs) { for(const Point2Pair& pair: pairs) {
Point2 dq = pair.first - cp; Point2 dp = pair.first - cp;
Point2 dp = pair.second - cq; Point2 dq = pair.second - cq;
c += dp.x() * dq.x() + dp.y() * dq.y(); c += dp.x() * dq.x() + dp.y() * dq.y();
s += dp.y() * dq.x() - dp.x() * dq.y(); // this works but is negative from formula above !! :-( s += -dp.y() * dq.x() + dp.x() * dq.y();
} }
// calculate angle and translation // calculate angle and translation