robot.hpp

/* $Id$ */

#ifndef SLAM_ROBOT_HPP
#define SLAM_ROBOT_HPP

#include "jmath/jblas.hpp"

#include "slam/abstractMapObject.hpp"

namespace jafar {
  namespace filter {
    class JacobianBlockCommandPredictModel;
  }
  namespace slam {

    class BaseRobot : public AbstractMapObject {
      friend class SlamEkf;
      public:
        jafar::filter::JacobianBlockCommandPredictModel& model;
      public:
        BaseRobot(unsigned int id, jafar::filter::JacobianBlockCommandPredictModel& model, std::size_t sizePose = 6);
        ~BaseRobot();
        std::size_t sizePose() const { return m_sizePose; }
        std::size_t sizeState() const;
        jblas::vec_range* refPose() { return m_refPose; }
        jblas::sym_mat_range* refPoseCov() { return m_refPoseCov; }
        const jblas::vec_range* refPose() const { return m_refPose; }
        const jblas::sym_mat_range* refPoseCov() const { return m_refPoseCov; }
      private:
        std::size_t m_sizePose;
        jblas::vec_range* m_refPose;
        jblas::sym_mat_range* m_refPoseCov;
    };
    
    
//     /** Base robot model.
//      *
//      * \ingroup slam
//      */
//     template<std::size_t sizePose, class PredictModel>
//     class BaseRobotModel {

//     protected:

//     public:

//       PredictModel predictModel;

//       BaseRobotModel() : 
//  predictModel()
//       {
//  JFR_PRECOND(sizePose < predictModel.sizeState(),
//        "BaseRobotModel::BaseRobotModel: invalid sizePose");
//       };

//       virtual ~BaseRobotModel() {};

//       std::size_t sizePose() const {return sizePose;};
//       std::size_t sizeState() const {return predictModel.sizeState();};


//     };




  } // namespace slam
} // namespace jafar


#endif // SLAM_ROBOT_HPP