Main MRPT website > C++ reference for MRPT 1.5.3
common.hpp
Go to the documentation of this file.
1 #ifndef SOPHUS_COMMON_HPP
2 #define SOPHUS_COMMON_HPP
3 
4 #include <cmath>
5 #include <cstdio>
6 #include <cstdlib>
7 #include <iostream>
8 
9 #include <Eigen/Core>
10 
11 // following boost's assert.hpp
12 #undef SOPHUS_ENSURE
13 
14 // ENSURES are similar to ASSERTS, but they are always checked for (including in
15 // release builds). At the moment there are no ASSERTS in Sophus which should
16 // only be used for checks which are performance critical.
17 
18 #ifdef __GNUC__
19 #define SOPHUS_FUNCTION __PRETTY_FUNCTION__
20 #elif (_MSC_VER >= 1310)
21 #define SOPHUS_FUNCTION __FUNCTION__
22 #else
23 #define SOPHUS_FUNCTION "unknown"
24 #endif
25 
26 // Make sure this compiles with older versions of Eigen which do not have
27 // EIGEN_DEVICE_FUNC defined.
28 #ifndef EIGEN_DEVICE_FUNC
29 #define EIGEN_DEVICE_FUNC
30 #endif
31 
32 #define SOPHUS_FUNC EIGEN_DEVICE_FUNC
33 
34 namespace Sophus {
35 namespace details {
36 
37 // Following: http://stackoverflow.com/a/22759544
38 template <class T>
39 class IsStreamable {
40  private:
41  template <class TT>
42  static auto test(int)
43  -> decltype(std::declval<std::stringstream&>() << std::declval<TT>(),
44  std::true_type());
45 
46  template <class>
47  static auto test(...) -> std::false_type;
48 
49  public:
50  static bool const value = decltype(test<T>(0))::value;
51 };
52 
53 template <class T>
54 class ArgToStream {
55  public:
56  static void impl(std::stringstream& stream, T&& arg) {
57  stream << std::forward<T>(arg);
58  }
59 };
60 
61 inline void FormatStream(std::stringstream& stream, char const* text) {
62  stream << text;
63  return;
64 }
65 
66 // Following: http://en.cppreference.com/w/cpp/language/parameter_pack
67 template <class T, typename... Args>
68 void FormatStream(std::stringstream& stream, char const* text, T&& arg,
69  Args&&... args) {
70  static_assert(IsStreamable<T>::value,
71  "One of the args has no ostream overload!");
72  for (; *text != '\0'; ++text) {
73  if (*text == '%') {
74  ArgToStream<T&&>::impl(stream, std::forward<T>(arg));
75  FormatStream(stream, text + 1, std::forward<Args>(args)...);
76  return;
77  }
78  stream << *text;
79  }
80  stream << "\nFormat-Warning: There are " << sizeof...(Args) + 1
81  << " args unused.";
82  return;
83 }
84 
85 template <class... Args>
86 std::string FormatString(char const* text, Args&&... args) {
87  std::stringstream stream;
88  FormatStream(stream, text, std::forward<Args>(args)...);
89  return stream.str();
90 }
91 
92 inline std::string FormatString() { return std::string(); }
93 } // namespace details
94 } // namespace Sophus
95 
96 #if defined(SOPHUS_DISABLE_ENSURES)
97 
98 #define SOPHUS_ENSURE(expr, ...) ((void)0)
99 
100 #elif defined(SOPHUS_ENABLE_ENSURE_HANDLER)
101 
102 namespace Sophus {
103 void ensureFailed(char const* function, char const* file, int line,
104  char const* description);
105 }
106 
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__) \
112  .c_str()))
113 #else
114 namespace Sophus {
115 template <class... Args>
116 SOPHUS_FUNC void defaultEnsure(char const* function, char const* file, int line,
117  char const* description, Args&&... args) {
118  std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n",
119  function, file, line);
120 #ifdef __CUDACC__
121  std::printf("%s", description);
122 #else
123  std::cout << details::FormatString(description, std::forward<Args>(args)...)
124  << std::endl;
125  std::abort();
126 #endif
127 }
128 } // namespace Sophus
129 #define SOPHUS_ENSURE(expr, description, ...) \
130  ((expr) ? ((void)0) \
131  : Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, __LINE__, \
132  (description), ##__VA_ARGS__))
133 #endif
134 
135 namespace Sophus {
136 
137 template <class Scalar>
138 struct Constants {
139  SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); }
140 
141  SOPHUS_FUNC static Scalar pi() { return Scalar(M_PI); }
142 };
143 
144 template <>
145 struct Constants<float> {
146  SOPHUS_FUNC static float constexpr epsilon() {
147  return static_cast<float>(1e-5);
148  }
149 
150  SOPHUS_FUNC static float constexpr pi() { return static_cast<float>(M_PI); }
151 };
152 
153 // Leightweight optional implementation which require ``T`` to have a
154 // default constructor.
155 //
156 // TODO: Replace with std::optional once Sophus moves to c++17.
157 //
158 struct nullopt_t {
159  explicit constexpr nullopt_t() {}
160 };
161 
162 constexpr nullopt_t nullopt{};
163 template <class T>
164 
165 class optional {
166  public:
167  optional() : is_valid_(false) {}
168 
169  optional(nullopt_t) : is_valid_(false) {}
170 
171  optional(T const& type) : type_(type), is_valid_(true) {}
172 
173  explicit operator bool() const { return is_valid_; }
174 
175  T const* operator->() const {
176  SOPHUS_ENSURE(is_valid_, "must be valid");
177  return &type_;
178  }
179 
180  T* operator->() {
181  SOPHUS_ENSURE(is_valid_, "must be valid");
182  return &type_;
183  }
184 
185  T const& operator*() const {
186  SOPHUS_ENSURE(is_valid_, "must be valid");
187  return type_;
188  }
189 
190  T& operator*() {
191  SOPHUS_ENSURE(is_valid_, "must be valid");
192  return type_;
193  }
194 
195  private:
196  T type_;
197  bool is_valid_;
198 };
199 
200 template <bool B, class T = void>
201 using enable_if_t = typename std::enable_if<B, T>::type;
202 } // namespace Sophus
203 
204 #endif // SOPHUS_COMMON_HPP
optional(nullopt_t)
Definition: common.hpp:169
#define SOPHUS_ENSURE(expr, description,...)
Definition: common.hpp:129
std::string FormatString(char const *text, Args &&... args)
Definition: common.hpp:86
static SOPHUS_FUNC float constexpr pi()
Definition: common.hpp:150
#define M_PI
Definition: bits.h:78
static bool const value
Definition: common.hpp:50
static SOPHUS_FUNC Scalar pi()
Definition: common.hpp:141
typename std::enable_if< B, T >::type enable_if_t
Definition: common.hpp:201
static SOPHUS_FUNC Scalar epsilon()
Definition: common.hpp:139
void FormatStream(std::stringstream &stream, char const *text)
Definition: common.hpp:61
T const & operator*() const
Definition: common.hpp:185
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)
Definition: common.hpp:116
static void impl(std::stringstream &stream, T &&arg)
Definition: common.hpp:56
static SOPHUS_FUNC float constexpr epsilon()
Definition: common.hpp:146
constexpr nullopt_t nullopt
Definition: common.hpp:162
optional(T const &type)
Definition: common.hpp:171
T const * operator->() const
Definition: common.hpp:175
constexpr nullopt_t()
Definition: common.hpp:159
T * operator->()
Definition: common.hpp:180
#define SOPHUS_FUNC
Definition: common.hpp:32



Page generated by Doxygen 1.8.13 for MRPT 1.5.3 at Tue Aug 22 01:03:35 UTC 2017