112 lines
2.6 KiB
C++
112 lines
2.6 KiB
C++
/*
|
|
* Rot2.h
|
|
*
|
|
* Created on: Dec 9, 2009
|
|
* Author: Frank Dellaert
|
|
*/
|
|
|
|
#ifndef ROT2_H_
|
|
#define ROT2_H_
|
|
|
|
#include "Testable.h"
|
|
#include "Point2.h"
|
|
#include "Matrix.h"
|
|
|
|
namespace gtsam {
|
|
|
|
/* Rotation matrix */
|
|
class Rot2: Testable<Rot2> {
|
|
private:
|
|
/** we store cos(theta) and sin(theta) */
|
|
double c_, s_;
|
|
|
|
/** private constructor from cos/sin */
|
|
Rot2(double c, double s) :
|
|
c_(c), s_(s) {
|
|
}
|
|
|
|
public:
|
|
|
|
/** default constructor, zero rotation */
|
|
Rot2() : c_(1.0), s_(0.0) {}
|
|
|
|
/** constructor from angle == exponential map at identity */
|
|
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
|
|
|
|
/** return angle */
|
|
inline double angle() const { return atan2(s_, c_); }
|
|
|
|
/** print */
|
|
void print(const std::string& s = "theta") const;
|
|
|
|
/** equals with an tolerance */
|
|
bool equals(const Rot2& R, double tol = 1e-9) const;
|
|
|
|
/** return DOF, dimensionality of tangent space */
|
|
inline size_t dim() const { return 1;}
|
|
|
|
/** Given 1-dim tangent vector, create new rotation */
|
|
Rot2 exmap(const Vector& d) const;
|
|
|
|
/** return vectorized form (column-wise)*/
|
|
inline Vector vector() const { return Vector_(2,c_,s_);}
|
|
|
|
/** return 2*2 rotation matrix */
|
|
Matrix matrix() const;
|
|
|
|
/** return 3*3 transpose (inverse) rotation matrix */
|
|
Matrix transpose() const;
|
|
|
|
/** inverse transformation */
|
|
Rot2 inverse() const;
|
|
|
|
/** composition via sum and difference formulas */
|
|
Rot2 operator*(const Rot2& R) const;
|
|
|
|
/** rotate from rotated to world, syntactic sugar = R*p */
|
|
Point2 operator*(const Point2& p) const;
|
|
|
|
/** rotate from world to rotated = R'*p */
|
|
Point2 unrotate(const Point2& p) const;
|
|
|
|
/** friends */
|
|
friend Matrix Dunrotate1(const Rot2& R, const Point2& p);
|
|
|
|
private:
|
|
/** Serialization function */
|
|
friend class boost::serialization::access;
|
|
template<class Archive>
|
|
void serialize(Archive & ar, const unsigned int version) {
|
|
ar & BOOST_SERIALIZATION_NVP(c_);
|
|
ar & BOOST_SERIALIZATION_NVP(s_);
|
|
}
|
|
};
|
|
|
|
/**
|
|
* Update Rotation with incremental rotation
|
|
* @param v a vector of incremental angle
|
|
* @param R a 2D rotation
|
|
* @return incremental rotation matrix
|
|
*/
|
|
Rot2 exmap(const Rot2& R, const Vector& v);
|
|
|
|
/**
|
|
* rotate point from rotated coordinate frame to
|
|
* world = R*p
|
|
*/
|
|
Point2 rotate(const Rot2& R, const Point2& p);
|
|
Matrix Drotate1(const Rot2& R, const Point2& p);
|
|
Matrix Drotate2(const Rot2& R); // does not depend on p !
|
|
|
|
/**
|
|
* rotate point from world to rotated
|
|
* frame = R'*p
|
|
*/
|
|
Point2 unrotate(const Rot2& R, const Point2& p);
|
|
Matrix Dunrotate1(const Rot2& R, const Point2& p);
|
|
Matrix Dunrotate2(const Rot2& R); // does not depend on p !
|
|
|
|
} // gtsam
|
|
|
|
#endif /* ROT2_H_ */
|