3dOccupancyGrid.hpp

/* $Id$ */

#ifndef GEOM_3D_OCCUPANCY_GRID_HPP
#define GEOM_3D_OCCUPANCY_GRID_HPP

#include <cmath>
#include <ostream>
#include <vector>
#include <map>

#include "boost/numeric/ublas/vector.hpp"
#include "boost/numeric/ublas/io.hpp"

#include "jafarConfig.h"

#include "jmath/jblas.hpp"

#include "geom/t3d.hpp"

#ifdef HAVE_OPENGL
#ifdef __APPLE__
#include </System/Library/Frameworks/OpenGL.framework/Versions/A/Headers/gl.h>
#else
#include <GL/gl.h>
#endif /* __APPLE__ */
#endif // HAVE_OPENGL

namespace jafar {
  namespace geom {

    class DenseOG;

    typedef boost::numeric::ublas::bounded_vector<double,3> VoxelCoord;
    typedef boost::numeric::ublas::bounded_vector<int,3> VoxelIndex;

    class VoxelIndexLess {
    public:
      inline bool operator()(VoxelIndex const& v1, VoxelIndex const& v2) const
      {
  if (v1(0) != v2(0))
    return v1(0) < v2(0);
  else if (v1(1) != v2(1))
    return v1(1) < v2(1);
  else
    return v1(2) < v2(2);
      }
    };

    template<typename T>
    struct Box {

      Box() : x(), size() {
  x.clear(); 
  size.clear();
      }

      Box(Box const& box) :
  x(box.x), size(box.size) {}

      boost::numeric::ublas::bounded_vector<T,3> x;
      boost::numeric::ublas::bounded_vector<T,3> size;

      bool isEmpty() {
  return size(0) == 0 || size(1) == 0 || size(2) == 0;
      }

      Box<T> intersect(Box<T> const& box) const {
  Box<T> inter;
  for (std::size_t i=0 ; i < 3 ; ++i) {
    inter.x(i) = std::max(box.x(i), x(i));
    inter.size(i) = std::min(box.x(i)+box.size(i), x(i)+size(i)) - inter.x(i);
    if (inter.size(i) <= 0)
      return Box<T>();
  }
  return inter;
      }

    };

    template<typename T>
    std::ostream& operator <<(std::ostream& s, Box<T> const& b) {
      s << "{" << b.x << " ; " << b.size << "}";
      return s;
    }

    Box<int> metricToIndex(Box<double> const& bd, double sizeVoxel);
    VoxelIndex metricToIndex(VoxelCoord const& vid, double sizeVoxel);
    VoxelCoord indexToMetric(VoxelIndex const& vii, double sizeVoxel);

    double  pToLogOddsRatio(double p) {
      return log(p/(1.0-p));
    }

    double  logOddsRatioToP(double l) {
      return 1.0 - 1.0/(1.0+exp(l));
    }

    VoxelIndex computeVoxelIndex(std::size_t flatIndex, Box<int> const& box);

    class OGSensorModel {

    public:

      OGSensorModel();
      virtual ~OGSensorModel();

      void setRefToSensor(jafar::geom::T3D const& refToSensor);

      virtual void setupUpdate(DenseOG const& og) = 0;

      Box<double> visibilityBox() const;

      double inverseObservation(VoxelCoord const& voxel) const;

    protected:

      virtual Box<double> visibilityBoxInSensorFrame() const = 0;
      virtual double inverseObservationInSensorFrame(VoxelCoord const& voxel) const = 0;

    private:

      T3D* m_sensorToRef;

    };

    class DenseOG {

    public:

      typedef std::vector<double> OGType;

      DenseOG(Box<double> const& box, double sizeVoxel, double emptyVoxelTh = 0.2, double fullVoxelTh = 0.8);
      DenseOG(Box<int> const& dox, double sizeVoxel, double emptyVoxelTh = 0.2, double fullVoxelTh = 0.8);

      double sizeVoxel() const {return m_sizeVoxel;}
      Box<int> const& box() const {return m_box;}
      OGType const& og() const {return m_og;}
      double& get(VoxelCoord const& vi);
      double& get(VoxelIndex const& vi);

      void randomize(unsigned int s = 1);

      void update(OGSensorModel& model);

    private:

      double m_sizeVoxel;
      Box<int> m_box;
      OGType m_og;

      double m_emptyVoxelLogTh;

      double m_fullVoxelLogTh;

      inline std::size_t computeFlatIndex(VoxelIndex const& vi)
      {
  return (size_t)(vi(0) - m_box.x(0))*m_box.size(1)*m_box.size(2) + 
    (size_t)(vi(1) - m_box.x(1))*m_box.size(2) + 
    (size_t)(vi(2) - m_box.x(2));

      }

#ifdef HAVE_OPENGL
    public:
      void glInit();
      void glDraw();
    private:
      GLuint glOGList;
#endif // HAVE_OPENGL

    }; // class DenseOG

    class SparseOG {

    public:

      typedef std::map<VoxelIndex,double,jafar::geom::VoxelIndexLess> OGType;
      
      SparseOG(DenseOG const& denseOG, double threshold);

      double sizeVoxel() const {return m_sizeVoxel;}
      Box<int> const& box() const {return m_box;}
      OGType const& og() const {return m_og;}
//       double& get(VoxelCoord const& vi);
//       double& get(VoxelIndex const& vi);

    private:

      double m_sizeVoxel;
      Box<int> m_box;
      OGType m_og;

#ifdef HAVE_OPENGL
    public:
      void glInit();
      void glDraw();
    private:
      GLuint glOGList;
#endif // HAVE_OPENGL
      
    }; // class SparseOG



  }
}

#endif // GEOM_3D_OCCUPANCY_GRID_HPP