From f43c9557b95082fec17c68e438bad0393dae639f Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 21 Aug 2013 22:15:36 +0000 Subject: [PATCH] Fixed exception handling with TBB --- gtsam/CMakeLists.txt | 4 +- gtsam/base/types.h | 357 +++++++++++++++++++----------- gtsam/geometry/CalibratedCamera.h | 4 +- gtsam/linear/linearExceptions.h | 10 +- 4 files changed, 236 insertions(+), 139 deletions(-) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 1f53911da..b9cc75ce1 100644 --- a/gtsam/CMakeLists.txt +++ b/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() diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 5a9ee28a3..dd867750b 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -1,46 +1,52 @@ -/* ---------------------------------------------------------------------------- - - * 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 - - * -------------------------------------------------------------------------- */ - -/** - * @file types.h - * @brief Typedefs for easier changing of types - * @author Richard Roberts - * @date Aug 21, 2010 - * @addtogroup base - */ - -#pragma once - -#include - -#include - -#include -#include -#include - -namespace gtsam { - - /// Integer variable index type - typedef size_t Index; - - /** A function to convert indices to strings, for example by translating back - * to a nonlinear key and then to a Symbol. */ - typedef boost::function IndexFormatter; - - GTSAM_EXPORT std::string _defaultIndexFormatter(Index j); - - /** The default IndexFormatter outputs the index */ - static const IndexFormatter DefaultIndexFormatter = &_defaultIndexFormatter; - +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file types.h + * @brief Typedefs for easier changing of types + * @author Richard Roberts + * @date Aug 21, 2010 + * @addtogroup base + */ + +#pragma once + +#include +#include + +#include + +#include +#include +#include +#include + +#ifdef GTSAM_USE_TBB +#include +#endif + +namespace gtsam { + + /// Integer variable index type + typedef size_t Index; + + /** A function to convert indices to strings, for example by translating back + * to a nonlinear key and then to a Symbol. */ + typedef boost::function 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. - */ - template - struct const_selector { - }; - - /** Specialization for the non-const version */ - template - struct const_selector { - typedef AS_NON_CONST type; - }; - - /** Specialization for the const version */ - template - struct const_selector { - 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. - */ - template - struct ValueWithDefault { - T value; - - /** Default constructor, initialize to default value supplied in template argument */ - ValueWithDefault() : value(defaultValue) {} - - /** Initialize to the given value */ - ValueWithDefault(const T& _value) : value(_value) {} - - /** Operator to access the value */ - T& operator*() { 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 - 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 >)); - - /** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */ - template - ListOfOneContainer ListOfOne(const T& element) { - return ListOfOneContainer(element); - } - - /** 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 - + + + /* ************************************************************************* */ + /** + * Helper class that uses templates to select between two types based on + * whether TEST_TYPE is const or not. + */ + template + struct const_selector { + }; + + /** Specialization for the non-const version */ + template + struct const_selector { + typedef AS_NON_CONST type; + }; + + /** Specialization for the const version */ + template + struct const_selector { + 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. + */ + template + struct ValueWithDefault { + T value; + + /** Default constructor, initialize to default value supplied in template argument */ + ValueWithDefault() : value(defaultValue) {} + + /** Initialize to the given value */ + ValueWithDefault(const T& _value) : value(_value) {} + + /** Operator to access the value */ + T& operator*() { 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 + 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 >)); + + /** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */ + template + ListOfOneContainer ListOfOne(const T& element) { + return ListOfOneContainer(element); + } + + /* ************************************************************************* */ + /// Base exception type that uses tbb_exception if GTSAM is compiled with TBB. + template + class ThreadsafeException : +#ifdef GTSAM_USE_TBB + public tbb::tbb_exception +#else + public std::exception +#endif + { + private: +#ifdef GTSAM_USE_TBB + typedef tbb::tbb_exception Base; +#else + typedef std::exception Base; +#endif + + protected: + bool dynamic_; ///< Whether this object was moved + boost::optional 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(*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 + { + public: + /// Construct with a string describing the exception + RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException(description) {} + }; + + /* ************************************************************************* */ + /// Threadsafe runtime error exception + class OutOfRangeThreadsafe : public ThreadsafeException + { + public: + /// Construct with a string describing the exception + OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException(description) {} + }; + + /* ************************************************************************* */ + /// Threadsafe invalid argument exception + class InvalidArgumentThreadsafe : public ThreadsafeException + { + 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 namespace std { template inline int isfinite(T a) { @@ -176,4 +268,7 @@ namespace std { #ifdef max #undef max #endif - + +#ifdef ERROR +#undef ERROR +#endif diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index d67c69cbc..495d928cf 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -24,9 +24,9 @@ namespace gtsam { - class GTSAM_EXPORT CheiralityException: public std::runtime_error { + class GTSAM_EXPORT CheiralityException: public ThreadsafeException { public: - CheiralityException() : std::runtime_error("Cheirality Exception") {} + CheiralityException() : ThreadsafeException("Cheirality Exception") {} }; /** diff --git a/gtsam/linear/linearExceptions.h b/gtsam/linear/linearExceptions.h index 68acf526b..0e53e3eba 100644 --- a/gtsam/linear/linearExceptions.h +++ b/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 { 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 { 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 { 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 { public: - InvalidDenseElimination(const char *message) : std::invalid_argument(message) {} + InvalidDenseElimination(const char *message) : ThreadsafeException(message) {} }; }