slamEkf.hpp

/* $Id$ */

#ifndef SLAM_SLAM_EKF_HPP
#define SLAM_SLAM_EKF_HPP

#include <string>
#include <vector>
#include <map>

#include "kernel/jafarException.hpp"
#include "kernel/timingTools.hpp"
#include "kernel/dataLog.hpp"

#include "jmath/jblas.hpp"

#include "geom/t3dEuler.hpp"

#include "filter/kalmanFilter.hpp"

#include "slam/baseSlam.hpp"
#include "slam/slamException.hpp"
#include "slam/mapManager.hpp"
#include "slam/feature.hpp"
#include "slam/pointFeature.hpp"
#include "slam/segmentFeature.hpp"
#include "slam/segmentAnchorPluckerFeature.hpp"
#include "slam/segmentInvDepthFeature.hpp"
#include "slam/slamEvents.hpp"
#include "slam/poseCopy.hpp"
#include "slam/robot.hpp"

namespace jafar {
  namespace slam {
    class BaseRobot;
    class SlamEkf : public BaseSlam, public jafar::kernel::DataLoggable {

    private:

      void initFeatureState(BaseFeature& feature, 
             FeatureObserveModel& observeModel,       
             Observation const& obs);

    protected:
      bool m_deleteSensors;

      unsigned int currentFrameIndex;
      unsigned int prevFrameIndex;

      jafar::filter::BlockExtendedKalmanFilter filter;      

      double mahalanobisDistanceToRemoveLandmark;

      std::size_t m_sizeRobotState;
      std::size_t m_sizeRobotPose;

//       jblas::vec_range m_refPose;
//       jblas::sym_mat_range m_refPoseCov;

      AbstractMapManager* mapManager;

      typedef std::list<SlamEventListener*> SlamEventListenersList;
      SlamEventListenersList slamEventListeners;

      bool enableInitCoordinateFrame; 
      
      typedef std::map<unsigned int, FeatureObserveModel*> FeatureObserveModelsContType;
      FeatureObserveModelsContType featureObserveModels;

      class Stats
      {
       public:
        double poseUncertaintyVolume;
        double mapUncertaintyLevel;
        int prevNbFoundLandmarks;
        int nbFoundLandmarks; // nbObservedLandmarks + nbInconsistentUpdates + nbNewLandmarks
        int nbLostLandmarks; // prevNbFoundLandmarks - (nbObservedLandmarks + nbInconsistentUpdates)
        int nbInconsistentLandmarks; // nbInconsistentUpdates
        int nbObservedLandmarks; // nbObservedLandmarks
        int nframes;

        Stats(): poseUncertaintyVolume(0), mapUncertaintyLevel(0), prevNbFoundLandmarks(0),
          nbFoundLandmarks(0), nbLostLandmarks(0), nbInconsistentLandmarks(0), nbObservedLandmarks(0),
          nframes(0) {}
      } stats;

      virtual void observeNewFeature(Observation* obs_);
      
      virtual void observeKnownFeature(Observation* obs_);

      void observeKnownFeature(Observation const& obs, BaseFeature& feature, BaseFeatureObserveModel& model);

      void applyConstraints(BaseFeature& feature);

      void fixFeature(BaseFeature& feature);

      //       void applyAllConstraints();

      void copyCurrentPoseAt(std::size_t index, int _robotId = 0);

      geom::T3D* m_refToRobot;
      geom::T3D* m_robotToRef;

      static BaseFeature* featureFactory(unsigned int id,
           BaseFeatureObserveModel& model);
      static BaseFeature* featureFactory(unsigned int id,
             FeatureModel& model, std::size_t sizeObs, Observation::ObservationType typeObs );

      void writePoseCopyToFile(PoseCopy const& pc, std::string const& dir);

      // for data logging
      unsigned short int nbInconsistentUpdates;
      unsigned short int nbNewLandmarks;
      unsigned short int nbObservedLandmarks;
      long updateElapsedTime;      

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

      jafar::kernel::DataLogger* p_masterLogger;

      void logFeature(BaseFeature& f);

    public:

      typedef std::map<unsigned int, BaseFeature*> FeaturesMapType;
      FeaturesMapType featuresMap;

      typedef std::map<unsigned int, PoseCopy*> PoseCopyContType;
      PoseCopyContType poseCopyCont;
      
      typedef std::map<unsigned int, BaseRobot*> RobotsMapType;
      RobotsMapType robotsMap;

      SlamEkf(std::size_t sizeMax_, std::size_t sizeRobotState_, std::size_t sizeRobotPose_);
    private:
      SlamEkf(const SlamEkf& ) : filter(0,0) { JFR_ASSERT(false, "Copying SlamEkf is not legal."); }
      SlamEkf& operator=(const SlamEkf& ) { JFR_ASSERT(false, "Copying SlamEkf is not legal."); }
    public:
      virtual ~SlamEkf();

      virtual void init(boost::posix_time::time_duration const& curTime);

      std::size_t sizeRobotPose() const {return m_sizeRobotPose;}
    
      std::size_t sizeRobotState() const {return m_sizeRobotState;}

      jblas::vec_range const& refPose(int _robotId = 0) const;
    
      jblas::sym_mat_range const& refPoseCov(int _robotId = 0) const;

      void enableAutoLogFeatures(jafar::kernel::DataLogger& masterLogger) {p_masterLogger = &masterLogger;}

      jafar::filter::BlockExtendedKalmanFilter& getFilter() {return filter;};
    
      jafar::filter::BlockExtendedKalmanFilter const& getFilter() const {return filter;};

      FeaturesMapType const& getMap() const {return featuresMap;}

      jafar::geom::T3D const& refToRobot() const {return *m_refToRobot;}
      jafar::geom::T3D const& robotToRef() const {return *m_robotToRef ;}
      void setRefToRobot(jafar::geom::T3DEuler const& refToRobot_);

      void setRobotPose(jblas::vec const& robotPose, int _robotId = 0);
      void setRobotPoseCov(jblas::vec const& robotPose, jblas::sym_mat const& robotPoseCov, int _robotId = 0);

//       BaseFeatureObserveModel& getBaseFeatureObserveModel(int sensorId) {
//  BaseFeatureObserveModelsContType::iterator it = baseFeatureObserveModels.find(sensorId);
//  JFR_PRED_ERROR(it != baseFeatureObserveModels.end(),
//           SlamException,
//           SlamException::UNKNOWN_SENSOR,
//           "Unknown sensor: " << sensorId);
//  return *(it->second);
//       }

      FeatureObserveModel& getFeatureObserveModel(unsigned int sensorId = 0) const {
  FeatureObserveModelsContType::const_iterator it = featureObserveModels.find(sensorId);
    JFR_PRED_ERROR(it != featureObserveModels.end(),
           SlamException,
           SlamException::UNKNOWN_SENSOR,
           "Unknown sensor: " << sensorId);
  return *(it->second);
      }

       unsigned int getCurrentFrameIndex() const {return currentFrameIndex;}
       unsigned int getPrevFrameIndex() const {return prevFrameIndex;}

      void setMapManager(AbstractMapManager* mapManager_);

      void setDeleteSensors( bool );
      void setSensor(FeatureObserveModel* model, int sensorId = 0);

//       void setRobotState(jblas::vec const& state_);

//       void setRobotState(jblas::vec const& state_, jblas::vec const& stateCov_);

      void setRobotZ(double z, double zStdDev, int _robotId = 0);

      void setRobotPitch(double pitch, double pitchStdDev, int _robotId = 0);

      void setRobotRoll(double roll, double rollStdDev, int _robotId = 0);

      virtual void setRobotToSensor(jblas::vec const& robotToSensor, int sensorId = 0);
      const jblas::vec& robotToSensor( int sensorId = 0) const;

      virtual void setRobotToSensor(jblas::vec const& robotToSensor, jblas::vec const& robotToSensorStdDev, 
            int sensorId = 0);

      virtual void addEventListener(SlamEventListener& listener) {
        JFR_ASSERT( &listener != 0, "Can't add null listener");
  slamEventListeners.push_back(&listener);
  //JFR_DEBUG("SlamEkf::addEventListener: " << slamEventListeners.size() << " listeners");
      }
      
      virtual void removeEventListener(SlamEventListener& listener) {
        slamEventListeners.remove(&listener);
      }

      bool hasFeature(unsigned int id, unsigned int _robotId = 0) const {
        return featuresMap.find(id) != featuresMap.end();
      };

      virtual bool isFeatureKnown(unsigned int id) const {
        //JFR_DEBUG("id :" << id);
      return hasFeature(id);
      };

      //       virtual void addLandmarkManually(AbstractMapObject::IdType id_, Observation::ObservationType featureType_, 
      //               jblas::vec const& featureState_, jblas::sym_mat const& stateCov_);

      BaseFeature& getFeature(unsigned int id) {
  FeaturesMapType::const_iterator feature_it = featuresMap.find(id);
  if (feature_it != featuresMap.end())
    return *(feature_it->second);
  else
  {
    JFR_DEBUG( "SlamEkf::getFeature: unknown feature " << id );
    JFR_ERROR(SlamException, SlamException::UNKNOWN_FEATURE,
        "SlamEkf::getFeature: unknown feature " << id);
  }
      }

      BaseFeature const& getFeature(unsigned int id) const {
  FeaturesMapType::const_iterator feature_it = featuresMap.find(id);
  if (feature_it != featuresMap.end())
    return *(feature_it->second);
  else
    JFR_ERROR(SlamException, SlamException::UNKNOWN_FEATURE,
        "SlamEkf::getFeature: unknown feature " << id);
      }

      virtual void removeFeature(unsigned int id);

      PoseCopy const& copyCurrentRefPoseToMap(unsigned int id);

      PoseCopy const& getPoseCopy(unsigned int id) const {
  PoseCopyContType::const_iterator poseIt = poseCopyCont.find(id);
  if (poseIt != poseCopyCont.end())
    return *(poseIt->second);
  else
    JFR_ERROR(SlamException, SlamException::UNKNOWN_POSE_COPY,
        "SlamEkf::getPoseCopy: unknown pose copy " << id);
      }

      void removePoseCopy(unsigned int id);

      void writePoseCopyToFile(unsigned int id, std::string const& dir);
      void writeAllPoseCopyToFile(std::string const& dir);

      double getRobotPoseUncertaintyLevel(unsigned int robotId_ = 0) const;

      void getRobotPose(jafar::geom::T3DEuler& pose, unsigned int _robotId = 0) const {
  if (refToRobot().isIdentity()) {
    pose.set(refPose(_robotId), refPoseCov(_robotId));
  }
  else {
    geom::T3DEuler worldToRef(refPose(_robotId), refPoseCov(_robotId));
    geom::T3D::compose(worldToRef, refToRobot(), pose);
  }
      }
      
      void transform( const geom::T3DEuler& _transform );

//       jblas::vec_range const& getRobotState() const {return robotState;}

      std::size_t sizeMap() const {return featuresMap.size();}
      double getMapUncertaintyLevel() const;

      // easy to use with swig/tcl
      FeaturesMapType::iterator browseFeaturesIt;
      void beginBrowseFeatures();
      bool hasNextFeature();
      BaseFeature* nextFeature();
      BaseFeature* feature( unsigned int id );

      virtual void predict(unsigned int _robotId, jblas::vec const& u);
      
      BaseRobot* robot( unsigned int _id );
      const BaseRobot* robot( unsigned int ) const;
      
      virtual void addRobot( BaseRobot* );
      virtual void addRobot( BaseRobot*, const jblas::vec& _robotState, const jblas::sym_mat& _robotStateCov );
    private:
      template<class SequenceType1, class SequenceType2>
      void processObservationsImpl(unsigned int frameIndex_,
              SequenceType1 const& knownObs,
              SequenceType2 const& newObs, unsigned int robotId_, int events_ ) 
      {
        nbInconsistentUpdates = 0;
        nbNewLandmarks = 0;
        nbObservedLandmarks = 0;

        if( frameIndex_ != currentFrameIndex )
        {
          prevFrameIndex = currentFrameIndex;
          currentFrameIndex = frameIndex_;
        }
        //JFR_DEBUG("RobotId:" << robotId_);
        if( (events_ & SlamEventListener::BEGIN_PROCESS_OBSERVATIONS) == SlamEventListener::BEGIN_PROCESS_OBSERVATIONS )
        {
          for(SlamEventListenersList::iterator it = slamEventListeners.begin() ; it != slamEventListeners.end() ; ++it) {
            (*it)->beginProcessObservations(robotId_);
          }
        }

        kernel::Chrono chrono;

        for (typename SequenceType1::const_iterator it = knownObs.begin() ; it != knownObs.end() ; ++it) {
          observeKnownFeature(*it);
        }

        for (typename SequenceType2::const_iterator it = newObs.begin() ; it != newObs.end() ; ++it) {
          observeNewFeature(*it);
        }

        //  applyAllConstraints();

        updateElapsedTime = chrono.elapsed();

        if( (events_ & SlamEventListener::END_PROCESS_OBSERVATIONS) == SlamEventListener::END_PROCESS_OBSERVATIONS )
        {
          for(SlamEventListenersList::iterator it = slamEventListeners.begin() ; it != slamEventListeners.end() ; ++it) {
            (**it).endProcessObservations(robotId_);
          }
        }
      }
    public:
        virtual void processObservations(unsigned int frameIndex_,
             std::list<Observation*> const& knownObs,
             std::list<Observation*> const& newObs, unsigned int robotId_ = 0, int events_ = SlamEventListener::ALL )
        {
          processObservationsImpl( frameIndex_, knownObs, newObs, robotId_, events_ );
        }
        virtual void processObservations(unsigned int frameIndex_,
             std::vector<Observation*> const& knownObs,
             std::vector<Observation*> const& newObs, unsigned int robotId_ = 0, int events_ = SlamEventListener::ALL )
        {
          processObservationsImpl( frameIndex_, knownObs, newObs, robotId_ , events_);
        }

      template<class SequenceType>
      void processObservations(unsigned int frameIndex_,
             SequenceType const& allObs, unsigned int robotId_ = 0, int events_ = SlamEventListener::ALL ) 
      {

      SequenceType knownObs;
      SequenceType newObs;
    
      for (typename SequenceType::const_iterator it = allObs.begin() ; it != allObs.end() ; ++it) {
      if ( isFeatureKnown((**it).id) )
          knownObs.push_back(*it);
      else
          newObs.push_back(*it);
      }
    
      processObservations(frameIndex_, knownObs, newObs, robotId_, events_);

      }