fix: UnaryFactor Jacobian #724
Conversation
nmahabadi
commented
Mar 23, 2021
nmahabadi
commented
- Fix Unaryfactor jacobian in LocalizationExample.cpp.
- Update gtsam document.
|
@dellaert @varunagrawal this is a new PR with the latest from "develop". Let me know if this seems fine. |
doc/Code/LocalizationFactor.cpp
Outdated
| @@ -8,7 +8,11 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> { | |||
| Vector evaluateError(const Pose2& q, | |||
| boost::optional<Matrix&> H = boost::none) const | |||
| { | |||
| if (H) (*H) = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0).finished(); | |||
| const double cosTheta = cos(q.theta()); | |||
There was a problem hiding this comment.
Add comment, now that you figured it out :-) Also, GTSAM actually stores a Rot2 as sin and cos, so no need to re-compute. You can just use:
const Rot2& R = q.rotation();
and then use R.s() and R.c()
examples/LocalizationExample.cpp
Outdated
| // 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()); |
There was a problem hiding this comment.
same comments as above of course :-)
There was a problem hiding this comment.
Right! it is smart to use the available feature, I will update the PR soon.
|
@dellaert I updated the code/doc as you suggested. |
|
Merging failed. Try merging in develop? |
|
@dellaert I merged the latest develop. The merge seems ok. |
