mapManager.hpp

#ifndef MAPMANAGER_HPP_
#define MAPMANAGER_HPP_

#include <list>

#include "rtslam/parents.hpp"
#include "rtslam/mapAbstract.hpp"
#include "rtslam/landmarkFactory.hpp"
#include "rtslam/spaceGrid.hpp"


namespace jafar {
  namespace rtslam {

    class LandmarkAbstract;
    class DataManagerAbstract;
    
    class MapManagerAbstract:
          public ParentOf<LandmarkAbstract>,
          public ParentOf<DataManagerAbstract>,
          public ChildOf<MapAbstract>,
          public boost::enable_shared_from_this<MapManagerAbstract>
    {
      public:
        // define the function linkToParentMap().
        ENABLE_LINK_TO_PARENT(MapAbstract,Map,MapManagerAbstract);
        // define the functions mapPtr() and map().
        ENABLE_ACCESS_TO_PARENT(MapAbstract,map);
        // define the type LandmarkList, and the function landmarkList().
        ENABLE_ACCESS_TO_CHILDREN(LandmarkAbstract,Landmark,landmark);
        // define the type DataManagerList, and the function dataManagerList().
        ENABLE_ACCESS_TO_CHILDREN(DataManagerAbstract,DataManager,dataManager);

      protected:
        landmark_factory_ptr_t lmkFactory;
      public:
        MapManagerAbstract(landmark_factory_ptr_t lmkFactory):
          lmkFactory(lmkFactory) {}
        virtual ~MapManagerAbstract(void) {
        }
        virtual bool mapSpaceForInit() {
          return mapPtr()->unusedStates(lmkFactory->sizeInit());
        }
        observation_ptr_t createNewLandmark(data_manager_ptr_t dmaOrigin);
        void reparametrizeLandmark(landmark_ptr_t lmkIter);
        LandmarkList::iterator reparametrizeLandmark(LandmarkList::iterator lmkIter)
        { // FIXME do better than this! will crash if only one element.
          landmark_ptr_t lmkPtr = *lmkIter;
          lmkIter++;
          reparametrizeLandmark(lmkPtr);
          lmkIter--;
          return lmkIter;
        }
        void unregisterLandmark(landmark_ptr_t lmkPtr, bool liberateFilter = true);
        LandmarkList::iterator unregisterLandmark(LandmarkList::iterator lmkIter, bool liberateFilter = true)
        { // FIXME do better than this! will crash if only one element.
          landmark_ptr_t lmkPtr = *lmkIter;
          lmkIter++;
          unregisterLandmark(lmkPtr, liberateFilter);
          lmkIter--;
          return lmkIter;
        }

        virtual void manage(void) = 0;
        virtual bool isExclusive(observation_ptr_t obsPtr) = 0;
    };

    
    class MapManager: public MapManagerAbstract {
      protected:
        double reparTh;    
        double killSizeTh; 
      protected:
        virtual void manageReparametrization();
        virtual void manageDefaultDeletion();
        virtual void manageDeletion() {}; // to overload
      public:
        MapManager(landmark_factory_ptr_t lmkFactory, double reparTh = 0.1, double killSizeTh = 100000):
          MapManagerAbstract(lmkFactory), reparTh(reparTh), killSizeTh(killSizeTh) {}
        virtual ~MapManager(void) {}
                
        virtual void manage()
        {
          manageDefaultDeletion();
          manageDeletion();
          manageReparametrization();
        }
        
        virtual bool isExclusive(observation_ptr_t obsPtr)
        {
          return true;
        }
    };

    
    class MapManagerGrid: public MapManager {
      protected:
        struct Cell { std::list<landmark_ptr_t> landmarks; };

        SphericalGrid<Cell> grid_visible_updated;
        SphericalGrid<Cell> grid_visible_failed;

      protected:
        void fillGrids();
        void processGrid(SphericalGrid<Cell> & grid);

      public:
        MapManagerGrid(landmark_factory_ptr_t lmkFactory, double reparTh, double killSizeTh,
                        double gridAngularRes, double gridDistInit, double gridDistFactor, int gridNDist, double gridPhiFactor):
          MapManager(lmkFactory, reparTh, killSizeTh),
          grid_visible_updated(gridAngularRes, gridDistInit, gridDistFactor, gridNDist, gridPhiFactor),
          grid_visible_failed(gridAngularRes, gridDistInit, gridDistFactor, gridNDist, gridPhiFactor)
          {}
        virtual void manageDeletion();
    };


    class MapManagerOdometry: public MapManagerGrid {
      public:
        MapManagerOdometry(landmark_factory_ptr_t lmkFactory, double reparTh, double killSizeTh,
                           double gridAngularRes, double gridDistInit, double gridDistFactor, int gridNDist, double gridPhiFactor):
          MapManagerGrid(lmkFactory, reparTh, killSizeTh, gridAngularRes, gridDistInit, gridDistFactor, gridNDist, gridPhiFactor)
        {}
        virtual void manageDeletion();
        virtual bool isExclusive(observation_ptr_t obsPtr);
    };


    class MapManagerGlobal: public MapManagerGrid {
      protected:
        double killSearchTh;      
        double killMatchTh;       
        double killConsistencyTh; 
        double killUncertaintyTh;     

        SphericalGrid<MapManagerGrid::Cell> grid_invisible;

      protected:
        void fillGrids(bool fill_visible);

      public:
        MapManagerGlobal(landmark_factory_ptr_t lmkFactory, double reparTh, double killSizeTh,
                        double killSearchTh, double killMatchTh, double killConsistencyTh, double killUncertaintyTh,
                        double gridAngularRes, double gridDistInit, double gridDistFactor, int gridNDist, double gridPhiFactor):
          MapManagerGrid(lmkFactory, reparTh, killSizeTh, gridAngularRes, gridDistInit, gridDistFactor, gridNDist, gridPhiFactor),
          killSearchTh(killSearchTh), killMatchTh(killMatchTh), killConsistencyTh(killConsistencyTh), killUncertaintyTh(killUncertaintyTh),
          grid_invisible(gridAngularRes, gridDistInit, gridDistFactor, gridNDist, gridPhiFactor)
          {}
        virtual void manageDeletion();
        virtual bool mapSpaceForInit();
        virtual bool isExclusive(observation_ptr_t obsPtr);
    };
    
    
    class MapManagerLocal: public MapManager {
      public:
        MapManagerLocal(landmark_factory_ptr_t lmkFactory, double reparTh, double killSizeTh):
          MapManager(lmkFactory, reparTh, killSizeTh) {}
        virtual void manageDeletion()
        {
          // TODO
        }
    };
    
    
  }
}

#endif // #ifndef MAPMANAGER_HPP_