MergeMapsStrategy.hpp

/* $Id$ */

#ifndef _MERGE_MAP_STRATEGY_HPP_
#define _MERGE_MAP_STRATEGY_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"

namespace jafar {
  namespace slammm {
    class StartNewMapStrategy;
    class MultiMapsSlam;
    class MapsPoseManager;
    
    class MergeMapsStrategy {
      public:
        MergeMapsStrategy();
        virtual ~MergeMapsStrategy();
        void setMultiMapsSlam( MultiMapsSlam* );
        MultiMapsSlam* multiMapsSlam();
        virtual void decideMergeMap( unsigned int currentMapId, slam::SlamEkf* currentMap ) = 0;
      private:
        MultiMapsSlam* m_multiMapsSlam;
    };
    
    class AlwaysMergeMapsStrategy : public MergeMapsStrategy{
      public:
        AlwaysMergeMapsStrategy( int minimalOverlapingFeaturesNumber );
        virtual void decideMergeMap( unsigned int currentMapId, slam::SlamEkf* currentMap );
      private:
        void getFeatureCov( slam::BaseFeature& baseFeature, jblas::vec& point, jblas::sym_mat& cov );
        void getFeatureSegCov( slam::BaseFeature& baseFeature, jblas::vec& line, jblas::sym_mat& cov );
      private:
        unsigned int m_minimalOverlapingFeaturesNumber;
        struct DMMInfoPoint;
        struct DMMInfoSegment;
    };
  }
}

#endif