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

Moved all Rot3 function implementations to cpp files instead of header files

release/4.3a0
Richard Roberts 2012-01-01 23:46:34 +00:00
parent 171886cef9
commit c28bc7b06e
4 changed files with 228 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
gtsam/geometry/Rot3M.cpp
View File

@ -30,6 +30,41 @@ INSTANTIATE_LIE(Rot3M);
static const Matrix I3 = eye(3);
/* ************************************************************************* */
Rot3M::Rot3M() :
r1_(Point3(1.0,0.0,0.0)),
r2_(Point3(0.0,1.0,0.0)),
r3_(Point3(0.0,0.0,1.0)) {}
/* ************************************************************************* */
Rot3M::Rot3M(const Point3& r1, const Point3& r2, const Point3& r3) :
r1_(r1), r2_(r2), r3_(r3) {}
/* ************************************************************************* */
Rot3M::Rot3M(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
r1_(Point3(R11, R21, R31)),
r2_(Point3(R12, R22, R32)),
r3_(Point3(R13, R23, R33)) {}
/* ************************************************************************* */
Rot3M::Rot3M(const Matrix& R):
r1_(Point3(R(0,0), R(1,0), R(2,0))),
r2_(Point3(R(0,1), R(1,1), R(2,1))),
r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
/* ************************************************************************* */
Rot3M::Rot3M(const Quaternion& q) {
Eigen::Matrix3d R = q.toRotationMatrix();
r1_ = Point3(R.col(0));
r2_ = Point3(R.col(1));
r3_ = Point3(R.col(2));
}
/* ************************************************************************* */
Rot3M::Rot3M(const Rot3M& r) : r1_(r.r1_), r2_(r.r2_), r3_(r.r3_) {}
/* ************************************************************************* */
Rot3M Rot3M::Rx(double t) {
double st = sin(t), ct = cos(t);
@ -140,12 +175,23 @@ Matrix Rot3M::transpose() const {
Point3 Rot3M::column(int index) const{
if(index == 3)
return r3_;
else if (index == 2)
else if(index == 2)
return r2_;
else
else if(index == 1)
return r1_; // default returns r1
else
throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
}
/* ************************************************************************* */
Point3 Rot3M::r1() const { return r1_; }
/* ************************************************************************* */
Point3 Rot3M::r2() const { return r2_; }
/* ************************************************************************* */
Point3 Rot3M::r3() const { return r3_; }
/* ************************************************************************* */
Vector Rot3M::xyz() const {
Matrix I;Vector q;
@ -226,6 +272,26 @@ Rot3M Rot3M::compose (const Rot3M& R2,
return *this * R2;
}
/* ************************************************************************* */
Point3 Rot3M::operator*(const Point3& p) const { return rotate(p); }
/* ************************************************************************* */
Rot3M Rot3M::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -matrix();
return Rot3M(
r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z());
}
/* ************************************************************************* */
Quaternion Rot3M::toQuaternion() const {
return Quaternion((Eigen::Matrix3d() <<
r1_.x(), r2_.x(), r3_.x(),
r1_.y(), r2_.y(), r3_.y(),
r1_.z(), r2_.z(), r3_.z()).finished());
}
/* ************************************************************************* */
Rot3M Rot3M::between (const Rot3M& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@ -234,6 +300,11 @@ Rot3M Rot3M::between (const Rot3M& R2,
return between_default(*this, R2);
}
/* ************************************************************************* */
Rot3M Rot3M::operator*(const Rot3M& R2) const {
return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
}
/* ************************************************************************* */
pair<Matrix, Vector> RQ(const Matrix& A) {

91
gtsam/geometry/Rot3M.h
View File

@ -26,11 +26,15 @@
namespace gtsam {
typedef Eigen::Quaternion<double,Eigen::DontAlign> Quaternion; ///< Typedef to an Eigen Quaternion<double>
/// Typedef to an Eigen Quaternion<double>, we disable alignment because
/// geometry objects are stored in boost pool allocators, Values containers,
/// and and these pool allocators do not support alignment.
typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
/**
* @brief 3D Rotations represented as rotation matrices
* @ingroup geometry
* \nosubgrouping
*/
class Rot3M {
public:
@ -42,11 +46,11 @@ namespace gtsam {
public:
/// @name Constructors and named constructors
/// @{
/** default constructor, unit rotation */
Rot3M() :
r1_(Point3(1.0,0.0,0.0)),
r2_(Point3(0.0,1.0,0.0)),
r3_(Point3(0.0,0.0,1.0)) {}
Rot3M();
/**
* Constructor from columns
@ -54,35 +58,26 @@ namespace gtsam {
* @param r2 Y-axis of rotated frame
* @param r3 Z-axis of rotated frame
*/
Rot3M(const Point3& r1, const Point3& r2, const Point3& r3) :
r1_(r1), r2_(r2), r3_(r3) {}
Rot3M(const Point3& r1, const Point3& r2, const Point3& r3);
/** constructor from doubles in *row* order !!! */
/** constructor from a rotation matrix, as doubles in *row-major* order !!! */
Rot3M(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
r1_(Point3(R11, R21, R31)),
r2_(Point3(R12, R22, R32)),
r3_(Point3(R13, R23, R33)) {}
double R31, double R32, double R33);
/** constructor from matrix */
Rot3M(const Matrix& R):
r1_(Point3(R(0,0), R(1,0), R(2,0))),
r2_(Point3(R(0,1), R(1,1), R(2,1))),
r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
/** constructor from a rotation matrix */
Rot3M(const Matrix& R);
/** Constructor from a quaternion. This can also be called using a plain
* Vector, due to implicit conversion from Vector to Quaternion
* @param q The quaternion
*/
Rot3M(const Quaternion& q) {
Eigen::Matrix3d R = q.toRotationMatrix();
r1_ = Point3(R.col(0));
r2_ = Point3(R.col(1));
r3_ = Point3(R.col(2));
}
Rot3M(const Quaternion& q);
/** Constructor from a rotation matrix in a Rot3M */
Rot3M(const Rot3M& r);
/** Static member function to generate some well known rotations */
/* Static member function to generate some well known rotations */
/**
* Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix
@ -102,14 +97,14 @@ namespace gtsam {
* as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf
* Assumes vehicle coordinate frame X forward, Y right, Z down
*/
static Rot3M yaw (double t) { return Rz(t);} // positive yaw is to right (as in aircraft heading)
static Rot3M pitch(double t) { return Ry(t);} // positive pitch is up (increasing aircraft altitude)
static Rot3M roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft)
static Rot3M yaw (double t) { return Rz(t); } // positive yaw is to right (as in aircraft heading)
static Rot3M pitch(double t) { return Ry(t); } // positive pitch is up (increasing aircraft altitude)
static Rot3M roll (double t) { return Rx(t); } // positive roll is to right (increasing yaw in aircraft)
/// Returns rotation matrix nRb from body to nav frame
static Rot3M ypr (double y, double p, double r) { return RzRyRx(r,p,y);}
/** Create from Quaternion parameters */
/** Create from Quaternion coefficients */
static Rot3M quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3M(q); }
/**
@ -129,14 +124,15 @@ namespace gtsam {
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param wx
* @param wy
* @param wz
* @param wx Incremental roll (about X)
* @param wy Incremental pitch (about Y)
* @param wz Incremental yaw (about Z)
* @return incremental rotation matrix
*/
static Rot3M rodriguez(double wx, double wy, double wz)
{ return rodriguez(Vector_(3,wx,wy,wz));}
/// @}
/// @name Testable
/// @{
@ -160,16 +156,10 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/// rotate point from rotated coordinate frame to world = R*p
inline Point3 operator*(const Point3& p) const { return rotate(p);}
Point3 operator*(const Point3& p) const;
/// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3M()
Rot3M inverse(boost::optional<Matrix&> H1=boost::none) const {
if (H1) *H1 = -matrix();
return Rot3M(
r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z());
}
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
Rot3M inverse(boost::optional<Matrix&> H1=boost::none) const;
/// @}
/// @name Manifold
@ -181,7 +171,7 @@ namespace gtsam {
/// return dimensionality of tangent space, DOF = 3
size_t dim() const { return dimension; }
/// Updates a with tangent space delta
/// Retraction from R^3 to Pose2 manifold neighborhood around current pose
Rot3M retract(const Vector& v) const { return compose(Expmap(v)); }
/// Returns inverse retraction
@ -203,7 +193,7 @@ namespace gtsam {
/**
* Log map at identity - return the canonical coordinates of this rotation
*/
static Vector Logmap(const Rot3M& R);
static Vector Logmap(const Rot3M& R);
/// @}
@ -215,9 +205,9 @@ namespace gtsam {
/** returns column vector specified by index */
Point3 column(int index) const;
Point3 r1() const { return r1_; }
Point3 r2() const { return r2_; }
Point3 r3() const { return r3_; }
Point3 r1() const;
Point3 r2() const;
Point3 r3() const;
/**
* Use RQ to calculate xyz angle representation
@ -250,12 +240,7 @@ namespace gtsam {
/** Compute the quaternion representation of this rotation.
* @return The quaternion
*/
Quaternion toQuaternion() const {
return Quaternion((Eigen::Matrix3d() <<
r1_.x(), r2_.x(), r3_.x(),
r1_.y(), r2_.y(), r3_.y(),
r1_.z(), r2_.z(), r3_.z()).finished());
}
Quaternion toQuaternion() const;
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
@ -265,9 +250,7 @@ namespace gtsam {
boost::optional<Matrix&> H2=boost::none) const;
/** compose two rotations */
Rot3M operator*(const Rot3M& R2) const {
return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
}
Rot3M operator*(const Rot3M& R2) const;
/**
* rotate point from rotated coordinate frame to

81
gtsam/geometry/Rot3Q.cpp
View File

@ -31,8 +31,44 @@ namespace gtsam {
static const Matrix I3 = eye(3);
/* ************************************************************************* */
// static member functions to construct rotations
Rot3Q::Rot3Q() : quaternion_(Quaternion::Identity()) {}
/* ************************************************************************* */
Rot3Q::Rot3Q(const Point3& r1, const Point3& r2, const Point3& r3) :
quaternion_((Eigen::Matrix3d() <<
r1.x(), r2.x(), r3.x(),
r1.y(), r2.y(), r3.y(),
r1.z(), r2.z(), r3.z()).finished()) {}
/* ************************************************************************* */
Rot3Q::Rot3Q(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
quaternion_((Eigen::Matrix3d() <<
R11, R12, R13,
R21, R22, R23,
R31, R32, R33).finished()) {}
/* ************************************************************************* */
Rot3Q::Rot3Q(const Matrix& R) :
quaternion_(Eigen::Matrix3d(R)) {}
/* ************************************************************************* */
Rot3Q::Rot3Q(const Quaternion& q) : quaternion_(q) {}
/* ************************************************************************* */
Rot3Q::Rot3Q(const Rot3M& r) : quaternion_(Eigen::Matrix3d(r.matrix())) {}
/* ************************************************************************* */
Rot3Q Rot3Q::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }
/* ************************************************************************* */
Rot3Q Rot3Q::Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); }
/* ************************************************************************* */
Rot3Q Rot3Q::Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); }
/* ************************************************************************* */
Rot3Q Rot3Q::RzRyRx(double x, double y, double z) { return Rot3Q(
Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
@ -61,6 +97,27 @@ namespace gtsam {
/* ************************************************************************* */
Matrix Rot3Q::transpose() const { return quaternion_.toRotationMatrix().transpose(); }
/* ************************************************************************* */
Point3 Rot3Q::column(int index) const{
if(index == 3)
return r3();
else if(index == 2)
return r2();
else if(index == 1)
return r1(); // default returns r1
else
throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
}
/* ************************************************************************* */
Point3 Rot3Q::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }
/* ************************************************************************* */
Point3 Rot3Q::r2() const { return Point3(quaternion_.toRotationMatrix().col(1)); }
/* ************************************************************************* */
Point3 Rot3Q::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }
/* ************************************************************************* */
Vector Rot3Q::xyz() const {
Matrix I;Vector q;
@ -68,11 +125,13 @@ namespace gtsam {
return q;
}
/* ************************************************************************* */
Vector Rot3Q::ypr() const {
Vector q = xyz();
return Vector_(3,q(2),q(1),q(0));
}
/* ************************************************************************* */
Vector Rot3Q::rpy() const {
Vector q = xyz();
return Vector_(3,q(0),q(1),q(2));
@ -118,6 +177,21 @@ namespace gtsam {
return Rot3Q(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
Point3 Rot3Q::operator*(const Point3& p) const {
Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z());
return Point3(r(0), r(1), r(2));
}
/* ************************************************************************* */
Rot3Q Rot3Q::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -matrix();
return Rot3Q(quaternion_.inverse());
}
/* ************************************************************************* */
Quaternion Rot3Q::toQuaternion() const { return quaternion_; }
/* ************************************************************************* */
Rot3Q Rot3Q::between(const Rot3Q& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@ -127,5 +201,10 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot3Q Rot3Q::operator*(const Rot3Q& R2) const {
return Rot3Q(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
} // namespace gtsam

74
gtsam/geometry/Rot3Q.h
View File

@ -49,35 +49,32 @@ namespace gtsam {
/// @{
/** default constructor, unit rotation */
Rot3Q() : quaternion_(Quaternion::Identity()) {}
Rot3Q();
/** constructor from columns */
Rot3Q(const Point3& r1, const Point3& r2, const Point3& r3) :
quaternion_((Eigen::Matrix3d() <<
r1.x(), r2.x(), r3.x(),
r1.y(), r2.y(), r3.y(),
r1.z(), r2.z(), r3.z()).finished()) {}
/**
* Constructor from columns
* @param r1 X-axis of rotated frame
* @param r2 Y-axis of rotated frame
* @param r3 Z-axis of rotated frame
*/
Rot3Q(const Point3& r1, const Point3& r2, const Point3& r3);
/** constructor from a rotation matrix, as doubles in *row-major* order !!! */
Rot3Q(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
quaternion_((Eigen::Matrix3d() <<
R11, R12, R13,
R21, R22, R23,
R31, R32, R33).finished()) {}
double R31, double R32, double R33);
/** constructor from a rotation matrix */
Rot3Q(const Matrix& R) : quaternion_(Eigen::Matrix3d(R)) {}
Rot3Q(const Matrix& R);
/** Constructor from a quaternion. This can also be called using a plain
* Vector, due to implicit conversion from Vector to Quaternion
* @param q The quaternion
*/
Rot3Q(const Quaternion& q) : quaternion_(q) {}
Rot3Q(const Quaternion& q);
/** Constructor from a rotation matrix in a Rot3M */
Rot3Q(const Rot3M& r) : quaternion_(Eigen::Matrix3d(r.matrix())) {}
Rot3Q(const Rot3M& r);
/* Static member function to generate some well known rotations */
@ -85,9 +82,9 @@ namespace gtsam {
* Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix
* Counterclockwise when looking from unchanging axis.
*/
static Rot3Q Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }
static Rot3Q Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); }
static Rot3Q Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); }
static Rot3Q Rx(double t);
static Rot3Q Ry(double t);
static Rot3Q Rz(double t);
static Rot3Q RzRyRx(double x, double y, double z);
inline static Rot3Q RzRyRx(const Vector& xyz) {
assert(xyz.size() == 3);
@ -102,6 +99,8 @@ namespace gtsam {
static Rot3Q yaw (double t) { return Rz(t); } // positive yaw is to right (as in aircraft heading)
static Rot3Q pitch(double t) { return Ry(t); } // positive pitch is up (increasing aircraft altitude)
static Rot3Q roll (double t) { return Rx(t); } // positive roll is to right (increasing yaw in aircraft)
/// Returns rotation matrix nRb from body to nav frame
static Rot3Q ypr (double y, double p, double r) { return RzRyRx(r,p,y);}
/** Create from Quaternion coefficients */
@ -151,21 +150,15 @@ namespace gtsam {
return Rot3Q();
}
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
Rot3Q inverse(boost::optional<Matrix&> H1=boost::none) const {
if (H1) *H1 = -matrix();
return Rot3Q(quaternion_.inverse());
}
/// Compose two rotations i.e., R= (*this) * R2
Rot3Q compose(const Rot3Q& R2,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/// rotate point from rotated coordinate frame to world = R*p
inline Point3 operator*(const Point3& p) const {
Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z());
return Point3(r(0), r(1), r(2));
}
Point3 operator*(const Point3& p) const;
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
Rot3Q inverse(boost::optional<Matrix&> H1=boost::none) const;
/// @}
/// @name Manifold
@ -210,9 +203,10 @@ namespace gtsam {
Matrix transpose() const;
/** returns column vector specified by index */
Point3 r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }
Point3 r2() const { return Point3(quaternion_.toRotationMatrix().col(1)); }
Point3 r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }
Point3 column(int index) const;
Point3 r1() const;
Point3 r2() const;
Point3 r3() const;
/**
* Use RQ to calculate xyz angle representation
@ -232,12 +226,20 @@ namespace gtsam {
*/
Vector rpy() const;
/** Compute the quaternion representation of this rotation - this is to
* maintain a standard Rot3 API with Rot3M, but here just returns the
* internal quaternion.
/**
* Accessors to get to components of angle representations
* NOTE: these are not efficient to get to multiple separate parts,
* you should instead use xyz() or ypr()
* TODO: make this more efficient
*/
inline double roll() const { return ypr()(2); }
inline double pitch() const { return ypr()(1); }
inline double yaw() const { return ypr()(0); }
/** Compute the quaternion representation of this rotation.
* @return The quaternion
*/
Quaternion toQuaternion() const { return quaternion_; }
Quaternion toQuaternion() const;
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
@ -247,7 +249,7 @@ namespace gtsam {
boost::optional<Matrix&> H2=boost::none) const;
/** compose two rotations */
Rot3Q operator*(const Rot3Q& R2) const { return Rot3Q(quaternion_ * R2.quaternion_); }
Rot3Q operator*(const Rot3Q& R2) const;
/**
* rotate point from rotated coordinate frame to