.. _program_listing_file__tmp_ws_src_ecl_core_ecl_linear_algebra_include_ecl_linear_algebra_sophus_formatters.hpp:

Program Listing for File formatters.hpp
=======================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_ecl_core_ecl_linear_algebra_include_ecl_linear_algebra_sophus_formatters.hpp>` (``/tmp/ws/src/ecl_core/ecl_linear_algebra/include/ecl/linear_algebra/sophus/formatters.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

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