occupancymapparams.h

Go to the documentation of this file.

#ifndef OCCUPANCYMAPPARAMS_H
#define OCCUPANCYMAPPARAMS_H

#include <almath/tools/almath.h>
#include <almath/api.h>

namespace AL {
namespace Math {

  struct ALMATH_API Point2Di {
    explicit Point2Di(int _x=0, int _y=0): x(_x), y(_y) {}
    int x;
    int y;
    bool operator == (const Point2Di& other) const {
      return (x == other.x && y == other.y);
    }
  };

  struct ALMATH_API Pose2Di {
    explicit Pose2Di(int _x=0, int _y=0, float _angle=0.0f)
        : x(_x), y(_y), theta(_angle) {}
    explicit Pose2Di(const Point2Di& pt, float _angle=0.0f)
        : x(pt.x), y(pt.y), theta(_angle) {}

    int x;
    int y;
    float theta;
    bool operator == (const Pose2Di& other) const {
      return (x == other.x && y == other.y && std::abs(theta - other.theta) < 1e-6f);
    }
  };

  typedef std::vector<Pose2D> Pose2DVect;
  typedef std::vector<Pose2Di> Pose2DiVect;

  static inline int roundToInt(float x) {
    if (x >= 0.0f) {
      return static_cast<int>(x + 0.5f);
    }
    return static_cast<int>(x - 0.5f);
  }

struct ALMATH_API OccupancyMapParams {
  explicit OccupancyMapParams(int size, float metersPerPixel,
      const Position2D &mapCenter, float obstacleThreshold = 0.25f);

  void initOriginOffset(const Position2D &mapCenter);

  static inline Point2Di getPixelFromOffsetAndScale(
      const Math::Pose2D& pose,
      float metersPerPixel,
      const Math::Position2D& originOffset) {
    const float kPixelPerMeter = 1.0f / metersPerPixel;

    const float xx = (pose.x - originOffset.x) * kPixelPerMeter;
    const int x = roundToInt(xx);

    const float yy = -(pose.y - originOffset.y) * kPixelPerMeter;
    const int y = roundToInt(yy);
    return Point2Di(x, y);
  }

  Point2Di getPixelFromPose(const Pose2D& pose) const;

  Point2Di getPixelFromPosition(const Position2D& position) const;

  Pose2D getPoseFromPixel(const Pose2Di &pixel) const;

  Position2D getPositionFromPixel(const Point2Di &pixel) const;

  Point2Di getDeltaPixelFromDeltaPosition(const Position2D& position) const;

  float getObstacleProbabilityThreshold() const;

  int size;
  float metersPerPixel;
  Position2D originOffset;
  float obstacleProbabilityThreshold;
};
} // Math
} // AL

#endif // OCCUPANCYMAPPARAMS_H