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

Fix pose2 align bug.

release/4.3a0
ss 2020-08-06 14:26:22 -04:00
parent 269dea3a24
commit ccdd1471ed
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
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