util.h

Go to the documentation of this file.

#pragma once

#include <stdlib.h>
#include <stdio.h>
#include <cmath>
#include <string>
#include <Eigen/Dense>

namespace isam {

// some math constants
#ifndef PI
const double PI = 3.14159265358979323846;
#endif
const double TWOPI = 2.0*PI;
const double HALFPI = PI/2.0;

// values up to this constant are considered zero and removed from the matrix
const double NUMERICAL_ZERO = 1e-12;

double tic();

double tic(std::string id);

double toc(double t0);

double toc(std::string id);

void tictoc_print();

double tictoc(std::string id);

Eigen::MatrixXd eye(int num);

void givens(const double a, const double b, double& c, double& s);

inline double standardRad(double t) {
  if (t >= 0.) {
    t = fmod(t+PI, TWOPI) - PI;
  } else {
    t = fmod(t-PI, -TWOPI) + PI;
  }
  return t;
}

inline double deg_to_rad(double d) {
  return (d/180.*PI);
}

inline double rad_to_deg(double r) {
  return (r/PI*180.);
}

template<typename T>
T pinv(const T &a, double eps = std::numeric_limits<typename T::Scalar>::epsilon()) {

  bool m_lt_n = (a.rows() < a.cols());

  Eigen::JacobiSVD<T> svd;
  if (m_lt_n) {
      T tmp = a.transpose();
      svd = tmp.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
  } else {
      svd = a.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
  }

  typename T::Scalar tolerance = eps * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs().maxCoeff();

  T result = svd.matrixV() *
             T( (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0) ).asDiagonal() *
             svd.matrixU().adjoint();

  if (m_lt_n) {
      return result.transpose();
  } else {
      return result;
  }
}

template<typename T>
T posdef_pinv(const T &a, double eps = std::numeric_limits<typename T::Scalar>::epsilon()) {

  Eigen::SelfAdjointEigenSolver<T> eig(a);
  if (eig.info() != Eigen::Success) {
    return T();
  }

  typename T::Scalar tolerance = eps * std::max(a.cols(), a.rows()) * eig.eigenvalues().array().abs().maxCoeff();

  T result = eig.eigenvectors() *
             T( (eig.eigenvalues().array() > tolerance).select(eig.eigenvalues().array().inverse(), 0) ).asDiagonal() *
             eig.eigenvectors().transpose();

  return result;
}


#ifdef NDEBUG
// Release mode
// remove requirements in inner loops for speed, but keep standard require functional
#define requireDebug(req,msg)
#define require(req,msg) if (!(req)) {fputs(msg, stderr);fputs("\n\n", stderr); exit(1);}
#else
// Debug mode
// cause a crash to allow backtracing
#define requireDebug(req,msg) if (!(req)) {fputs(msg, stderr);fputs("\n\n", stderr); abort();}
#define require(req,msg) requireDebug(req,msg)
#endif

}