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

Rot2 done, make check works

release/4.3a0
nsrinivasan7 2014-11-30 18:01:15 -05:00
parent 186b01fd71
commit bee32cc472
2 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/Rot2.cpp
View File

@ -65,9 +65,9 @@ Rot2& Rot2::normalize() {
/* ************************************************************************* */ /* ************************************************************************* */
Matrix2 Rot2::matrix() const { Matrix2 Rot2::matrix() const {
Matrix2 rvalue; Matrix2 rvalue_;
rvalue << c_, -s_, s_, c_; rvalue_ << c_, -s_, s_, c_;
return rvalue; return rvalue_;
} }
/* ************************************************************************* */ /* ************************************************************************* */

16
gtsam/geometry/Rot2.h
View File

@ -116,10 +116,10 @@ namespace gtsam {
} }
/** Compose - make a new rotation by adding angles */ /** Compose - make a new rotation by adding angles */
inline Rot2 compose(const Rot2& R, boost::optional<Matrix&> H1 = inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const { boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 = eye(1); if (H1) (*H1)<< 1; // set to 1x1 identity matrix
if (H2) *H2 = eye(1); if (H2) (*H2)<< 1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); 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 */ /** Between using the default implementation */
inline Rot2 between(const Rot2& R, boost::optional<Matrix&> H1 = inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const { boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 = -eye(1); if (H1) *H1 << -1; // set to 1x1 identity matrix
if (H2) *H2 = eye(1); if (H2) *H2 << 1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
} }