1 #ifndef SOPHUS_COMMON_HPP 2 #define SOPHUS_COMMON_HPP 19 #define SOPHUS_FUNCTION __PRETTY_FUNCTION__ 20 #elif (_MSC_VER >= 1310) 21 #define SOPHUS_FUNCTION __FUNCTION__ 23 #define SOPHUS_FUNCTION "unknown" 28 #ifndef EIGEN_DEVICE_FUNC 29 #define EIGEN_DEVICE_FUNC 32 #define SOPHUS_FUNC EIGEN_DEVICE_FUNC 43 -> decltype(std::declval<std::stringstream&>() << std::declval<TT>(),
47 static auto test(...) -> std::false_type;
50 static bool const value = decltype(test<T>(0))::value;
56 static void impl(std::stringstream& stream, T&& arg) {
57 stream << std::forward<T>(arg);
61 inline void FormatStream(std::stringstream& stream,
char const* text) {
67 template <
class T,
typename... Args>
68 void FormatStream(std::stringstream& stream,
char const* text, T&& arg,
71 "One of the args has no ostream overload!");
72 for (; *text !=
'\0'; ++text) {
75 FormatStream(stream, text + 1, std::forward<Args>(args)...);
80 stream <<
"\nFormat-Warning: There are " <<
sizeof...(Args) + 1
85 template <
class... Args>
87 std::stringstream stream;
96 #if defined(SOPHUS_DISABLE_ENSURES) 98 #define SOPHUS_ENSURE(expr, ...) ((void)0) 100 #elif defined(SOPHUS_ENABLE_ENSURE_HANDLER) 103 void ensureFailed(
char const*
function,
char const* file,
int line,
104 char const* description);
107 #define SOPHUS_ENSURE(expr, description, ...) \ 108 ((expr) ? ((void)0) \ 109 : ::Sophus::ensureFailed( \ 110 SOPHUS_FUNCTION, __FILE__, __LINE__, \ 111 Sophus::details::FormatString((description), ##__VA_ARGS__) \ 115 template <
class... Args>
117 char const* description, Args&&... args) {
118 std::printf(
"Sophus ensure failed in function '%s', file '%s', line %d.\n",
119 function, file, line);
121 std::printf(
"%s", description);
123 std::cout << details::FormatString(description, std::forward<Args>(args)...)
129 #define SOPHUS_ENSURE(expr, description, ...) \ 130 ((expr) ? ((void)0) \ 131 : Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, __LINE__, \ 132 (description), ##__VA_ARGS__)) 137 template <
class Scalar>
147 return static_cast<float>(1
e-5);
171 optional(T
const& type) : type_(type), is_valid_(true) {}
173 explicit operator bool()
const {
return is_valid_; }
200 template <
bool B,
class T =
void>
204 #endif // SOPHUS_COMMON_HPP
#define SOPHUS_ENSURE(expr, description,...)
std::string FormatString(char const *text, Args &&... args)
static SOPHUS_FUNC float constexpr pi()
static SOPHUS_FUNC Scalar pi()
typename std::enable_if< B, T >::type enable_if_t
static SOPHUS_FUNC Scalar epsilon()
void FormatStream(std::stringstream &stream, char const *text)
T const & operator*() const
static auto test(int) -> decltype(std::declval< std::stringstream &>()<< std::declval< TT >(), std::true_type())
SOPHUS_FUNC void defaultEnsure(char const *function, char const *file, int line, char const *description, Args &&... args)
static void impl(std::stringstream &stream, T &&arg)
static SOPHUS_FUNC float constexpr epsilon()
constexpr nullopt_t nullopt
T const * operator->() const