iSAM
util.cpp
Go to the documentation of this file.
00001 
00028 #include <map>
00029 #include <sys/time.h>
00030 #include <cmath>
00031 #include <algorithm> // abs
00032 
00033 #include "isam/util.h"
00034 
00035 using namespace std;
00036 
00037 namespace isam {
00038 
00039 // simple class for accumulating execution timing information by name
00040 class Timing {
00041   class Stats {
00042   public:
00043     double t0;
00044     double t;
00045     double t_max;
00046     double t_min;
00047     int n;
00048   };
00049   map<string, Stats> stats;
00050 public:
00051   void add_t0(string id, double t0) {
00052     stats[id].t0 = t0;
00053   }
00054   double get_t0(string id) {
00055     return stats[id].t0;
00056   }
00057   void add_dt(string id, double dt) {
00058     Stats& s = stats[id];
00059     s.t += dt;
00060     s.n++;
00061     if (s.n==1 || s.t_max < dt) s.t_max = dt;
00062     if (s.n==1 || s.t_min > dt) s.t_min = dt;
00063   }
00064   void print() {
00065     map<string, Stats>::iterator it;
00066     for(it = stats.begin(); it!=stats.end(); it++) {
00067       Stats& s = it->second;
00068       printf("%s: %g (%i times, min: %g, max: %g)\n",
00069              it->first.c_str(), s.t, s.n, s.t_min, s.t_max);
00070     }
00071   }
00072   double time(string id) {
00073     Stats& s = stats[id];
00074     return s.t;
00075   }
00076 };
00077 Timing timing;
00078 
00079 double tic() {
00080   struct timeval t;
00081   gettimeofday(&t, NULL);
00082   return ((double)t.tv_sec + ((double)t.tv_usec)/1000000.);
00083 }
00084 
00085 double tic(string id) {
00086   double t0 = tic();
00087   timing.add_t0(id, t0);
00088   return t0;
00089 }
00090 
00091 double toc(double t) {
00092   double s = tic();
00093   return (max(0., s-t));
00094 }
00095 
00096 double toc(string id) {
00097   double dt = toc(timing.get_t0(id));
00098   timing.add_dt(id, dt);
00099   return dt;
00100 }
00101 
00102 void tictoc_print() {
00103   timing.print();
00104 }
00105 
00106 double tictoc(string id) {
00107   return (timing.time(id));
00108 }
00109 
00110 Eigen::MatrixXd eye(int num) {
00111   return Eigen::MatrixXd::Identity(num, num);
00112 }
00113 
00114 void givens(const double a, const double b, double& c, double& s) {
00115   if (b==0) {
00116     c = 1.0;
00117     s = 0.0;
00118   } else if (fabs(b)>fabs(a)) {
00119     double t = -a/b;
00120     s = 1/sqrt(1+t*t);
00121     c = t*s;
00122   } else {
00123     double t = -b/a;
00124     c = 1/sqrt(1+t*t);
00125     s = t*c;
00126   }
00127 }
00128 
00129 }
 All Classes Files Functions Variables