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

added OptionalJacobian to relativeBearing

release/4.3a0
nsrinivasan7 2014-11-30 11:59:32 -05:00
parent 61d9948c3d
commit 9137123f5f
2 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/geometry/Rot2.cpp
View File

@ -96,13 +96,15 @@ Point2 Rot2::unrotate(const Point2& p,
}
/* ************************************************************************* */
Rot2 Rot2::relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
if(fabs(n) > 1e-5) {
if (H) *H = (Matrix(1, 2) << -y / d2, x / d2).finished();
if (H)
*H << -y / d2, x / d2;
return Rot2::fromCosSin(x / n, y / n);
} else {
if (H) *H = (Matrix(1, 2) << 0.0, 0.0).finished();
if (H)
(*H) << 0.0, 0.0;
return Rot2();
}
}

2
gtsam/geometry/Rot2.h
View File

@ -87,7 +87,7 @@ namespace gtsam {
* @param H optional reference for Jacobian
* @return 2D rotation \f$ \in SO(2) \f$
*/
static Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H =
static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H =
boost::none);
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */