MultiMapManager.hpp

/* $Id$ */

#ifndef _MULTI_MAPS_MANAGER_HPP_
#define _MULTI_MAPS_MANAGER_HPP_

#include <kernel/dataLog.hpp>
#include <geom/t3dEuler.hpp>
#include <slam/slamEkf.hpp>
#include <slam/robot.hpp>
#include <slam/baseSlam.hpp>
#include <slam/MapManagerFactory.hpp>

#include "slammm/StartNewMapStrategy.hpp"
#include "slammm/MergeMapsStrategy.hpp"

namespace jafar {
  namespace slammm {
    class StartNewMapStrategy;
    class MultiMapsSlam;
    class MapsPoseManager;
    class MultiMapEventListener;
    
    class MultiMapsSlam : public slam::BaseSlam, public kernel::DataLoggable  {
      typedef std::map<unsigned int, slam::FeatureObserveModel*> FeatureObserveModelsContType;
      public:
        MultiMapsSlam(std::size_t sizeMax_, MapsPoseManager* _mapsPoseManager, StartNewMapStrategy* _startNewMapStrategy, MergeMapsStrategy* _mergeMapsStrategy, slam::MapManagerFactory* mapManagerFactory = 0, int firstMapId = 0 );
        ~MultiMapsSlam();
      private:
        template<class SequenceType1, class SequenceType2>
        void processObservationsImpl(unsigned int frameIndex_,
              SequenceType1 const& knownObs,
              SequenceType2 const& newObs, unsigned int _robotId, int events_  );
      public:
        void mergeMap( unsigned int _map1Id, unsigned int _map2Id, const geom::T3DEuler& transfo );
        slam::SlamEkf* getCurrentMap( unsigned int _robotId = 0 );
        const slam::SlamEkf* getCurrentMap( unsigned int _robotId = 0 ) const;
        slam::SlamEkf* getMap( unsigned int _mapId, unsigned int _robotId = 0 );

        unsigned int mapId( slam::SlamEkf* ) const;

        void getWorldRobotPose(jafar::geom::T3DEuler& pose, unsigned int _robotId = 0 ) const;
        jafar::geom::T3DEuler getWorldRobotPose( unsigned int _robotId = 0 ) const;
        
        void getWorldFeatureState(jblas::vec& _result , unsigned int _featureId, unsigned int _currentMapId ) const;
        void getWorldFeatureState(jblas::vec& _result, jblas::sym_mat& _resultCov, unsigned int _featureId, unsigned int _currentMapId ) const;
        jafar::geom::T3DEuler getWorldToCurrentMapTransformation( unsigned int _robotId = 0 ) const;
        jafar::geom::T3DEuler getWorldToMapTransformation( unsigned int _mapId ) const;

        void beginBrowseMaps();
        bool hasNextMap();
        slam::SlamEkf* nextMap();
        
        const std::map< unsigned int, slam::SlamEkf*> getMaps() const;
        
        slam::SlamEkf* getGlobalMap( ) const;
        void addRobot( slam::BaseRobot* );
        void addRobot( slam::BaseRobot*, const jblas::vec& _robotState, const jblas::sym_mat& _robotStateCov );
        void setSensor(slam::FeatureObserveModel* model, unsigned int sensorId, unsigned int _robotId );
        void setRobotToSensor(jblas::vec const& robotToSensor, int sensorId, unsigned int _robotId );
        // Implementation of BaseSlam functions
        virtual void predict(unsigned int _robotId, jblas::vec const& u);
        virtual bool hasFeature(unsigned int id, unsigned int _robotId = 0) const;
        virtual void getRobotPose(jafar::geom::T3DEuler& pose, unsigned int _robotId = 0) const;
        virtual void removeFeature(unsigned int id);
        virtual void addEventListener(slam::SlamEventListener& listener);
        virtual void removeEventListener(slam::SlamEventListener& listener);
        virtual void processObservations(unsigned int frameIndex_,
             std::list<slam::Observation*> const& knownObs,
             std::list<slam::Observation*> const& newObs, unsigned int robotId_ = 0, int events_ = slam::SlamEventListener::ALL );
        virtual void processObservations(unsigned int frameIndex_,
             std::vector<slam::Observation*> const& knownObs,
             std::vector<slam::Observation*> const& newObs, unsigned int robotId_ = 0, int events_ = slam::SlamEventListener::ALL );
        template<typename SequenceType>
        void processObservations(unsigned int frameIndex_,
                                 SequenceType const& allObs, unsigned int robotId_ = 0, int events_ = slam::SlamEventListener::ALL );
      public:
        // Functions related to events
        void rendezVous( unsigned int _firstRobotId, unsigned int _secondRobotId, const geom::T3DEuler& firstToSecond );
        void gpsFix( const std::map<unsigned int, geom::T3DEuler>& worldToRobots );
        void gpsFix( unsigned int _robotId, const geom::T3DEuler& worldToRobot );
        void closeLoop( unsigned int _firstMapId, unsigned int _secondMapId, const geom::T3DEuler& firstToSecond );
        bool canCloseLoop( unsigned int _firstMapId, unsigned int _secondMapId) const;
      public:
        virtual void writeLogHeader(kernel::DataLogger& log) const;
        virtual void writeLogData(kernel::DataLogger& log) const;
        virtual void addMembersToLog(kernel::DataLogger& log) const;
        void setEventsDataLogger( kernel::DataLogger* log );
      public:
        void addEventListener( MultiMapEventListener* listener );
        void removeEventListener( MultiMapEventListener* listener );
      public:
        void printInfo();
      private: // Fucntion related to map management
        struct RobotMaps;
        void initNewMap( unsigned int _robotId);
        void startNewMap( RobotMaps& );
      public:
        void startNewMap( unsigned int _robotId);
      private:
        void updateCacheWorldToCurrentMap();
      private:
        struct RobotMaps {
          RobotMaps( ) : currentMapId(-1), currentMap(0), baseRobot(0)
          {
          }
          unsigned int currentMapId;
          slam::SlamEkf* currentMap;
          std::map< unsigned int, slam::SlamEkf*> maps;
          jafar::geom::T3DEuler worldToCurrentMap;
          slam::BaseRobot* baseRobot;
          FeatureObserveModelsContType featureObserveModels;
          std::map<int, jblas::vec> robotToSensors;
        };
        int nextMapId;
        struct SlamEkfInfo {
          slam::SlamEkf* slamEkf;
          std::map< unsigned int, unsigned int > robotPositionAge;
        };
        std::map< unsigned int, SlamEkfInfo> m_maps;
        std::map<unsigned int, RobotMaps> m_robotMaps;
        MapsPoseManager* m_mapsPoseManager;
        StartNewMapStrategy* m_startNewMapStrategy;
        MergeMapsStrategy* m_mergeMapsStrategy;
        slam::MapManagerFactory* m_mapManagerFactory;
        std::size_t m_sizeMax;
        std::list<unsigned int > m_sensorsIds; 
        std::list<slam::SlamEventListener*> m_listeners;
        std::map< unsigned int, SlamEkfInfo>::iterator browseFeaturesIt;
        kernel::DataLogger* m_eventsLogger;
        unsigned int m_frameIndex;
        std::list<MultiMapEventListener*> m_eventListners;
    };
    template<class SequenceType1, class SequenceType2>
    void MultiMapsSlam::processObservationsImpl(unsigned int frameIndex_,
          SequenceType1 const& knownObs,
          SequenceType2 const& newObs, unsigned int _robotId, int events_ )
    {
      // Get the robot
      std::map<unsigned int, RobotMaps>::iterator it = m_robotMaps.find( _robotId );
      JFR_ASSERT( it != m_robotMaps.end(), "No robot " << _robotId );
      bool newmap = m_startNewMapStrategy->startNewMap( *it->second.currentMap, _robotId, newObs.size() );
      if(newmap)
      {
        m_mergeMapsStrategy->decideMergeMap( it->second.currentMapId, it->second.currentMap );
        startNewMap( it->second );
        SequenceType2 obses;
        for(typename SequenceType1::const_iterator it2 = knownObs.begin(); it2 != knownObs.end(); ++it2)
        {
          obses.push_back(*it2);
        }
        for(typename SequenceType2::const_iterator it2 = newObs.begin(); it2 != newObs.end(); ++it2)
        {
          obses.push_back(*it2);
        }
        it->second.currentMap->processObservations( frameIndex_, SequenceType1(), obses, _robotId, events_ );
      } else {
        // Process observation
        it->second.currentMap->processObservations( frameIndex_, knownObs, newObs, _robotId, events_ );
      }
      // Decide wether to start a new map
      JFR_DEBUG( "================================== Nb of features = " << it->second.currentMap->featuresMap.size() << " new obs = " << newObs.size());
    }
    
    template<class SequenceType>
    void MultiMapsSlam::processObservations(unsigned int frameIndex_, SequenceType const& allObs, unsigned int _robotId, int events_ ) 
    {
      m_frameIndex = frameIndex_;
      // Get the robot
      std::map<unsigned int, RobotMaps>::iterator itRobot = m_robotMaps.find( _robotId );
      JFR_ASSERT( itRobot != m_robotMaps.end(), "No robot " << _robotId );

      SequenceType knownObs;
      SequenceType newObs;

      for (typename SequenceType::const_iterator it = allObs.begin() ; it != allObs.end() ; ++it) {
        if ( itRobot->second.currentMap->isFeatureKnown((**it).id) )
          knownObs.push_back(*it);
        else
          newObs.push_back(*it);
      }

      processObservations(frameIndex_, knownObs, newObs, _robotId, events_);

    }
  }
}

#endif