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); double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
if(fabs(n) > 1e-5) { 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); return Rot2::fromCosSin(x / n, y / n);
} else { } else {
if (H) *H = (Matrix(1, 2) << 0.0, 0.0).finished(); if (H)
(*H) << 0.0, 0.0;
return Rot2(); return Rot2();
} }
} }

2
gtsam/geometry/Rot2.h
View File

@ -87,7 +87,7 @@ namespace gtsam {
* @param H optional reference for Jacobian * @param H optional reference for Jacobian
* @return 2D rotation \f$ \in SO(2) \f$ * @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); boost::none);
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */ /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */