Fixed exception handling with TBB

gtsam/CMakeLists.txt

@@ -140,7 +140,9 @@ set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp"
 	
 # Special cases
 if(MSVC)
-	set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp"
+	set_property(SOURCE
+		"${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp"
+		"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ISAM2.cpp"
 		APPEND PROPERTY COMPILE_FLAGS "/bigobj")
 endif()
 

gtsam/base/types.h

@@ -1,46 +1,52 @@
-/* ----------------------------------------------------------------------------
+/* ----------------------------------------------------------------------------
-
+
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
+ * Atlanta, Georgia 30332-0415
- * All Rights Reserved
+ * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
+
- * See LICENSE for the license information
+ * See LICENSE for the license information
-
+
- * -------------------------------------------------------------------------- */
+ * -------------------------------------------------------------------------- */
-
+
-/**
+/**
- * @file     types.h
+ * @file     types.h
- * @brief    Typedefs for easier changing of types
+ * @brief    Typedefs for easier changing of types
- * @author   Richard Roberts
+ * @author   Richard Roberts
- * @date     Aug 21, 2010
+ * @date     Aug 21, 2010
- * @addtogroup base
+ * @addtogroup base
- */
+ */
-
+
-#pragma once
+#pragma once
-
+
-#include <gtsam/dllexport.h>
+#include <gtsam/dllexport.h>
-
+#include <gtsam/config.h>
-#include <cstddef>
+
-
+#include <cstddef>
-#include <string>
+
-#include <boost/function/function1.hpp>
+#include <string>
-#include <boost/range/concepts.hpp>
+#include <boost/function/function1.hpp>
-
+#include <boost/range/concepts.hpp>
-namespace gtsam {
+#include <boost/optional.hpp>
-
+
-  /// Integer variable index type
+#ifdef GTSAM_USE_TBB
-  typedef size_t Index;
+#include <tbb/tbb_exception.h>
-
+#endif
-  /** A function to convert indices to strings, for example by translating back
+
-   * to a nonlinear key and then to a Symbol. */
+namespace gtsam {
-  typedef boost::function<std::string(Index)> IndexFormatter;
+
-
+  /// Integer variable index type
-  GTSAM_EXPORT std::string _defaultIndexFormatter(Index j);
+  typedef size_t Index;
-
+
-  /** The default IndexFormatter outputs the index */
+  /** A function to convert indices to strings, for example by translating back
-  static const IndexFormatter DefaultIndexFormatter = &_defaultIndexFormatter;
+   * to a nonlinear key and then to a Symbol. */
-
+  typedef boost::function<std::string(Index)> IndexFormatter;
+
+  GTSAM_EXPORT std::string _defaultIndexFormatter(Index j);
+
+  /** The default IndexFormatter outputs the index */
+  static const IndexFormatter DefaultIndexFormatter = &_defaultIndexFormatter;
+
 
   /// Integer nonlinear key type
   typedef size_t Key;
@@ -59,93 +65,179 @@ namespace gtsam {
 
   /// The index type for Eigen objects
   typedef ptrdiff_t DenseIndex;
-
+
-
+
-  /**
+  /* ************************************************************************* */
-   * Helper class that uses templates to select between two types based on
+  /**
-   * whether TEST_TYPE is const or not.
+   * Helper class that uses templates to select between two types based on
-   */
+   * whether TEST_TYPE is const or not.
-  template<typename TEST_TYPE, typename BASIC_TYPE, typename AS_NON_CONST,
+   */
-      typename AS_CONST>
+  template<typename TEST_TYPE, typename BASIC_TYPE, typename AS_NON_CONST,
-  struct const_selector {
+      typename AS_CONST>
-  };
+  struct const_selector {
-
+  };
-  /** Specialization for the non-const version */
+
-  template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
+  /** Specialization for the non-const version */
-  struct const_selector<BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
+  template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
-    typedef AS_NON_CONST type;
+  struct const_selector<BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
-  };
+    typedef AS_NON_CONST type;
-
+  };
-  /** Specialization for the const version */
+
-  template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
+  /** Specialization for the const version */
-  struct const_selector<const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
+  template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
-    typedef AS_CONST type;
+  struct const_selector<const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
-  };
+    typedef AS_CONST type;
-
+  };
-  /**
+
-   * Helper struct that encapsulates a value with a default, this is just used
+  /* ************************************************************************* */
-   * as a member object so you don't have to specify defaults in the class
+  /**
-   * constructor.
+   * Helper struct that encapsulates a value with a default, this is just used
-   */
+   * as a member object so you don't have to specify defaults in the class
-  template<typename T, T defaultValue>
+   * constructor.
-  struct ValueWithDefault {
+   */
-    T value;
+  template<typename T, T defaultValue>
-
+  struct ValueWithDefault {
-    /** Default constructor, initialize to default value supplied in template argument */
+    T value;
-    ValueWithDefault() : value(defaultValue) {}
+
-
+    /** Default constructor, initialize to default value supplied in template argument */
-    /** Initialize to the given value */
+    ValueWithDefault() : value(defaultValue) {}
-    ValueWithDefault(const T& _value) : value(_value) {}
+
-
+    /** Initialize to the given value */
-    /** Operator to access the value */
+    ValueWithDefault(const T& _value) : value(_value) {}
-    T& operator*() { return value; }
+
-
+    /** Operator to access the value */
-    /** Implicit conversion allows use in if statements for bool type, etc. */
+    T& operator*() { return value; }
-    operator T() const { return value; }
+
-  };
+    /** Implicit conversion allows use in if statements for bool type, etc. */
-
+    operator T() const { return value; }
-  /** A helper class that behaves as a container with one element, and works with
+  };
-   * boost::range */
+
-  template<typename T>
+  /* ************************************************************************* */
-  class ListOfOneContainer {
+  /** A helper class that behaves as a container with one element, and works with
-    T element_;
+   * boost::range */
-  public:
+  template<typename T>
-    typedef T value_type;
+  class ListOfOneContainer {
-    typedef const T* const_iterator;
+    T element_;
-    typedef T* iterator;
+  public:
-    ListOfOneContainer(const T& element) : element_(element) {}
+    typedef T value_type;
-    const T* begin() const { return &element_; }
+    typedef const T* const_iterator;
-    const T* end() const { return &element_ + 1; }
+    typedef T* iterator;
-    T* begin() { return &element_; }
+    ListOfOneContainer(const T& element) : element_(element) {}
-    T* end() { return &element_ + 1; }
+    const T* begin() const { return &element_; }
-    size_t size() const { return 1; }
+    const T* end() const { return &element_ + 1; }
-  };
+    T* begin() { return &element_; }
-
+    T* end() { return &element_ + 1; }
-  BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept<ListOfOneContainer<int> >));
+    size_t size() const { return 1; }
-
+  };
-  /** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */
+
-  template<typename T>
+  BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept<ListOfOneContainer<int> >));
-  ListOfOneContainer<T> ListOfOne(const T& element) {
+
-    return ListOfOneContainer<T>(element);
+  /** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */
-  }
+  template<typename T>
-
+  ListOfOneContainer<T> ListOfOne(const T& element) {
-  /** An assertion that throws an exception if NDEBUG is not defined and
+    return ListOfOneContainer<T>(element);
-   * evaluates to an empty statement otherwise. */
+  }
-#ifdef NDEBUG
+
-#define assert_throw(CONDITION, EXCEPTION) ((void)0)
+  /* ************************************************************************* */
-#else
+  /// Base exception type that uses tbb_exception if GTSAM is compiled with TBB.
-#define assert_throw(CONDITION, EXCEPTION) \
+  template<class DERIVED>
-  if(!(CONDITION)) { \
+  class ThreadsafeException :
-    throw (EXCEPTION); \
+#ifdef GTSAM_USE_TBB
-  }
+    public tbb::tbb_exception
-#endif
+#else
-
+    public std::exception
-}
+#endif
-
+  {
-#ifdef _MSC_VER
+  private:
-
+#ifdef GTSAM_USE_TBB
-// Define some common g++ functions and macros we use that MSVC does not have
+    typedef tbb::tbb_exception Base;
-
+#else
+    typedef std::exception Base;
+#endif
+
+  protected:
+    bool dynamic_; ///< Whether this object was moved
+    boost::optional<std::string> description_; ///< Optional description
+
+    /// Default constructor is protected - may only be created from derived classes
+    ThreadsafeException() : dynamic_(false) {}
+
+    /// Copy constructor is protected - may only be created from derived classes
+    ThreadsafeException(const ThreadsafeException& other) : Base(other), dynamic_(false) {}
+
+    /// Construct with description string
+    ThreadsafeException(const std::string& description) : dynamic_(false), description_(description) {}
+
+  public:
+    // Implement functions for tbb_exception
+#ifdef GTSAM_USE_TBB
+    virtual tbb::tbb_exception* move() {
+      DERIVED* clone = ::new DERIVED(static_cast<DERIVED&>(*this));
+      clone->dynamic_ = true;
+      return clone;
+    }
+
+    virtual void destroy() throw() {
+      if (dynamic_)
+        ::delete this;
+    }
+
+    virtual void throw_self() {
+      throw *this; }
+
+    virtual const char* name() const throw() {
+      return typeid(DERIVED).name(); }
+#endif
+
+    virtual const char* what() const throw() {
+      return description_ ? description_->c_str() : ""; }
+  };
+
+  /* ************************************************************************* */
+  /// Threadsafe runtime error exception
+  class RuntimeErrorThreadsafe : public ThreadsafeException<RuntimeErrorThreadsafe>
+  {
+  public:
+    /// Construct with a string describing the exception
+    RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException(description) {}
+  };
+
+  /* ************************************************************************* */
+  /// Threadsafe runtime error exception
+  class OutOfRangeThreadsafe : public ThreadsafeException<OutOfRangeThreadsafe>
+  {
+  public:
+    /// Construct with a string describing the exception
+    OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException(description) {}
+  };
+
+  /* ************************************************************************* */
+  /// Threadsafe invalid argument exception
+  class InvalidArgumentThreadsafe : public ThreadsafeException<InvalidArgumentThreadsafe>
+  {
+  public:
+    /// Construct with a string describing the exception
+    InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException(description) {}
+  };
+
+}
+
+/* ************************************************************************* */
+/** An assertion that throws an exception if NDEBUG is not defined and
+* evaluates to an empty statement otherwise. */
+#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
+
 #include <boost/math/special_functions/fpclassify.hpp>
 namespace std {
   template<typename T> inline int isfinite(T a) {
@@ -176,4 +268,7 @@ namespace std {
 #ifdef max
 #undef max
 #endif
-
+
+#ifdef ERROR
+#undef ERROR
+#endif

gtsam/geometry/CalibratedCamera.h

@@ -24,9 +24,9 @@
 
 namespace gtsam {
 
-  class GTSAM_EXPORT CheiralityException: public std::runtime_error {
+  class GTSAM_EXPORT CheiralityException: public ThreadsafeException<CheiralityException> {
   public:
-    CheiralityException() : std::runtime_error("Cheirality Exception") {}
+    CheiralityException() : ThreadsafeException("Cheirality Exception") {}
   };
 
   /**

gtsam/linear/linearExceptions.h

@@ -92,7 +92,7 @@ namespace gtsam {
      ordered in elimination order and occupy scalars in the same way as
      described for Jacobian columns in the previous bullet.
    */
-  class IndeterminantLinearSystemException : public std::exception {
+  class IndeterminantLinearSystemException : public ThreadsafeException<IndeterminantLinearSystemException> {
     Index j_;
     mutable std::string what_;
   public:
@@ -117,7 +117,7 @@ on gtsam::IndeterminantLinearSystemException for more information.\n";
   /* ************************************************************************* */
   /** An exception indicating that the noise model dimension passed into a
    * JacobianFactor has a different dimensionality than the factor. */
-  class GTSAM_EXPORT InvalidNoiseModel : public std::exception {
+  class GTSAM_EXPORT InvalidNoiseModel : public ThreadsafeException<InvalidNoiseModel> {
   public:
     const DenseIndex factorDims; ///< The dimensionality of the factor
     const DenseIndex noiseModelDims; ///< The dimensionality of the noise model
@@ -135,7 +135,7 @@ on gtsam::IndeterminantLinearSystemException for more information.\n";
   /* ************************************************************************* */
   /** An exception indicating that a matrix block passed into a
    * JacobianFactor has a different dimensionality than the factor. */
-  class GTSAM_EXPORT InvalidMatrixBlock : public std::exception {
+  class GTSAM_EXPORT InvalidMatrixBlock : public ThreadsafeException<InvalidMatrixBlock> {
   public:
     const DenseIndex factorRows; ///< The dimensionality of the factor
     const DenseIndex blockRows; ///< The dimensionality of the noise model
@@ -151,9 +151,9 @@ on gtsam::IndeterminantLinearSystemException for more information.\n";
   };
 
   /* ************************************************************************* */
-  class InvalidDenseElimination : public std::invalid_argument {
+  class InvalidDenseElimination : public ThreadsafeException<InvalidDenseElimination> {
   public:
-    InvalidDenseElimination(const char *message) : std::invalid_argument(message) {}
+    InvalidDenseElimination(const char *message) : ThreadsafeException(message) {}
   };
 
  }