diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 5d9308b13..af8d701ec 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -65,9 +65,9 @@ Rot2& Rot2::normalize() { /* ************************************************************************* */ Matrix2 Rot2::matrix() const { - Matrix2 rvalue; - rvalue << c_, -s_, s_, c_; - return rvalue; + Matrix2 rvalue_; + rvalue_ << c_, -s_, s_, c_; + return rvalue_; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 2f9d1a398..558dd06f6 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -116,10 +116,10 @@ namespace gtsam { } /** Compose - make a new rotation by adding angles */ - 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); + inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 = + boost::none, OptionalJacobian<1,1> H2 = boost::none) const { + if (H1) (*H1)<< 1; // set to 1x1 identity matrix + if (H2) (*H2)<< 1; // set to 1x1 identity matrix return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); } @@ -129,10 +129,10 @@ namespace gtsam { } /** Between using the default implementation */ - 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); + inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 = + boost::none, OptionalJacobian<1,1> H2 = boost::none) const { + if (H1) *H1 << -1; // set to 1x1 identity matrix + if (H2) *H2 << 1; // set to 1x1 identity matrix return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); }