amcl3d::ParticleFilter Class Reference

#include <ParticleFilter.h>

Public Member Functions
void	buildParticlesPoseMsg (const geometry_msgs::Point32 &offset, geometry_msgs::PoseArray &msg) const
Particle	getMean () const
	Get mean value from particles. More...
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)
	Set the initial pose of the particle filter. More...
bool	isInitialized () const
	ParticleFilter ()
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	resample ()
	Resample the set of particles using low-variance sampling. More...
void	update (const Grid3d &grid3d, const std::vector< pcl::PointXYZ > &points, const std::vector< Range > &range_data, const double alpha, const double sigma)
	Update Particles with a pointcloud update. More...
virtual	~ParticleFilter ()

Private Member Functions
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)

Private Attributes
std::mt19937	generator_
bool	initialized_ { false }
	Indicates if the filter was initialized. More...
Particle	mean_
std::vector< Particle >	p_
	Particles. More...
std::random_device	rd_
	Random number generator. More...

Detailed Description

Definition at line 62 of file ParticleFilter.h.

Constructor & Destructor Documentation

amcl3d::ParticleFilter::ParticleFilter ( )

explicit

Definition at line 22 of file ParticleFilter.cpp.

amcl3d::ParticleFilter::~ParticleFilter ( )

virtual

Definition at line 26 of file ParticleFilter.cpp.

Member Function Documentation

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

Definition at line 30 of file ParticleFilter.cpp.

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

private

Definition at line 217 of file ParticleFilter.cpp.

Particle amcl3d::ParticleFilter::getMean ( ) const

inline

Get mean value from particles.

Definition at line 74 of file ParticleFilter.h.

void amcl3d::ParticleFilter::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
	)

Set the initial pose of the particle filter.

Resize particle set

Sample the given pose

Definition at line 46 of file ParticleFilter.cpp.

bool amcl3d::ParticleFilter::isInitialized ( ) const

inline

Definition at line 68 of file ParticleFilter.h.

void amcl3d::ParticleFilter::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
	)

This function implements the PF prediction stage. Translation in X, Y and Z in meters and yaw angle incremenet in rad

Make a prediction for all particles according to the odometry

Definition at line 97 of file ParticleFilter.cpp.

float amcl3d::ParticleFilter::ranGaussian ( const double  mean, const double  sigma  )

private

Definition at line 239 of file ParticleFilter.cpp.

void amcl3d::ParticleFilter::resample

(

)

Resample the set of particles using low-variance sampling.

Do resamplig

Asign the new particles set

Definition at line 191 of file ParticleFilter.cpp.

float amcl3d::ParticleFilter::rngUniform ( const float  range_from, const float  range_to  )

private

Definition at line 245 of file ParticleFilter.cpp.

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

Update Particles with a pointcloud update.

Incorporate measurements

Get particle information

Check the particle is into the map

Evaluate the weight of the point cloud

Evaluate the weight of the range sensors

Increase the summatory of weights

Normalize all weights

Definition at line 121 of file ParticleFilter.cpp.

Member Data Documentation

std::mt19937 amcl3d::ParticleFilter::generator_

private

Definition at line 113 of file ParticleFilter.h.

bool amcl3d::ParticleFilter::initialized_ { false }

private

Indicates if the filter was initialized.

Definition at line 105 of file ParticleFilter.h.

Particle amcl3d::ParticleFilter::mean_

private

Definition at line 109 of file ParticleFilter.h.

std::vector<Particle> amcl3d::ParticleFilter::p_

private

Particles.

Definition at line 108 of file ParticleFilter.h.

std::random_device amcl3d::ParticleFilter::rd_

private

Random number generator.

Definition at line 112 of file ParticleFilter.h.

---

The documentation for this class was generated from the following files:

• ParticleFilter.h
• ParticleFilter.cpp