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