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

update: use avialble rot2 class

release/4.3a0
Navid Mahabadi 2021-03-23 16:13:13 +01:00
parent 554dd790d9
commit 587ad0fad3
3 changed files with 5 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
doc/Code/LocalizationFactor.cpp
View File

@ -8,11 +8,10 @@ public:
Vector evaluateError(const Pose2& q, Vector evaluateError(const Pose2& q,
boost::optional<Matrix&> H = boost::none) const boost::optional<Matrix&> H = boost::none) const
{ {
const double cosTheta = cos(q.theta()); const Rot2& R = q.rotation();
const double sinTheta = sin(q.theta());
if (H) (*H) = (gtsam::Matrix(2, 3) << if (H) (*H) = (gtsam::Matrix(2, 3) <<
cosTheta, -sinTheta, 0.0, R.c(), -R.s(), 0.0,
sinTheta, cosTheta, 0.0).finished(); R.s(), R.c(), 0.0).finished();
return (Vector(2) << q.x() - mx_, q.y() - my_).finished(); return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
} }
}; };

BIN
doc/gtsam.pdf
View File

Binary file not shown.

5
examples/LocalizationExample.cpp
View File

@ -92,9 +92,8 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
// Node's orientation reflects in the Jacobian, in tangent space this is equal to the right-hand rule rotation matrix // 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 ] // H = [ cos(q.theta) -sin(q.theta) 0 ]
// [ sin(q.theta) cos(q.theta) 0 ] // [ sin(q.theta) cos(q.theta) 0 ]
const double cosTheta = cos(q.theta()); const Rot2& R = q.rotation();
const double sinTheta = sin(q.theta()); if (H) (*H) = (gtsam::Matrix(2, 3) << R.c(), -R.s(), 0.0, R.s(), R.c(), 0.0).finished();
if (H) (*H) = (gtsam::Matrix(2, 3) << cosTheta, -sinTheta, 0.0, sinTheta, cosTheta, 0.0).finished();
return (Vector(2) << q.x() - mx_, q.y() - my_).finished(); return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
} }