visibilityMap.hpp

Go to the documentation of this file.

#ifndef VISIBILITY_MANAGER_HPP_
#define VISIBILITY_MANAGER_HPP_

#include <map>
#include "jmath/misc.hpp"
#include "jmath/jblas.hpp"
#include "rtslam/rtSlam.hpp"
#include "rtslam/spaceGrid.hpp"

namespace jafar {
namespace rtslam {

  class VisibilityMap
  {
    protected:
      struct Cell
      {
        unsigned nSuccess;
        unsigned nFailure;
        bool lastResult;
        unsigned lastTryFrame;
        Cell(): nSuccess(0), nFailure(0), lastResult(false), lastTryFrame(0) {}
      };
      SphericalGrid<Cell> grid;
      double nCertainty;
      Cell *lastCell;
      double lastVis, lastVisUncert;
    protected:
      Cell* getCell(const observation_ptr_t obsPtr, bool create = false);
    public:
      VisibilityMap(double angularRes, double distInit, double distFactor, int nDist, int nCertainty);
      VisibilityMap();
      void addObservation(const observation_ptr_t obsPtr);
      void estimateVisibility(const observation_ptr_t obsPtr, double &visibility, double &certainty);
      
      friend std::ostream& operator <<(std::ostream & s, Cell const & cell);
      friend std::ostream& operator <<(std::ostream & s, VisibilityMap const & vismap);
  };
  




}}

#endif