map_util.h

Go to the documentation of this file.

#ifndef JPS_MAP_UTIL_H
#define JPS_MAP_UTIL_H

#include <iostream>
#include <jps_basis/data_type.h>

namespace JPS {
  using Tmap = std::vector<signed char>;
  template <int Dim> class MapUtil {
    public:
      MapUtil() {}
      Tmap getMap() { return map_; }
      decimal_t getRes() { return res_; }
      Veci<Dim> getDim() { return dim_; }
      Vecf<Dim> getOrigin() { return origin_d_; }
      int getIndex(const Veci<Dim>& pn) {
          return Dim == 2 ? pn(0) + dim_(0) * pn(1) :
                            pn(0) + dim_(0) * pn(1) + dim_(0) * dim_(1) * pn(2);
      }

      bool isOutsideXYZ(const Veci<Dim> &n, int i) { return n(i) < 0 || n(i) >= dim_(i); }
      bool isFree(int idx) { return map_[idx] == val_free; }
      bool isUnknown(int idx) { return map_[idx] == val_unknown; }
      bool isOccupied(int idx) { return map_[idx] > val_free; }

      bool isOutside(const Veci<Dim> &pn) {
        for(int i = 0; i < Dim; i++)
          if (pn(i) < 0 || pn(i) >= dim_(i))
            return true;
        return false;
      }
      bool isFree(const Veci<Dim> &pn) {
        if (isOutside(pn))
          return false;
        else
          return isFree(getIndex(pn));
      }
      bool isOccupied(const Veci<Dim> &pn) {
        if (isOutside(pn))
          return false;
        else
          return isOccupied(getIndex(pn));
      }
      bool isUnknown(const Veci<Dim> &pn) {
        if (isOutside(pn))
          return false;
        return map_[getIndex(pn)] == val_unknown;
      }

      void setMap(const Vecf<Dim>& ori, const Veci<Dim>& dim, const Tmap &map, decimal_t res) {
        map_ = map;
        dim_ = dim;
        origin_d_ = ori;
        res_ = res;
      }

      void info() {
        Vecf<Dim> range = dim_.template cast<decimal_t>() * res_;
        std::cout << "MapUtil Info ========================== " << std::endl;
        std::cout << "   res: [" << res_ << "]" << std::endl;
        std::cout << "   origin: [" << origin_d_.transpose() << "]" << std::endl;
        std::cout << "   range: [" << range.transpose() << "]" << std::endl;
        std::cout << "   dim: [" << dim_.transpose() << "]" << std::endl;
      };

      Veci<Dim> floatToInt(const Vecf<Dim> &pt) {
        Veci<Dim> pn;
        for(int i = 0; i < Dim; i++)
          pn(i) = std::round((pt(i) - origin_d_(i)) / res_ - 0.5);
        return pn;
      }
      Vecf<Dim> intToFloat(const Veci<Dim> &pn) {
        //return pn.template cast<decimal_t>() * res_ + origin_d_;
        return (pn.template cast<decimal_t>() + Vecf<Dim>::Constant(0.5)) * res_ + origin_d_;
      }

      vec_Veci<Dim> rayTrace(const Vecf<Dim> &pt1, const Vecf<Dim> &pt2) {
        Vecf<Dim> diff = pt2 - pt1;
        decimal_t k = 0.8;
        int max_diff = (diff / res_).template lpNorm<Eigen::Infinity>() / k;
        decimal_t s = 1.0 / max_diff;
        Vecf<Dim> step = diff * s;

        vec_Veci<Dim> pns;
        Veci<Dim> prev_pn = Veci<Dim>::Constant(-1);
        for (int n = 1; n < max_diff; n++) {
          Vecf<Dim> pt = pt1 + step * n;
          Veci<Dim> new_pn = floatToInt(pt);
          if (isOutside(new_pn))
            break;
          if (new_pn != prev_pn)
            pns.push_back(new_pn);
          prev_pn = new_pn;
        }
        return pns;
      }

      bool isBlocked(const Vecf<Dim>& p1, const Vecf<Dim>& p2, int8_t val = 100) {
        vec_Veci<Dim> pns = rayTrace(p1, p2);
        for (const auto &pn : pns) {
          if (map_[getIndex(pn)] >= val)
            return true;
        }
        return false;
      }

      vec_Vecf<Dim> getCloud() {
        vec_Vecf<Dim> cloud;
        Veci<Dim> n;
        if(Dim == 3) {
          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
              for (n(2) = 0; n(2) < dim_(2); n(2)++) {
                if (isOccupied(getIndex(n)))
                  cloud.push_back(intToFloat(n));
              }
            }
          }
        }
        else if (Dim == 2) {
          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
              if (isOccupied(getIndex(n)))
                cloud.push_back(intToFloat(n));
            }
          }
        }

        return cloud;
      }

      vec_Vecf<Dim> getFreeCloud() {
        vec_Vecf<Dim> cloud;
        Veci<Dim> n;
        if(Dim == 3) {
          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
              for (n(2) = 0; n(2) < dim_(2); n(2)++) {
                if (isFree(getIndex(n)))
                  cloud.push_back(intToFloat(n));
              }
            }
          }
        }
        else if (Dim == 2) {
          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
              if (isFree(getIndex(n)))
                cloud.push_back(intToFloat(n));
            }
          }
        }

        return cloud;
      }

      vec_Vecf<Dim> getUnknownCloud() {
        vec_Vecf<Dim> cloud;
        Veci<Dim> n;
        if(Dim == 3) {
          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
              for (n(2) = 0; n(2) < dim_(2); n(2)++) {
                if (isUnknown(getIndex(n)))
                  cloud.push_back(intToFloat(n));
              }
            }
          }
        }
        else if (Dim == 2) {
          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
              if (isUnknown(getIndex(n)))
                cloud.push_back(intToFloat(n));
            }
          }
        }

        return cloud;
      }

      void dilate(const vec_Veci<Dim>& dilate_neighbor) {
        Tmap map = map_;
        Veci<Dim> n = Veci<Dim>::Zero();
        if(Dim == 3) {
          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
              for (n(2) = 0; n(2) < dim_(2); n(2)++) {
                if (isOccupied(getIndex(n))) {
                  for (const auto &it : dilate_neighbor) {
                    if (!isOutside(n + it))
                      map[getIndex(n + it)] = val_occ;
                  }
                }
              }
            }
          }
        }
        else if(Dim == 2) {
          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
              if (isOccupied(getIndex(n))) {
                for (const auto &it : dilate_neighbor) {
                  if (!isOutside(n + it))
                    map[getIndex(n + it)] = val_occ;
                }
              }
            }
          }
        }

        map_ = map;
      }

      void freeUnknown() {
        Veci<Dim> n;
        if(Dim == 3) {
          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
              for (n(2) = 0; n(2) < dim_(2); n(2)++) {
                if (isUnknown(getIndex(n)))
                  map_[getIndex(n)] = val_free;
              }
            }
          }
        }
        else if (Dim == 2) {
          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
              if (isUnknown(getIndex(n)))
                map_[getIndex(n)] = val_free;
            }
          }
        }
      }

      Tmap map_;
    protected:
      decimal_t res_;
      Vecf<Dim> origin_d_;
      Veci<Dim> dim_;
      int8_t val_occ = 100;
      int8_t val_free = 0;
      int8_t val_unknown = -1;
  };

  typedef MapUtil<2> OccMapUtil;

  typedef MapUtil<3> VoxelMapUtil;

}

#endif