mapManager.hpp

/* $Id$ */

#ifndef SLAM_MAP_MANAGER_HPP
#define SLAM_MAP_MANAGER_HPP

#include <list>

#include "jmath/jblas.hpp"

#include "slam/abstractMapManager.hpp"
#include "slam/feature.hpp"
#include "slam/slamEvents.hpp"

// #define DEBUG_FREE_STATE_COLLECTOR

namespace jafar {
  namespace slam {

    class SlamEkf;
    
    class FreeStateCollector {
        struct StateBlock {
          StateBlock(std::size_t index_, std::size_t size_) : index(index_), size(size_) {}
          std::size_t index;
          std::size_t size;
          inline bool operator <(StateBlock const& block2) const {
            return index < block2.index;
          }
        };
#ifdef DEBUG_FREE_STATE_COLLECTOR
        struct HistoryItem {
          std::string what;
          int index;
          int countObject;
        };
        int m_countObject;
        std::list<HistoryItem> m_history;
#endif

        typedef std::list<StateBlock> StateBlockCont;
        StateBlockCont m_freeStateBlocks;
        SlamEkf& m_slam;
      public:
        FreeStateCollector(SlamEkf& slam_);
        ~FreeStateCollector();
        std::size_t getFreeIndex(std::size_t sizeState);
        void collectIndex( std::size_t index, std::size_t sizeState);
        void clear();
        void defrag();
        void dump() const;
    };

    class DefaultMapManager : public AbstractMapManager {
  
    public:

      DefaultMapManager(SlamEkf& slam_) : AbstractMapManager(slam_) {}

      void clear();
      void setMapObjectState(AbstractMapObject& mapObj);
      void removeMapObject(AbstractMapObject const& mapObj);

    };

    class LocalMapManager : public AbstractMapManager, public SlamEventAdapter {

    protected:

      const std::size_t m_sizeFeatureState;

      const std::size_t m_sizeLocalMapMax;

      FreeStateCollector m_freeStateCollector;

     public:

      LocalMapManager(SlamEkf& slam_,
          std::size_t sizeLocalMapMax,
          std::size_t sizeFeatureState);
      ~LocalMapManager();

      void clear();
      void setMapObjectState(AbstractMapObject& mapObj);
      void removeMapObject(AbstractMapObject const& mapObj);

      virtual void endProcessObservations(unsigned int robotId_);
    };

#ifndef SWIG
    namespace details {

      struct VoxelCoord {
  int x,y,z;
      };

      std::ostream& operator <<(std::ostream& s, VoxelCoord  const& v);

      inline bool operator <(VoxelCoord const& v1, VoxelCoord const& v2)
      {
  if (v1.x != v2.x)
    return v1.x < v2.x;
  else if (v1.y != v2.y)
    return v1.y < v2.y;
  else
    return v1.z < v2.z;
      }
    } // namespace details
#endif // SWIG

    class GlobalMapManager : public AbstractMapManager, public SlamEventAdapter {

     protected:
      unsigned int m_nbObsMin;
      double m_minUncertainty;
      double m_sizeVoxel;
      typedef std::map<details::VoxelCoord, BaseFeature*> VoxelsType;
      VoxelsType m_voxels;
      details::VoxelCoord getVoxelCoord(BaseFeature const& f);
      FreeStateCollector m_freeStateCollector;

      int m_ndel_density;
      int m_ndel_obs;
      int m_ndel_uncertainty;

      class Stats
      {
       public:
        int m_ndel_density;
        int m_ndel_obs;
        int m_ndel_uncertainty;

        Stats(): m_ndel_density(0), m_ndel_obs(0), m_ndel_uncertainty(0) {}
      } stats;


     public:
      GlobalMapManager(SlamEkf& slam_, double sizeVoxel_, unsigned int nbObsMin, double minUncertainty);
      GlobalMapManager(SlamEkf& slam_);
      ~GlobalMapManager();

      void setParams(double sizeVoxel = 1.0, unsigned int nbObsMin = 5, double minUncertainty = 0.01);
      void clear();
      void setMapObjectState(AbstractMapObject& mapObj);
      void removeMapObject(AbstractMapObject const& mapObj);
      virtual void endProcessObservations(unsigned int robotId_);
      virtual void writeLogHeader(jafar::kernel::DataLogger& log) const ;
      virtual void writeLogData(jafar::kernel::DataLogger& log) const;
      virtual void writeLogStats(jafar::kernel::DataLogger& log) const;
    };

 }
}

#endif // SLAM_MAP_MANAGER_HPP