robotConstantVelocity.hpp

Go to the documentation of this file.

#ifndef ROBOTCONSTANTVELOCITY_HPP_
#define ROBOTCONSTANTVELOCITY_HPP_

#include "rtslam/robotAbstract.hpp"

namespace jafar {
  namespace rtslam {

    class RobotConstantVelocity;
    typedef boost::shared_ptr<RobotConstantVelocity> robconstvel_ptr_t;


    class RobotConstantVelocity: public RobotAbstract {
      public:


        RobotConstantVelocity(const map_ptr_t & _mapPtr);

        ~RobotConstantVelocity(void) {
        }

        virtual std::string typeName() const {
          return "Constant-velocity";
        }


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

        void computePertJacobian();

        static size_t size() {
          return 13;
        }
        static size_t size_control() {
          return 0;
        }
        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();}

        void setVelocityStd(double velLinStd, double velAngStd){
          for (size_t i = pose.size(); i < pose.size() + 3; i++){
            state.P(i,i) = velLinStd*velLinStd;
          }
          for (size_t i = pose.size() + 3; i < pose.size() + 6; i++){
            state.P(i,i) = velAngStd*velAngStd;
          }
        }

        virtual void writeLogHeader(kernel::DataLogger& log) const;
        virtual void writeLogData(kernel::DataLogger& log) const;
        
      protected:
        template<class Vx, class Vp, class Vq, class Vv, class Vw>
        inline void splitState(const Vx x, Vp & p, Vq & q, Vv & v, Vw & w) const {
          p = ublas::subrange(x, 0, 3);
          q = ublas::subrange(x, 3, 7);
          v = ublas::subrange(x, 7, 10);
          w = ublas::subrange(x, 10, 13);
        }


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


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

      private:
        // temporary matrices to speed up Jacobian computation
        struct TempVariables
        {
          mat33 PNEW_v; 
          mat43 QWDT_wdt; 
          mat44 QNEW_qwdt; 
          mat43 QNEW_wdt; 
          mat44 QNEW_q; 
          mat44 QNORM_qnew; 
        };
        mutable TempVariables tempvars0, tempvars1;

    };
  }
}

#endif /* ROBOTCONSTANTVELOCITY_HPP_ */