linear_algebra/sophus/formatters.hpp

Go to the documentation of this file.

/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef ecl_linear_algebra_SOPHUS_FORMATTERS_HPP_
#define ecl_linear_algebra_SOPHUS_FORMATTERS_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#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 ) ecl_assert_throw_decl(ecl::StandardException);

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_decl(ecl::StandardException) {

  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_ */