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

smarter compose

release/4.3a0
Frank Dellaert 2013-06-05 23:40:05 +00:00
parent f4ad135040
commit 64334e2584
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/geometry/Rot2.h
View File

@ -116,11 +116,11 @@ namespace gtsam {
} }
/** Compose - make a new rotation by adding angles */ /** Compose - make a new rotation by adding angles */
inline Rot2 compose(const Rot2& R1, boost::optional<Matrix&> H1 = inline Rot2 compose(const Rot2& R, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const { boost::none, boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = eye(1); if (H1) *H1 = eye(1);
if (H2) *H2 = eye(1); if (H2) *H2 = eye(1);
return *this * R1; return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
} }
/** Compose - make a new rotation by adding angles */ /** Compose - make a new rotation by adding angles */
@ -129,11 +129,11 @@ namespace gtsam {
} }
/** Between using the default implementation */ /** Between using the default implementation */
inline Rot2 between(const Rot2& p2, boost::optional<Matrix&> H1 = inline Rot2 between(const Rot2& R, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const { boost::none, boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = -eye(1); if (H1) *H1 = -eye(1);
if (H2) *H2 = eye(1); if (H2) *H2 = eye(1);
return between_default(*this, p2); return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
} }
/// @} /// @}