diff --git a/timing/timeRot3.cpp b/timing/timeRot3.cpp index 00c02c250..3a6a156d5 100644 --- a/timing/timeRot3.cpp +++ b/timing/timeRot3.cpp @@ -23,33 +23,33 @@ using namespace std; using namespace gtsam; -#define TEST(TITLE,STATEMENT) \ - cout << endl << TITLE << endl;\ - timeLog = clock();\ - for(int i = 0; i < n; i++)\ - STATEMENT;\ - timeLog2 = clock();\ - seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;\ - cout << seconds << " seconds" << endl;\ - cout << ((double)n/seconds) << " calls/second" << endl; +#define TEST(TITLE, STATEMENT) \ + cout << endl << TITLE << endl; \ + timeLog = clock(); \ + for (int i = 0; i < n; i++) STATEMENT; \ + timeLog2 = clock(); \ + seconds = static_cast(timeLog2 - timeLog) / CLOCKS_PER_SEC; \ + cout << 1000 * seconds << " milliseconds" << endl; \ + cout << (1e9 * seconds / static_cast(n)) << " nanosecs/call" << endl; -int main() -{ - int n = 100000; long timeLog, timeLog2; double seconds; +int main() { + int n = 100000; + clock_t timeLog, timeLog2; + double seconds; // 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; + 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).finished(); Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2), R2 = R.retract(v); - TEST("Rodriguez formula given axis angle", Rot3::AxisAngle(v,0.001)) + TEST("Rodriguez formula given axis angle", Rot3::AxisAngle(v, 0.001)) TEST("Rodriguez formula given canonical coordinates", Rot3::Rodrigues(v)) - TEST("Expmap", R*Rot3::Expmap(v)) + TEST("Expmap", R * Rot3::Expmap(v)) TEST("Retract", R.retract(v)) TEST("Logmap", Rot3::Logmap(R.between(R2))) TEST("localCoordinates", R.localCoordinates(R2)) - TEST("Slow rotation matrix",Rot3::Rz(z)*Rot3::Ry(y)*Rot3::Rx(x)) - TEST("Fast Rotation matrix", Rot3::RzRyRx(x,y,z)) + TEST("Slow rotation matrix", Rot3::Rz(z) * Rot3::Ry(y) * Rot3::Rx(x)) + TEST("Fast Rotation matrix", Rot3::RzRyRx(x, y, z)) return 0; }