diff --git a/doc/Code/LocalizationFactor.cpp b/doc/Code/LocalizationFactor.cpp index 623aef8cb..d298091dc 100644 --- a/doc/Code/LocalizationFactor.cpp +++ b/doc/Code/LocalizationFactor.cpp @@ -8,11 +8,10 @@ public: Vector evaluateError(const Pose2& q, boost::optional H = boost::none) const { - const double cosTheta = cos(q.theta()); - const double sinTheta = sin(q.theta()); + const Rot2& R = q.rotation(); if (H) (*H) = (gtsam::Matrix(2, 3) << - cosTheta, -sinTheta, 0.0, - sinTheta, cosTheta, 0.0).finished(); + R.c(), -R.s(), 0.0, + R.s(), R.c(), 0.0).finished(); return (Vector(2) << q.x() - mx_, q.y() - my_).finished(); } }; diff --git a/doc/gtsam.pdf b/doc/gtsam.pdf index cd125a713..c6a39a79c 100644 Binary files a/doc/gtsam.pdf and b/doc/gtsam.pdf differ diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index aa8e0db19..d9b205359 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -92,9 +92,8 @@ class UnaryFactor: public NoiseModelFactor1 { // Node's orientation reflects in the Jacobian, in tangent space this is equal to the right-hand rule rotation matrix // H = [ cos(q.theta) -sin(q.theta) 0 ] // [ sin(q.theta) cos(q.theta) 0 ] - const double cosTheta = cos(q.theta()); - const double sinTheta = sin(q.theta()); - if (H) (*H) = (gtsam::Matrix(2, 3) << cosTheta, -sinTheta, 0.0, sinTheta, cosTheta, 0.0).finished(); + const Rot2& R = q.rotation(); + if (H) (*H) = (gtsam::Matrix(2, 3) << R.c(), -R.s(), 0.0, R.s(), R.c(), 0.0).finished(); return (Vector(2) << q.x() - mx_, q.y() - my_).finished(); }