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

More efficient derivative

release/4.3a0
dellaert 2014-02-01 00:50:03 -05:00
parent f6a733fb3e
commit f6731fe559
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -60,8 +60,12 @@ public:
static Sphere2 unrotate(const Rot2& R, const Sphere2& p,
boost::optional<Matrix&> HR = boost::none) {
Sphere2 q = Rot3::yaw(R.theta()) * p;
if (HR) // 2*3 3*1
(*HR) = -q.basis().transpose() * q.skew().col(2);
if (HR) {
HR->resize(2, 1);
Point3 Q = q.unitVector();
Matrix B = q.basis().transpose();
(*HR) = Q.x() * B.col(1) - Q.y() * B.col(0);
}
return q;
}