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

Changed timing script to use random direction, properly normalized (or rodrigues crashes)

release/4.3a0
Frank Dellaert 2012-01-07 21:09:53 +00:00
parent 1813942b6b
commit 3100ce9f1d
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/geometry/tests/timeRot3.cpp
View File

@ -24,7 +24,7 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
#define TEST(TITLE,STATEMENT) \ #define TEST(TITLE,STATEMENT) \
cout << TITLE << endl;\ cout << endl << TITLE << endl;\
timeLog = clock();\ timeLog = clock();\
for(int i = 0; i < n; i++)\ for(int i = 0; i < n; i++)\
STATEMENT;\ STATEMENT;\
@ -35,18 +35,21 @@ using namespace gtsam;
int main() int main()
{ {
int n = 300000; long timeLog, timeLog2; double seconds; int n = 100000; long timeLog, timeLog2; double seconds;
Vector v = Vector_(3,1.0,0.0,0.0);//0.1,0.2,-0.1); // create a random direction:
double norm=sqrt(1.0+16.0+4.0);
double x=1.0/norm, y=4.0/norm, z=2.0/norm;
Vector v = Vector_(3,x,y,z);
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2), R2 = R.retract(v); Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2), R2 = R.retract(v);
TEST("Rodriguez formula given axis angle", Rot3::rodriguez(v,0.001)) TEST("Rodriguez formula given axis angle", Rot3::rodriguez(v,0.001))
TEST("Rodriguez formula given canonical coordinates", Rot3::rodriguez(v)) TEST("Rodriguez formula given canonical coordinates", Rot3::rodriguez(v))
TEST("Expmap", R*Rot3::Expmap(v)) TEST("Expmap", R*Rot3::Expmap(v))
TEST("Logmap", Rot3::Logmap(R))
TEST("Retract", R.retract(v)) TEST("Retract", R.retract(v))
TEST("Logmap", Rot3::Logmap(R.between(R2)))
TEST("localCoordinates", R.localCoordinates(R2)) TEST("localCoordinates", R.localCoordinates(R2))
TEST("Slow rotation matrix",Rot3::Rz(0.3)*Rot3::Ry(0.2)*Rot3::Rx(0.1)) TEST("Slow rotation matrix",Rot3::Rz(z)*Rot3::Ry(y)*Rot3::Rx(x))
TEST("Fast Rotation matrix", Rot3::RzRyRx(0.1,0.2,0.3)) TEST("Fast Rotation matrix", Rot3::RzRyRx(x,y,z))
return 0; return 0;
} }