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

Rot2 rotate() and unrotate() changes to OptionalJacobians

release/4.3a0
nsrinivasan7 2014-11-30 16:09:46 -05:00
parent 010631a2eb
commit ca9c66073f
4 changed files with 32 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/base/Matrix.h
View File

@ -43,6 +43,13 @@ typedef Eigen::Matrix<double,1,4> Matrix14;
typedef Eigen::Matrix<double,1,5> Matrix15;
typedef Eigen::Matrix<double,1,6> Matrix16;
typedef Eigen::Matrix<double,2,1> Matrix21;
typedef Eigen::Matrix<double,3,1> Matrix31;
typedef Eigen::Matrix<double,4,1> Matrix41;
typedef Eigen::Matrix<double,5,1> Matrix51;
typedef Eigen::Matrix<double,6,1> Matrix61;
typedef Eigen::Matrix2d Matrix2;
typedef Eigen::Matrix3d Matrix3;
typedef Eigen::Matrix4d Matrix4;

24
gtsam/geometry/Rot2.cpp
View File

@ -71,16 +71,22 @@ Matrix2 Rot2::matrix() const {
}
/* ************************************************************************* */
Matrix Rot2::transpose() const {
return (Matrix(2, 2) << c_, s_, -s_, c_).finished();
Matrix2 Rot2::transpose() const {
Matrix2 rvalue_;
rvalue_ << c_, s_, -s_, c_;
return rvalue_;
}
/* ************************************************************************* */
// see doc/math.lyx, SO(2) section
Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Point2 Rot2::rotate(const Point2& p, OptionalJacobian<2, 1> H1,
OptionalJacobian<2, 2> H2) const {
const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
if (H1) *H1 = (Matrix(2, 1) << -q.y(), q.x()).finished();
if (H1) {
Matrix21 H1_;
H1_ << -q.y(), q.x();
*H1 = H1_;
}
if (H2) *H2 = matrix();
return q;
}
@ -88,9 +94,13 @@ Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
/* ************************************************************************* */
// see doc/math.lyx, SO(2) section
Point2 Rot2::unrotate(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<2, 1> H1, OptionalJacobian<2, 2> H2) const {
const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
if (H1) *H1 = (Matrix(2, 1) << q.y(), -q.x()).finished(); // R_{pi/2}q
if (H1) {
Matrix21 H1_;
H1_ << q.y(), -q.x();
*H1 = H1_; // R_{pi/2}q
}
if (H2) *H2 = transpose();
return q;
}

10
gtsam/geometry/Rot2.h
View File

@ -180,8 +180,8 @@ namespace gtsam {
/**
* rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$
*/
Point2 rotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const;
/** syntactic sugar for rotate */
inline Point2 operator*(const Point2& p) const {
@ -191,8 +191,8 @@ namespace gtsam {
/**
* rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
*/
Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const;
/// @}
/// @name Standard Interface
@ -228,7 +228,7 @@ namespace gtsam {
Matrix2 matrix() const;
/** return 2*2 transpose (inverse) rotation matrix */
Matrix transpose() const;
Matrix2 transpose() const;
private:
/** Serialization function */

4
gtsam/geometry/tests/testRot2.cpp
View File

@ -49,7 +49,9 @@ TEST( Rot2, unit)
/* ************************************************************************* */
TEST( Rot2, transpose)
{
CHECK(assert_equal(R.inverse().matrix(),R.transpose()));
Matrix expected = R.inverse().matrix();
Matrix actual = R.transpose();
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */