ParticleFilter.h

Go to the documentation of this file.

#pragma once

#include "Grid3d.h"
#include "Parameters.h"

#include <random>

#include <ros/ros.h>

#include <geometry_msgs/Point32.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

#include <boost/thread.hpp>

namespace amcl3d
{
struct Particle
{
  float x, y, z;

  float a;

  float w, wp, wr;

  Particle() : x(0), y(0), z(0), a(0), w(0), wp(0), wr(0)
  {
  }
};

struct Range
{
  float r, ax, ay, az;

  Range(const float r_, const float ax_, const float ay_, const float az_) : r(r_), ax(ax_), ay(ay_), az(az_)
  {
  }
};

class ParticleFilter
{
public:
  explicit ParticleFilter();
  virtual ~ParticleFilter();

  bool isInitialized() const
  {
    return initialized_;
  }

  Particle getMean() const
  {
    return mean_;
  }

  void buildParticlesPoseMsg(const geometry_msgs::Point32& offset, geometry_msgs::PoseArray& msg) const;

  void init(const int num_particles, const float x_init, const float y_init, const float z_init, const float a_init,
            const float x_dev, const float y_dev, const float z_dev, const float a_dev);

  void predict(const double odom_x_mod, const double odom_y_mod, const double odom_z_mod, const double odom_a_mod,
               const double delta_x, const double delta_y, const double delta_z, const double delta_a);

  void update(const Grid3d& grid3d, const std::vector<pcl::PointXYZ>& points, const std::vector<Range>& range_data,
              const double alpha, const double sigma);

  void resample();

private:
  float computeRangeWeight(const float x, const float y, const float z, const std::vector<Range>& range_data,
                           const double sigma);

  float ranGaussian(const double mean, const double sigma);
  float rngUniform(const float range_from, const float range_to);

  bool initialized_{ false };

  std::vector<Particle> p_;
  Particle mean_;

  std::random_device rd_;
  std::mt19937 generator_;
};

}  // namespace amcl3d