robotCenteredConstantVelocity.hpp

Go to the documentation of this file.

#ifndef ROBOTCENTEREDCONSTANTVELOCITY_HPP_
#define ROBOTCENTEREDCONSTANTVELOCITY_HPP_

#include "rtslam/robotAbstract.hpp"

namespace jafar {
  namespace rtslam {

    class RobotCenteredConstantVelocity;
    typedef boost::shared_ptr<RobotCenteredConstantVelocity> robcenteredconstvel_ptr_t;


    class RobotCenteredConstantVelocity: public RobotAbstract {
      public:


        RobotCenteredConstantVelocity(const map_ptr_t & _mapPtr);

        ~RobotCenteredConstantVelocity(void) {
        }

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

        void getFrameForReframing(vec7 & frame) ;

        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() {
          int size_x_Rk_Rkm1 = 13 ; // P,Q,V,W
          int size_x_B_Rkm1  = 7  ; // P,Q
          return size_x_Rk_Rkm1 + size_x_B_Rkm1  ;
        }
        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;
          }
        }

      protected:
//        /**
//         * Split state vector.
//         *
//         * Extracts \a p, \a q, \a v and \a w from the state vector, \a x = [\a p, \a q, \a v, \a w].
//         * \param x the state vector
//         * \param p the position
//         * \param q the quaternion
//         * \param v the linear velocity
//         * \param w the angular velocity
//         */
//        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) {
//          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 Vx, class Vp, class Vq, class Vv, class Vw>
        inline void splitState(const Vx x, Vp & p, Vq & q, Vv & v, Vw & w, Vp & pBase, Vq & qBase) 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);
          pBase = ublas::subrange(x, 13, 16);
          qBase = ublas::subrange(x, 16, 20);
        }


//        /**
//         * Compose state vector.
//         *
//         * Composes the state vector with \a p, \a q, \a v and \a w, \a x = [\a p, \a q, \a v, \a w].
//         * \param p the position
//         * \param q the quaternion
//         * \param v the linear velocity
//         * \param w the angular velocity
//         * \param x the state vector
//         */
//        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) {
//          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 Vp, class Vq, class Vv, class Vw, class Vx>
        inline void unsplitState(const Vp & p, const Vq & q, const Vv & v, const Vw & w, const Vp & pBase, const Vq & qBase, 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    ;
          ublas::subrange(x, 13, 16) = pBase;
          ublas::subrange(x, 16, 20) = qBase;
        }


        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
        {
          vec7 lastJump; // F contains [p,q]' of the last reframe prediction.
          mat33 PNEW_v; 
          mat43 QNEW_wdt; 
  //        mat43 PBASENEW_p; ///<    Temporary Jacobian matrix
  //        mat44 PBASENEW_q; ///<    Temporary Jacobian matrix
  //        mat43 QBASENEW_p; ///<    Temporary Jacobian matrix
  //        mat44 QBASENEW_q; ///<    Temporary Jacobian matrix
  //        mat44 QNEW_q; ///<      Temporary Jacobian matrix
        };
        mutable TempVariables tempvars0, tempvars1;

    };
  }
}

#endif /* ROBOTCENTEREDCONSTANTVELOCITY_HPP_ */