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

Added more rodriguez variants -- although some should be deprecated (e.g., Point and Vector versions, for which it is possible to give incorrect input) 

git-svn-id: https://svn.cc.gatech.edu/borg/gtsam/trunk@20419 898a188c-9671-0410-8e00-e3fd810bbb7f
release/4.3a0
Frank Dellaert 2013-12-21 04:13:58 +00:00 committed by Richard Roberts
parent dd447f2c6c
commit 9636ace3bb
2 changed files with 30 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Rot3.cpp
View File

@ -33,6 +33,16 @@ void Rot3::print(const std::string& s) const {
gtsam::print((Matrix)matrix(), s);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Point3& w, double theta) {
return rodriguez((Vector)w.vector(),theta);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Sphere2& w, double theta) {
return rodriguez(w.point3(),theta);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w) {
double t = w.norm();

21
gtsam/geometry/Rot3.h
View File

@ -150,7 +150,10 @@ namespace gtsam {
static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);}
/** Create from Quaternion coefficients */
static Rot3 quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3(q); }
static Rot3 quaternion(double w, double x, double y, double z) {
Quaternion q(w, x, y, z);
return Rot3(q);
}
/**
* Rodriguez' formula to compute an incremental rotation matrix
@ -160,6 +163,22 @@ namespace gtsam {
*/
static Rot3 rodriguez(const Vector& w, double theta);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param w is the rotation axis, unit length
* @param theta rotation angle
* @return incremental rotation matrix
*/
static Rot3 rodriguez(const Point3& w, double theta);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param w is the rotation axis
* @param theta rotation angle
* @return incremental rotation matrix
*/
static Rot3 rodriguez(const Sphere2& w, double theta);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param v a vector of incremental roll,pitch,yaw