formatters.hpp
Go to the documentation of this file.
00001 
00004 /*****************************************************************************
00005 ** Ifdefs
00006 *****************************************************************************/
00007 
00008 #ifndef ecl_linear_algebra_SOPHUS_FORMATTERS_HPP_
00009 #define ecl_linear_algebra_SOPHUS_FORMATTERS_HPP_
00010 
00011 /*****************************************************************************
00012 ** Includes
00013 *****************************************************************************/
00014 
00015 #include <ecl/exceptions.hpp>
00016 #include <ecl/formatters.hpp>
00017 #include <sophus/se3.hpp>
00018 
00019 /*****************************************************************************
00020 ** Namespaces
00021 *****************************************************************************/
00022 
00023 namespace Sophus {
00024 
00025 /*****************************************************************************
00026 ** Interfaces
00027 *****************************************************************************/
00028 
00038 class SE3fFormatter
00039 {
00040 public:
00048   SE3fFormatter(const int &w = -1, const unsigned int &p = 2)
00049   : format(w, p, ecl::RightAlign)
00050   , tmp_width(w)
00051   , tmp_precision(p)
00052   , tmp_formatting(false)
00053   , ready_to_format(false)
00054   , s_(NULL)
00055 {}
00056   virtual ~SE3fFormatter() {}
00063   SE3fFormatter& precision( const unsigned int &p ) {
00064     format.precision(p);
00065     return *this;
00066   }
00075   SE3fFormatter& width( const int &w ) {
00076     format.width(w);
00077     return *this;
00078   }
00083   unsigned int precision() { return format.precision(); }
00084 
00089   int width() { return format.width(); }
00090 
00102   SE3fFormatter& operator() (const Sophus::SE3f & s ) {
00103     s_ = &s;
00104     ready_to_format = true;
00105     return (*this);
00106   }
00123   SE3fFormatter& operator() (const Sophus::SE3f & s, const int &w, const unsigned int &p) {
00124           s_ = &s;
00125           tmp_precision = p;
00126           tmp_width = w;
00127           tmp_formatting = true;
00128           ready_to_format = true;
00129           return (*this);
00130   }
00144   template <typename OutputStream>
00145   friend OutputStream& operator << ( OutputStream& ostream, SE3fFormatter & formatter ) ecl_assert_throw_decl(ecl::StandardException);
00146 
00147 private:
00148   ecl::Format<float> format;
00149   int tmp_width;
00150   unsigned int tmp_precision;
00151   bool tmp_formatting;
00152   bool ready_to_format;
00153   const Sophus::SE3f *s_;
00154 };
00155 
00156 template <typename OutputStream>
00157 OutputStream& operator << (OutputStream& ostream, SE3fFormatter & formatter ) ecl_assert_throw_decl(ecl::StandardException) {
00158 
00159   ecl_assert_throw( formatter.s_, ecl::StandardException(LOC,ecl::UsageError,"The formatter cannot print any data - "
00160           "sophus object was not initialised "
00161           "please pass the your matrix through () operator") );
00162 
00163   ecl_assert_throw(formatter.ready_to_format, ecl::StandardException(LOC,ecl::UsageError,"The formatter cannot print any data - "
00164           "either there is no data available, or you have tried to use the "
00165           "formatter more than once in a single streaming operation. "
00166           "C++ produces unspecified results when functors are used multiply "
00167           "in the same stream sequence, so this is not permitted here.") );
00168 
00169   if ( formatter.ready_to_format ) {
00170     unsigned int prm_precision = formatter.format.precision();;
00171     int prm_width = formatter.format.width();
00172     if ( formatter.tmp_formatting ) {
00173       formatter.format.precision(formatter.tmp_precision);
00174       formatter.format.width(formatter.tmp_width);
00175     }
00176 
00177     /*********************
00178     ** Stream Sophus
00179     **********************/
00180     Eigen::Vector3f translation = formatter.s_->translation();
00181     Eigen::Quaternionf quaternion = formatter.s_->unit_quaternion();
00182     ostream << "[";
00183     ostream <<  "x:" << formatter.format(translation.x()) << "  ";
00184     ostream <<  "y:" << formatter.format(translation.y()) << "  ";
00185     ostream <<  "z:" << formatter.format(translation.z());
00186     ostream << "]";
00187     ostream << "[";
00188     ostream <<  "x:" << formatter.format(quaternion.x()) << "  ";
00189     ostream <<  "y:" << formatter.format(quaternion.y()) << "  ";
00190     ostream <<  "z:" << formatter.format(quaternion.z()) << "  ";
00191     ostream <<  "w:" << formatter.format(quaternion.w()) << "  ";
00192     ostream << "]";
00193 
00194     if ( formatter.tmp_formatting ) {
00195       formatter.format.precision(prm_precision);
00196       formatter.format.width(prm_width);
00197       formatter.tmp_formatting = false;
00198     }
00199     formatter.ready_to_format = false;
00200   }
00201   return ostream;
00202 }
00203 
00204 } // namespace Sophus
00205 
00206 /*****************************************************************************
00207  ** ECL Format Type
00208  *****************************************************************************/
00209 
00210 namespace ecl {
00211 
00212   template <>
00213   class Format<Sophus::SE3f> : public Sophus::SE3fFormatter {};
00214 
00215 } // namespace ecl
00216 
00217 #endif /* ecl_linear_algebra_SOPHUS_FORMATTERS_HPP_ */


ecl_linear_algebra
Author(s): Daniel Stonier
autogenerated on Mon Jul 3 2017 02:21:35