00001
00002 #include <exception>
00003 #include <stdexcept>
00004 #include <stdio.h>
00005 #include <sys/time.h>
00006 #include <iostream>
00007 #include <unistd.h>
00008 #include <signal.h>
00009 #include "trackerd_orientation.hh"
00010
00011 using namespace std;
00012
00013 double sum = 0.0;
00014 int i = 0;
00015
00016 void quit(int sig) {
00017 printf("average time = %.10f ms (n = %d)\n", sum/(i-1), i-1);
00018 exit(0);
00019 }
00020
00021
00022 int main(int argc, char** argv) {
00023 signal(SIGINT, quit);
00024 signal(SIGKILL, quit);
00025 try {
00026 TrackerdOrientation o;
00027 double y, p, r;
00028 double m[3][3];
00029 struct timeval tv;
00030 double before, after;
00031 sum = 0.0;
00032 for(i = 1; i <= 10000; i++) {
00033 gettimeofday(&tv, NULL);
00034 before = (double)tv.tv_sec + (tv.tv_usec / 1000000.0);
00035 o.getEulerAngles(&y, &p, &r);
00036 printf("y = %.8f, p = %.8f, r = %.8f\n", y, p, r);
00037 o.getOrientationMatrix(m);
00038 printf("m = [%f %f %f]\n"
00039 " [%f %f %f]\n"
00040 " [%f %f %f]\n",
00041 m[0][0], m[0][1], m[0][2],
00042 m[1][0], m[1][1], m[1][2],
00043 m[2][0], m[2][1], m[2][2]);
00044 printf("------------------------\n");
00045 gettimeofday(&tv, NULL);
00046 after = (double)tv.tv_sec + (tv.tv_usec / 1000000.0);
00047 sum += after - before;
00048 sleep(0);
00049 }
00050 quit(0);
00051 } catch(exception& e) {
00052 cerr << "Error: " << e.what();
00053 }
00054 }
