iSAM
util.h
Go to the documentation of this file.
00001 
00028 #pragma once
00029 
00030 #include <stdlib.h>
00031 #include <stdio.h>
00032 #include <cmath>
00033 #include <string>
00034 #include <Eigen/Dense>
00035 
00036 namespace isam {
00037 
00038 // some math constants
00039 #ifndef PI
00040 const double PI = 3.14159265358979323846;
00041 #endif
00042 const double TWOPI = 2.0*PI;
00043 const double HALFPI = PI/2.0;
00044 
00045 // values up to this constant are considered zero and removed from the matrix
00046 const double NUMERICAL_ZERO = 1e-12;
00047 
00051 double tic();
00052 
00057 double tic(std::string id);
00058 
00063 double toc(double t0);
00064 
00070 double toc(std::string id);
00071 
00076 void tictoc_print();
00077 
00082 double tictoc(std::string id);
00083 
00087 Eigen::MatrixXd eye(int num);
00088 
00096 void givens(const double a, const double b, double& c, double& s);
00097 
00101 inline double standardRad(double t) {
00102   if (t >= 0.) {
00103     t = fmod(t+PI, TWOPI) - PI;
00104   } else {
00105     t = fmod(t-PI, -TWOPI) + PI;
00106   }
00107   return t;
00108 }
00109 
00110 inline double deg_to_rad(double d) {
00111   return (d/180.*PI);
00112 }
00113 
00114 inline double rad_to_deg(double r) {
00115   return (r/PI*180.);
00116 }
00117 
00124 template<typename T>
00125 T pinv(const T &a, double eps = std::numeric_limits<typename T::Scalar>::epsilon()) {
00126 
00127   bool m_lt_n = (a.rows() < a.cols());
00128 
00129   Eigen::JacobiSVD<T> svd;
00130   if (m_lt_n) {
00131       T tmp = a.transpose();
00132       svd = tmp.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
00133   } else {
00134       svd = a.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
00135   }
00136 
00137   typename T::Scalar tolerance = eps * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs().maxCoeff();
00138 
00139   T result = svd.matrixV() *
00140              T( (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0) ).asDiagonal() *
00141              svd.matrixU().adjoint();
00142 
00143   if (m_lt_n) {
00144       return result.transpose();
00145   } else {
00146       return result;
00147   }
00148 }
00149 
00156 template<typename T>
00157 T posdef_pinv(const T &a, double eps = std::numeric_limits<typename T::Scalar>::epsilon()) {
00158 
00159   Eigen::SelfAdjointEigenSolver<T> eig(a);
00160   if (eig.info() != Eigen::Success) {
00161     return T();
00162   }
00163 
00164   typename T::Scalar tolerance = eps * std::max(a.cols(), a.rows()) * eig.eigenvalues().array().abs().maxCoeff();
00165 
00166   T result = eig.eigenvectors() *
00167              T( (eig.eigenvalues().array() > tolerance).select(eig.eigenvalues().array().inverse(), 0) ).asDiagonal() *
00168              eig.eigenvectors().transpose();
00169 
00170   return result;
00171 }
00172 
00173 
00174 #ifdef NDEBUG
00175 // Release mode
00176 // remove requirements in inner loops for speed, but keep standard require functional
00177 #define requireDebug(req,msg)
00178 #define require(req,msg) if (!(req)) {fputs(msg, stderr);fputs("\n\n", stderr); exit(1);}
00179 #else
00180 // Debug mode
00181 // cause a crash to allow backtracing
00182 #define requireDebug(req,msg) if (!(req)) {fputs(msg, stderr);fputs("\n\n", stderr); abort();}
00183 #define require(req,msg) requireDebug(req,msg)
00184 #endif
00185 
00186 }
 All Classes Files Functions Variables