types.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/dllexport.h>
#include <gtsam/config.h>

#include <cstddef>
#include <string>
#include <iostream>
#include <boost/function/function1.hpp>
#include <boost/range/concepts.hpp>
#include <boost/optional.hpp>

#ifdef GTSAM_USE_TBB
#include <tbb/task_scheduler_init.h>
#include <tbb/tbb_exception.h>
#include <tbb/scalable_allocator.h>
#endif

#ifdef GTSAM_USE_EIGEN_MKL_OPENMP
#include <omp.h>
#endif

#ifdef __clang__
#  define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) \
  _Pragma("clang diagnostic push") \
  _Pragma("clang diagnostic ignored \"" diag "\"")
#else
#  define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag)
#endif

#ifdef __clang__
#  define CLANG_DIAGNOSTIC_POP() _Pragma("clang diagnostic pop")
#else
#  define CLANG_DIAGNOSTIC_POP()
#endif

namespace gtsam {

  typedef size_t Key;

  typedef boost::function<std::string(Key)> KeyFormatter;

  // Helper function for DefaultKeyFormatter
  GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);

  static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;


  typedef ptrdiff_t DenseIndex;


  /* ************************************************************************* */
  template<typename TEST_TYPE, typename BASIC_TYPE, typename AS_NON_CONST,
      typename AS_CONST>
  struct const_selector {
  };

  template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
  struct const_selector<BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
    typedef AS_NON_CONST type;
  };

  template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
  struct const_selector<const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
    typedef AS_CONST type;
  };

  /* ************************************************************************* */
  template<typename T, T defaultValue>
  struct ValueWithDefault {
    T value;

    ValueWithDefault() : value(defaultValue) {}

    ValueWithDefault(const T& _value) : value(_value) {}

    T& operator*() { return value; }

    const T& operator*() const { return value; }

    operator T() const { return value; }
  };

  /* ************************************************************************* */
  template<typename T>
  class ListOfOneContainer {
    T element_;
  public:
    typedef T value_type;
    typedef const T* const_iterator;
    typedef T* iterator;
    ListOfOneContainer(const T& element) : element_(element) {}
    const T* begin() const { return &element_; }
    const T* end() const { return &element_ + 1; }
    T* begin() { return &element_; }
    T* end() { return &element_ + 1; }
    size_t size() const { return 1; }
  };

  BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept<ListOfOneContainer<int> >));

  template<typename T>
  ListOfOneContainer<T> ListOfOne(const T& element) {
    return ListOfOneContainer<T>(element);
  }

  /* ************************************************************************* */
  template<class DERIVED>
  class ThreadsafeException :
#ifdef GTSAM_USE_TBB
    public tbb::tbb_exception
#else
    public std::exception
#endif
  {
#ifdef GTSAM_USE_TBB
  private:
    typedef tbb::tbb_exception Base;
  protected:
    typedef std::basic_string<char, std::char_traits<char>, tbb::tbb_allocator<char> > String;
#else
  private:
    typedef std::exception Base;
  protected:
    typedef std::string String;
#endif

  protected:
    bool dynamic_; 
    mutable boost::optional<String> description_; 

    ThreadsafeException() : dynamic_(false) {}

    ThreadsafeException(const ThreadsafeException& other) : Base(other), dynamic_(false) {}

    ThreadsafeException(const std::string& description) : dynamic_(false), description_(String(description.begin(), description.end())) {}

    virtual ~ThreadsafeException() throw () {}

  public:
    // Implement functions for tbb_exception
#ifdef GTSAM_USE_TBB
    virtual tbb::tbb_exception* move() throw () {
      void* cloneMemory = scalable_malloc(sizeof(DERIVED));
      if (!cloneMemory) {
        std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl;
        exit(-1);
      }
      DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast<DERIVED&>(*this));
      clone->dynamic_ = true;
      return clone;
    }

    virtual void destroy() throw() {
      if (dynamic_) {
        DERIVED* derivedPtr = static_cast<DERIVED*>(this);
        derivedPtr->~DERIVED();
        scalable_free(derivedPtr);
      }
    }

    virtual void throw_self() {
      throw *static_cast<DERIVED*>(this); }

    virtual const char* name() const throw() {
      return typeid(DERIVED).name(); }
#endif

    virtual const char* what() const throw() {
      return description_ ? description_->c_str() : ""; }
  };

  /* ************************************************************************* */
  class RuntimeErrorThreadsafe : public ThreadsafeException<RuntimeErrorThreadsafe>
  {
  public:
    RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException<RuntimeErrorThreadsafe>(description) {}
  };

  /* ************************************************************************* */
  class OutOfRangeThreadsafe : public ThreadsafeException<OutOfRangeThreadsafe>
  {
  public:
    OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException<OutOfRangeThreadsafe>(description) {}
  };

  /* ************************************************************************* */
  class InvalidArgumentThreadsafe : public ThreadsafeException<InvalidArgumentThreadsafe>
  {
  public:
    InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException<InvalidArgumentThreadsafe>(description) {}
  };

  /* ************************************************************************* */
#ifdef __clang__
#  pragma clang diagnostic push
#  pragma clang diagnostic ignored "-Wunused-private-field" // Clang complains that previousOpenMPThreads is unused in the #else case below
#endif

  class TbbOpenMPMixedScope
  {
    int previousOpenMPThreads;

  public:
#if defined GTSAM_USE_TBB && defined GTSAM_USE_EIGEN_MKL_OPENMP
    TbbOpenMPMixedScope() :
      previousOpenMPThreads(omp_get_num_threads())
    {
      omp_set_num_threads(omp_get_num_procs() / 4);
    }

    ~TbbOpenMPMixedScope()
    {
      omp_set_num_threads(previousOpenMPThreads);
    }
#else
    TbbOpenMPMixedScope() : previousOpenMPThreads(-1) {}
    ~TbbOpenMPMixedScope() {}
#endif
  };

#ifdef __clang__
#  pragma clang diagnostic pop
#endif

}

/* ************************************************************************* */
#ifdef NDEBUG
#define assert_throw(CONDITION, EXCEPTION) ((void)0)
#else
#define assert_throw(CONDITION, EXCEPTION) \
  if (!(CONDITION)) { \
  throw (EXCEPTION); \
  }
#endif

#ifdef _MSC_VER

// Define some common g++ functions and macros we use that MSVC does not have

#if (_MSC_VER < 1800)

#include <boost/math/special_functions/fpclassify.hpp>
namespace std {
  template<typename T> inline int isfinite(T a) {
    return (int)boost::math::isfinite(a); }
  template<typename T> inline int isnan(T a) {
    return (int)boost::math::isnan(a); }
  template<typename T> inline int isinf(T a) {
    return (int)boost::math::isinf(a); }
}

#endif

#include <boost/math/constants/constants.hpp>
#ifndef M_PI
#define M_PI (boost::math::constants::pi<double>())
#endif
#ifndef M_PI_2
#define M_PI_2 (boost::math::constants::pi<double>() / 2.0)
#endif
#ifndef M_PI_4
#define M_PI_4 (boost::math::constants::pi<double>() / 4.0)
#endif

#endif

#ifdef min
#undef min
#endif

#ifdef max
#undef max
#endif

#ifdef ERROR
#undef ERROR
#endif