Program Listing for File formatters.hpp
↰ Return to documentation for file (include/ecl/linear_algebra/sophus/formatters.hpp
)
/*****************************************************************************
** Ifdefs
*****************************************************************************/
#ifndef ecl_linear_algebra_SOPHUS_FORMATTERS_HPP_
#define ecl_linear_algebra_SOPHUS_FORMATTERS_HPP_
/*****************************************************************************
** Includes
*****************************************************************************/
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ecl/exceptions.hpp>
#include <ecl/formatters.hpp>
#include <sophus/se3.hpp>
/*****************************************************************************
** Namespaces
*****************************************************************************/
namespace Sophus {
/*****************************************************************************
** Interfaces
*****************************************************************************/
class SE3fFormatter
{
public:
SE3fFormatter(const int &w = -1, const unsigned int &p = 2)
: format(w, p, ecl::RightAlign)
, tmp_width(w)
, tmp_precision(p)
, tmp_formatting(false)
, ready_to_format(false)
, s_(NULL)
{}
virtual ~SE3fFormatter() {}
SE3fFormatter& precision( const unsigned int &p ) {
format.precision(p);
return *this;
}
SE3fFormatter& width( const int &w ) {
format.width(w);
return *this;
}
unsigned int precision() { return format.precision(); }
int width() { return format.width(); }
SE3fFormatter& operator() (const Sophus::SE3f & s ) {
s_ = &s;
ready_to_format = true;
return (*this);
}
SE3fFormatter& operator() (const Sophus::SE3f & s, const int &w, const unsigned int &p) {
s_ = &s;
tmp_precision = p;
tmp_width = w;
tmp_formatting = true;
ready_to_format = true;
return (*this);
}
template <typename OutputStream>
friend OutputStream& operator << ( OutputStream& ostream, SE3fFormatter & formatter );
private:
ecl::Format<float> format;
int tmp_width;
unsigned int tmp_precision;
bool tmp_formatting;
bool ready_to_format;
const Sophus::SE3f *s_;
};
template <typename OutputStream>
OutputStream& operator << (OutputStream& ostream, SE3fFormatter & formatter ) {
ecl_assert_throw( formatter.s_, ecl::StandardException(LOC,ecl::UsageError,"The formatter cannot print any data - "
"sophus object was not initialised "
"please pass the your matrix through () operator") );
ecl_assert_throw(formatter.ready_to_format, ecl::StandardException(LOC,ecl::UsageError,"The formatter cannot print any data - "
"either there is no data available, or you have tried to use the "
"formatter more than once in a single streaming operation. "
"C++ produces unspecified results when functors are used multiply "
"in the same stream sequence, so this is not permitted here.") );
if ( formatter.ready_to_format ) {
unsigned int prm_precision = formatter.format.precision();;
int prm_width = formatter.format.width();
if ( formatter.tmp_formatting ) {
formatter.format.precision(formatter.tmp_precision);
formatter.format.width(formatter.tmp_width);
}
/*********************
** Stream Sophus
**********************/
Eigen::Vector3f translation = formatter.s_->translation();
Eigen::Quaternionf quaternion = formatter.s_->unit_quaternion();
ostream << "[";
ostream << "x:" << formatter.format(translation.x()) << " ";
ostream << "y:" << formatter.format(translation.y()) << " ";
ostream << "z:" << formatter.format(translation.z());
ostream << "]";
ostream << "[";
ostream << "x:" << formatter.format(quaternion.x()) << " ";
ostream << "y:" << formatter.format(quaternion.y()) << " ";
ostream << "z:" << formatter.format(quaternion.z()) << " ";
ostream << "w:" << formatter.format(quaternion.w()) << " ";
ostream << "]";
if ( formatter.tmp_formatting ) {
formatter.format.precision(prm_precision);
formatter.format.width(prm_width);
formatter.tmp_formatting = false;
}
formatter.ready_to_format = false;
}
return ostream;
}
} // namespace Sophus
/*****************************************************************************
** ECL Format Type
*****************************************************************************/
namespace ecl {
template <>
class Format<Sophus::SE3f> : public Sophus::SE3fFormatter {};
} // namespace ecl
#endif /* ecl_linear_algebra_SOPHUS_FORMATTERS_HPP_ */