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

quick cleanup - expmap comments

release/4.3a0
Frank Dellaert 2009-12-11 14:22:35 +00:00
parent 16e1f6e56d
commit abc268a13d
2 changed files with 47 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
cpp/Rot2.cpp
View File

@ -24,7 +24,46 @@ namespace gtsam {
/* ************************************************************************* */
Rot2 Rot2::exmap(const Vector& v) const {
if (zero(v)) return (*this);
return Rot2(v(0)) * (*this);
return Rot2(v(0)) * (*this); // exponential map then compose
}
/* ************************************************************************* */
Matrix Rot2::matrix() const {
double r[] = { c_, -s_, s_, c_ };
return Matrix_(2, 2, r);
}
/* ************************************************************************* */
Matrix Rot2::transpose() const {
double r[] = { c_, s_, -s_, c_ };
return Matrix_(2, 2, r);
}
/* ************************************************************************* */
Rot2 Rot2::inverse() const { return Rot2(c_, -s_);}
/* ************************************************************************* */
Rot2 Rot2::operator*(const Rot2& R) const {
return Rot2(
c_ * R.c_ - s_ * R.s_,
s_ * R.c_ + c_ * R.s_
);
}
/* ************************************************************************* */
Point2 Rot2::operator*(const Point2& p) const {
return Point2(
c_ * p.x() - s_ * p.y(),
s_ * p.x() + c_ * p.y()
);
}
/* ************************************************************************* */
Point2 Rot2::unrotate(const Point2& p) const {
return Point2(
c_ * p.x() + s_ * p.y(),
-s_ * p.x() + c_ * p.y()
);
}
/* ************************************************************************* */

26
cpp/Rot2.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
/** default constructor, zero rotation */
Rot2() : c_(1.0), s_(0.0) {}
/** constructor from angle */
/** constructor from angle == exponential map at identity */
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
/** return angle */
@ -52,34 +52,22 @@ namespace gtsam {
inline Vector vector() const { return Vector_(2,c_,s_);}
/** return 2*2 rotation matrix */
Matrix matrix() const {
double r[] = { c_, -s_, s_, c_ };
return Matrix_(2, 2, r);
}
Matrix matrix() const;
/** return 3*3 transpose (inverse) rotation matrix */
Matrix transpose() const {
double r[] = { c_, s_, -s_, c_ };
return Matrix_(2, 2, r);
}
Matrix transpose() const;
/** inverse transformation */
Rot2 inverse() const { return Rot2(c_, -s_);}
Rot2 inverse() const;
/** composition via sum and difference formulas */
inline Rot2 operator*(const Rot2& R) const {
return Rot2(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
Rot2 operator*(const Rot2& R) const;
/** rotate from rotated to world, syntactic sugar = R*p */
inline Point2 operator*(const Point2& p) const {
return Point2(c_ * p.x() - s_ * p.y(), s_ * p.x() + c_ * p.y());
}
Point2 operator*(const Point2& p) const;
/** rotate from world to rotated = R'*p */
Point2 unrotate(const Point2& p) const {
return Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
}
Point2 unrotate(const Point2& p) const;
/** friends */
friend Matrix Dunrotate1(const Rot2& R, const Point2& p);