8 #ifndef ecl_linear_algebra_SOPHUS_FORMATTERS_HPP_ 9 #define ecl_linear_algebra_SOPHUS_FORMATTERS_HPP_ 17 #include <sophus/se3.hpp> 144 template <
typename OutputStream>
153 const Sophus::SE3f *
s_;
156 template <
typename OutputStream>
160 "sophus object was not initialised " 161 "please pass the your matrix through () operator") );
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.") );
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);
180 Eigen::Vector3f translation = formatter.s_->translation();
181 Eigen::Quaternionf quaternion = formatter.s_->unit_quaternion();
183 ostream <<
"x:" << formatter.format(translation.x()) <<
" ";
184 ostream <<
"y:" << formatter.format(translation.y()) <<
" ";
185 ostream <<
"z:" << formatter.format(translation.z());
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()) <<
" ";
194 if ( formatter.tmp_formatting ) {
195 formatter.format.precision(prm_precision);
196 formatter.format.width(prm_width);
197 formatter.tmp_formatting =
false;
199 formatter.ready_to_format =
false;
OutputStream & operator<<(OutputStream &ostream, SE3fFormatter &formatter) ecl_assert_throw_decl(ecl
#define ecl_assert_throw(expression, exception)
#define ecl_assert_throw_decl(exception)