From 64334e25842ca47dcda2372da2201f8a1df24496 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 5 Jun 2013 23:40:05 +0000 Subject: [PATCH] smarter compose --- gtsam/geometry/Rot2.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 591e5c8a0..45012770f 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -116,11 +116,11 @@ namespace gtsam { } /** Compose - make a new rotation by adding angles */ - inline Rot2 compose(const Rot2& R1, boost::optional H1 = + inline Rot2 compose(const Rot2& R, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { if (H1) *H1 = 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 */ @@ -129,11 +129,11 @@ namespace gtsam { } /** Between using the default implementation */ - inline Rot2 between(const Rot2& p2, boost::optional H1 = + inline Rot2 between(const Rot2& R, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { if (H1) *H1 = -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_); } /// @}