Much faster compund rotation using Justin's (indeed correct) formula
Frank Dellaert
parent
d5eade62ef
commit
30367e35fb
23
cpp/Rot3.cpp
23
cpp/Rot3.cpp
|
|
@ -43,6 +43,29 @@ namespace gtsam {
|
|||
0, 0, 1);
|
||||
}
|
||||
|
||||
// Considerably faster than composing matrices above !
|
||||
Rot3 Rot3::RzRyRx(double x, double y, double z) {
|
||||
double cx=cos(x),sx=sin(x);
|
||||
double cy=cos(y),sy=sin(y);
|
||||
double cz=cos(z),sz=sin(z);
|
||||
double ss_ = sx * sy;
|
||||
double cs_ = cx * sy;
|
||||
double sc_ = sx * cy;
|
||||
double cc_ = cx * cy;
|
||||
double c_s = cx * sz;
|
||||
double s_s = sx * sz;
|
||||
double _cs = cy * sz;
|
||||
double _cc = cy * cz;
|
||||
double s_c = sx * cz;
|
||||
double c_c = cx * cz;
|
||||
double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
|
||||
return Rot3(
|
||||
_cc,- c_s + ssc, s_s + csc,
|
||||
_cs, c_c + sss, -s_c + css,
|
||||
-sy, sc_, cc_
|
||||
);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Rot3::equals(const Rot3 & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
|
|
|
|||
22
cpp/Rot3.h
22
cpp/Rot3.h
|
|
@ -63,19 +63,17 @@ namespace gtsam {
|
|||
static Rot3 Rx(double t);
|
||||
static Rot3 Ry(double t);
|
||||
static Rot3 Rz(double t);
|
||||
static Rot3 RzRyRx(double x, double y, double z) { return Rz(z)*Ry(y)*Rx(x);}
|
||||
static Rot3 RzRyRx(double x, double y, double z);
|
||||
|
||||
/**
|
||||
* Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw)
|
||||
* as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf
|
||||
* Differs from one defined by Justin, who uses, (x,y,z) = (roll,-pitch,yaw)
|
||||
* and http://en.wikipedia.org/wiki/Yaw,_pitch,_and_roll, where (x,y,z) = (-roll,pitch,yaw)
|
||||
* Assumes vehicle coordinate frame X forward, Y right, Z down
|
||||
*/
|
||||
static Rot3 yaw (double t) { return Rz(t);} // positive yaw is to right (as in aircraft heading)
|
||||
static Rot3 pitch(double t) { return Ry(t);} // positive pitch is up (increasing aircraft altitude)
|
||||
static Rot3 roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft)
|
||||
static Rot3 ypr (double y, double p, double r) { return yaw(y)*pitch(p)*roll(r);}
|
||||
static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);}
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
|
||||
|
|
@ -178,22 +176,6 @@ namespace gtsam {
|
|||
R.r3().x(), R.r3().y(), R.r3().z());
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
* Update Rotation with incremental rotation
|
||||
* @param v a vector of incremental roll,pitch,yaw
|
||||
* @param R a rotated frame
|
||||
* @return incremental rotation matrix
|
||||
*/
|
||||
//Rot3 exp(const Rot3& R, const Vector& v);
|
||||
|
||||
/**
|
||||
* @param a rotation R
|
||||
* @param a rotation S
|
||||
* @return log(S*R'), i.e. canonical coordinates of between(R,S)
|
||||
*/
|
||||
//Vector log(const Rot3& R, const Rot3& S);
|
||||
|
||||
/**
|
||||
* rotate point from rotated coordinate frame to
|
||||
* world = R*p
|
||||
|
|
|
|||
|
|
@ -129,12 +129,28 @@ TEST(Rot3, log)
|
|||
Rot3 t1 = rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 t2 = rodriguez(0.3, 0.1, 0.7);
|
||||
Rot3 origin;
|
||||
|
||||
// log behaves correctly
|
||||
Vector d12 = logmap(t1, t2);
|
||||
CHECK(assert_equal(t2, expmap(t1,d12)));
|
||||
CHECK(assert_equal(t2, expmap<Rot3>(d12)*t1));
|
||||
Vector d21 = logmap(t2, t1);
|
||||
CHECK(assert_equal(t1, expmap(t2,d21)));
|
||||
CHECK(assert_equal(t1, expmap<Rot3>(d21)*t2));
|
||||
|
||||
// Check that log(t1,t2)=-log(t2,t1)
|
||||
CHECK(assert_equal(d12,-d21));
|
||||
|
||||
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
|
||||
Vector d = Vector_(3,0.1,0.2,0.3);
|
||||
// exp(-d)=inverse(exp(d))
|
||||
CHECK(assert_equal(expmap<Rot3>(-d),inverse(expmap<Rot3>(d))));
|
||||
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
|
||||
Rot3 R2 = expmap<Rot3>(2*d);
|
||||
Rot3 R3 = expmap<Rot3>(3*d);
|
||||
Rot3 R5 = expmap<Rot3>(5*d);
|
||||
CHECK(assert_equal(R5,R2*R3));
|
||||
CHECK(assert_equal(R5,R3*R2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -233,15 +249,39 @@ TEST( Rot3, xyz )
|
|||
{
|
||||
double t = 0.1, st = sin(t), ct = cos(t);
|
||||
|
||||
Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
|
||||
// Make sure all counterclockwise
|
||||
// Diagrams below are all from from unchanging axis
|
||||
|
||||
// z
|
||||
// | * Y=(ct,st)
|
||||
// x----y
|
||||
Rot3 expected1(
|
||||
1, 0, 0,
|
||||
0, ct,-st,
|
||||
0, st, ct);
|
||||
CHECK(assert_equal(expected1,Rot3::Rx(t)));
|
||||
|
||||
Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
|
||||
// x
|
||||
// | * Z=(ct,st)
|
||||
// y----z
|
||||
Rot3 expected2(
|
||||
ct, 0, st,
|
||||
0, 1, 0,
|
||||
-st, 0, ct);
|
||||
CHECK(assert_equal(expected2,Rot3::Ry(t)));
|
||||
|
||||
// yaw is around z axis
|
||||
Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
|
||||
// y
|
||||
// | X=* (ct,st)
|
||||
// z----x
|
||||
Rot3 expected3(
|
||||
ct, -st, 0,
|
||||
st, ct, 0,
|
||||
0, 0, 1);
|
||||
CHECK(assert_equal(expected3,Rot3::Rz(t)));
|
||||
|
||||
// Check compound rotation
|
||||
Rot3 expected = Rot3::Rz(0.3)*Rot3::Ry(0.2)*Rot3::Rx(0.1);
|
||||
CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -257,6 +297,10 @@ TEST( Rot3, yaw_pitch_roll )
|
|||
|
||||
// roll is around x axis
|
||||
CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t)));
|
||||
|
||||
// Check compound rotation
|
||||
Rot3 expected = Rot3::yaw(0.1)*Rot3::pitch(0.2)*Rot3::roll(0.3);
|
||||
CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -14,9 +14,10 @@ using namespace gtsam;
|
|||
|
||||
int main()
|
||||
{
|
||||
int n = 3000000;
|
||||
int n = 300000;
|
||||
Vector v = Vector_(3,1.,0.,0.);
|
||||
|
||||
// Rodriguez formula given axis angle
|
||||
long timeLog = clock();
|
||||
for(int i = 0; i < n; i++)
|
||||
rodriguez(v,0.001);
|
||||
|
|
@ -25,6 +26,7 @@ int main()
|
|||
cout << seconds << " seconds" << endl;
|
||||
cout << ((double)n/seconds) << " calls/second" << endl;
|
||||
|
||||
// Rodriguez formula given canonical coordinates
|
||||
timeLog = clock();
|
||||
for(int i = 0; i < n; i++)
|
||||
rodriguez(v);
|
||||
|
|
@ -33,5 +35,23 @@ int main()
|
|||
cout << seconds << " seconds" << endl;
|
||||
cout << ((double)n/seconds) << " calls/second" << endl;
|
||||
|
||||
// Slow rotation matrix
|
||||
timeLog = clock();
|
||||
for(int i = 0; i < n; i++)
|
||||
Rot3::Rz(0.3)*Rot3::Ry(0.2)*Rot3::Rx(0.1);
|
||||
timeLog2 = clock();
|
||||
seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
|
||||
cout << seconds << " seconds" << endl;
|
||||
cout << ((double)n/seconds) << " calls/second" << endl;
|
||||
|
||||
// Fast Rotation matrix
|
||||
timeLog = clock();
|
||||
for(int i = 0; i < n; i++)
|
||||
Rot3::RzRyRx(0.1,0.2,0.3);
|
||||
timeLog2 = clock();
|
||||
seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
|
||||
cout << seconds << " seconds" << endl;
|
||||
cout << ((double)n/seconds) << " calls/second" << endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue