linear_algebra/sophus/formatters.hpp
Go to the documentation of this file.
1 
4 /*****************************************************************************
5 ** Ifdefs
6 *****************************************************************************/
7 
8 #ifndef ecl_linear_algebra_SOPHUS_FORMATTERS_HPP_
9 #define ecl_linear_algebra_SOPHUS_FORMATTERS_HPP_
10 
11 /*****************************************************************************
12 ** Includes
13 *****************************************************************************/
14 
15 #include <ecl/exceptions.hpp>
16 #include <ecl/formatters.hpp>
17 #include <sophus/se3.hpp>
18 
19 /*****************************************************************************
20 ** Namespaces
21 *****************************************************************************/
22 
23 namespace Sophus {
24 
25 /*****************************************************************************
26 ** Interfaces
27 *****************************************************************************/
28 
38 class SE3fFormatter
39 {
40 public:
48  SE3fFormatter(const int &w = -1, const unsigned int &p = 2)
49  : format(w, p, ecl::RightAlign)
50  , tmp_width(w)
51  , tmp_precision(p)
52  , tmp_formatting(false)
53  , ready_to_format(false)
54  , s_(NULL)
55 {}
56  virtual ~SE3fFormatter() {}
63  SE3fFormatter& precision( const unsigned int &p ) {
64  format.precision(p);
65  return *this;
66  }
75  SE3fFormatter& width( const int &w ) {
76  format.width(w);
77  return *this;
78  }
83  unsigned int precision() { return format.precision(); }
84 
89  int width() { return format.width(); }
90 
102  SE3fFormatter& operator() (const Sophus::SE3f & s ) {
103  s_ = &s;
104  ready_to_format = true;
105  return (*this);
106  }
123  SE3fFormatter& operator() (const Sophus::SE3f & s, const int &w, const unsigned int &p) {
124  s_ = &s;
125  tmp_precision = p;
126  tmp_width = w;
127  tmp_formatting = true;
128  ready_to_format = true;
129  return (*this);
130  }
144  template <typename OutputStream>
145  friend OutputStream& operator << ( OutputStream& ostream, SE3fFormatter & formatter ) ecl_assert_throw_decl(ecl::StandardException);
146 
147 private:
149  int tmp_width;
150  unsigned int tmp_precision;
151  bool tmp_formatting;
152  bool ready_to_format;
153  const Sophus::SE3f *s_;
154 };
155 
156 template <typename OutputStream>
157 OutputStream& operator << (OutputStream& ostream, SE3fFormatter & formatter ) ecl_assert_throw_decl(ecl::StandardException) {
158 
159  ecl_assert_throw( formatter.s_, ecl::StandardException(LOC,ecl::UsageError,"The formatter cannot print any data - "
160  "sophus object was not initialised "
161  "please pass the your matrix through () operator") );
162 
163  ecl_assert_throw(formatter.ready_to_format, ecl::StandardException(LOC,ecl::UsageError,"The formatter cannot print any data - "
164  "either there is no data available, or you have tried to use the "
165  "formatter more than once in a single streaming operation. "
166  "C++ produces unspecified results when functors are used multiply "
167  "in the same stream sequence, so this is not permitted here.") );
168 
169  if ( formatter.ready_to_format ) {
170  unsigned int prm_precision = formatter.format.precision();;
171  int prm_width = formatter.format.width();
172  if ( formatter.tmp_formatting ) {
173  formatter.format.precision(formatter.tmp_precision);
174  formatter.format.width(formatter.tmp_width);
175  }
176 
177  /*********************
178  ** Stream Sophus
179  **********************/
180  Eigen::Vector3f translation = formatter.s_->translation();
181  Eigen::Quaternionf quaternion = formatter.s_->unit_quaternion();
182  ostream << "[";
183  ostream << "x:" << formatter.format(translation.x()) << " ";
184  ostream << "y:" << formatter.format(translation.y()) << " ";
185  ostream << "z:" << formatter.format(translation.z());
186  ostream << "]";
187  ostream << "[";
188  ostream << "x:" << formatter.format(quaternion.x()) << " ";
189  ostream << "y:" << formatter.format(quaternion.y()) << " ";
190  ostream << "z:" << formatter.format(quaternion.z()) << " ";
191  ostream << "w:" << formatter.format(quaternion.w()) << " ";
192  ostream << "]";
193 
194  if ( formatter.tmp_formatting ) {
195  formatter.format.precision(prm_precision);
196  formatter.format.width(prm_width);
197  formatter.tmp_formatting = false;
198  }
199  formatter.ready_to_format = false;
200  }
201  return ostream;
202 }
203 
204 } // namespace Sophus
205 
206 /*****************************************************************************
207  ** ECL Format Type
208  *****************************************************************************/
209 
210 namespace ecl {
211 
212  template <>
213  class Format<Sophus::SE3f> : public Sophus::SE3fFormatter {};
214 
215 } // namespace ecl
216 
217 #endif /* ecl_linear_algebra_SOPHUS_FORMATTERS_HPP_ */
#define LOC
Stringify the line of code you are at.
Formatter for float types.
Definition: floats.hpp:496
std::ostream & operator<<(std::ostream &out, const SE3Group< T > &se3)
Definition: helpers.hpp:56
ecl::Format< float > format
Stream the formatter.
#define ecl_assert_throw(expression, exception)
Debug mode throw with a logical condition check.
Standard exception type, provides code location and error string.
SE3fFormatter(const int &w=-1, const unsigned int &p=2)
UsageError
FormatFloat< Number > & precision(const unsigned int p)
SE3fFormatter & operator()(const Sophus::SE3f &s)
RightAlign
#define ecl_assert_throw_decl(exception)
Assure throw exception declaration.
unsigned int precision()
FormatFloat< Number > & width(const int w)
Definition: floats.hpp:258


xbot_driver
Author(s): Roc, wangpeng@droid.ac.cn
autogenerated on Sat Oct 10 2020 03:27:37