robotOdometry.hpp

Go to the documentation of this file.

#ifndef ROBOTODOMETRY_HPP_
#define ROBOTODOMETRY_HPP_

#include "rtslam/robotAbstract.hpp"

namespace jafar {
  namespace rtslam {

    class RobotOdometry;
    typedef boost::shared_ptr<RobotOdometry> robodo_ptr_t;

    class RobotOdometry: public RobotAbstract {
      public:
        using RobotAbstract::move;

        RobotOdometry(const map_ptr_t _mapPtr);

        ~RobotOdometry(void) {
        }

        virtual std::string typeName() const {
          return "Odometry";
        }

        void move_func(const vec & _x, const vec & _u, const vec & _n,
            const double _dt, vec & _xnew, mat & _XNEW_x, mat & _XNEW_u, unsigned tempSet = 0) const;
        
        bool move(double time);

        void init_func(const vec & _x, const vec & _u, vec & _xnew);

        static size_t size() {
          return 7;
        }

        static size_t size_control() {
          return 6;
        }

        static size_t size_perturbation() {
          return 6;
        }
        virtual size_t mySize() {
          return size();
        }
        virtual size_t mySize_control() {
          return size_control();
        }
        virtual size_t mySize_perturbation() {
          return size_perturbation();
        }
        
        virtual void writeLogHeader(kernel::DataLogger& log) const;
        virtual void writeLogData(kernel::DataLogger& log) const;
        

      protected:
        template<class Vx, class Vp, class Vq>
        inline void splitState(const Vx x, Vp & p, Vq & q) const {
          p = ublas::subrange(x, 0, 3);
          q = ublas::subrange(x, 3, 7);
        }

        template<class Vp, class Vq, class Vx>
        inline void unsplitState(const Vp & p, const Vq & q, Vx & x) const {
          ublas::subrange(x, 0, 3) = p;
          ublas::subrange(x, 3, 7) = q;
        }

        template<class Vu, class V>
        inline void splitControl(Vu & u, V & dx, V & dv) const {
          dx = project(u, ublas::range(0, 3));
          dv = project(u, ublas::range(3, 6));
        }

      private:
        // temporary matrices to speed up Jacobian computation
        struct TempVariables
        {
          mat33 PNEW_dx;  
          mat43 QDV_dv; 
          mat44 QNEW_qdv; 
          mat43 QNEW_dv;  
          mat44 QNEW_q; 
        };
        mutable TempVariables tempvars0, tempvars1;
    };
  }
}

#endif /* ROBOTODOMETRY_HPP_ */