mapAbstract.hpp

Go to the documentation of this file.

#ifndef MAPABSTRACT_HPP_
#define MAPABSTRACT_HPP_

#include "kernel/dataLog.hpp"
#include "jmath/jblas.hpp"
#include "rtslam/rtSlam.hpp"

#include "rtslam/gaussian.hpp"
#include "rtslam/kalmanFilter.hpp"
#include "rtslam/parents.hpp"
#include "rtslam/worldAbstract.hpp"

namespace jafar {
  namespace rtslam {


    // some forward declarations.
    class RobotAbstract;
    class LandmarkAbstract;
    class MapManagerAbstract;
    class ObservationFactory;

    class MapAbstract: public ObjectAbstract, public ChildOf<WorldAbstract>, 
      public boost::enable_shared_from_this<MapAbstract>,
      public ParentOf<RobotAbstract> , public ParentOf<MapManagerAbstract>, public kernel::DataLoggable {

      ENABLE_LINK_TO_PARENT(WorldAbstract,World,MapAbstract);
      ENABLE_ACCESS_TO_PARENT(WorldAbstract,world);
      
      ENABLE_ACCESS_TO_CHILDREN(RobotAbstract,Robot,robot);
      ENABLE_ACCESS_TO_CHILDREN(MapManagerAbstract,MapManager,mapManager);

    public:
        friend std::ostream& operator <<(std::ostream & s, MapAbstract const & map);

      public:


        MapAbstract(size_t _max_size);
        MapAbstract(const ekfInd_ptr_t & ekfPtr);

        virtual ~MapAbstract() {
        }

        virtual std::string categoryName() const {
          return "MAP";
        }

        Gaussian state;

        ekfInd_ptr_t filterPtr;
//        ExtendedKalmanFilterIndirect filter;

        size_t max_size;
        size_t current_size;
        jblas::vecb used_states;

        inline jblas::ind_array ia_used_states() {
          jblas::ind_array res;
          res = jmath::ublasExtra::ia_set(used_states);
          return res;
        }


        jblas::vec & x();
        jblas::sym_mat & P();
        double & x(size_t i);
        double & P(size_t i, size_t j);

        inline std::size_t unusedStates() const {
          return max_size - current_size;
        }


        inline bool unusedStates(const size_t N) const {
          return (unusedStates() >= N);
        }

        jblas::ind_array reserveStates(const std::size_t _size);

                    jblas::ind_array convertStates(const jblas::ind_array & _ia,const std::size_t N,jblas::ind_array & _icomp);

        void liberateStates(const jblas::ind_array & _ia);

        void clear();
        void fillSeq();
        void fillDiag();
        void fillDiagSeq();
        void fillRndm();

        virtual void writeLogHeader(kernel::DataLogger& log) const;
        virtual void writeLogData(kernel::DataLogger& log) const;
        
      private:


    };

  }
}

#endif /* MAPABSTRACT_HPP_ */