#include <ObservationModel.h>
Public Member Functions | |
virtual void | integrateMeasurement (Particles &particles, const PointCloud &pc, const std::vector< float > &ranges, float max_range, const tf::Transform &baseToSensor)=0 |
virtual void | integratePoseMeasurement (Particles &particles, double roll, double pitch, const tf::StampedTransform &footprintToTorso) |
ObservationModel (ros::NodeHandle *nh, boost::shared_ptr< MapModel > mapModel, EngineT *rngEngine) | |
virtual void | setMap (boost::shared_ptr< octomap::OcTree > map) |
virtual | ~ObservationModel () |
Static Public Member Functions | |
static double | logLikelihood (double x, double sigma) |
Helper function to compute the log likelihood. | |
static double | logLikelihoodSq (double x_sq, double sigma) |
Helper function to compute the log likelihood based on sqared distances. | |
Protected Member Functions | |
virtual bool | getHeightError (const Particle &p, const tf::StampedTransform &footprintToBase, double &heightError) const =0 |
Protected Attributes | |
boost::shared_ptr < octomap::OcTree > | m_map |
boost::shared_ptr< MapModel > | m_mapModel |
ros::Publisher | m_pc_pub |
EngineT | m_rngEngine |
NormalGeneratorT | m_rngNormal |
UniformGeneratorT | m_rngUniform |
double | m_sigmaPitch |
double | m_sigmaRoll |
double | m_sigmaZ |
bool | m_use_squared_error |
double | m_weightPitch |
double | m_weightRoll |
double | m_weightZ |
Definition at line 51 of file ObservationModel.h.
humanoid_localization::ObservationModel::ObservationModel | ( | ros::NodeHandle * | nh, |
boost::shared_ptr< MapModel > | mapModel, | ||
EngineT * | rngEngine | ||
) |
Definition at line 31 of file ObservationModel.cpp.
Definition at line 55 of file ObservationModel.cpp.
virtual bool humanoid_localization::ObservationModel::getHeightError | ( | const Particle & | p, |
const tf::StampedTransform & | footprintToBase, | ||
double & | heightError | ||
) | const [protected, pure virtual] |
Implemented in humanoid_localization::EndpointModel, and humanoid_localization::RaycastingModel.
virtual void humanoid_localization::ObservationModel::integrateMeasurement | ( | Particles & | particles, |
const PointCloud & | pc, | ||
const std::vector< float > & | ranges, | ||
float | max_range, | ||
const tf::Transform & | baseToSensor | ||
) | [pure virtual] |
Integrate a measurement in particle set, update weights accordingly Particle weights should be in log scale before, weights are added.
Implemented in humanoid_localization::EndpointModel, and humanoid_localization::RaycastingModel.
void humanoid_localization::ObservationModel::integratePoseMeasurement | ( | Particles & | particles, |
double | roll, | ||
double | pitch, | ||
const tf::StampedTransform & | footprintToTorso | ||
) | [virtual] |
Definition at line 58 of file ObservationModel.cpp.
static double humanoid_localization::ObservationModel::logLikelihood | ( | double | x, |
double | sigma | ||
) | [inline, static] |
Helper function to compute the log likelihood.
Definition at line 57 of file ObservationModel.h.
static double humanoid_localization::ObservationModel::logLikelihoodSq | ( | double | x_sq, |
double | sigma | ||
) | [inline, static] |
Helper function to compute the log likelihood based on sqared distances.
Definition at line 63 of file ObservationModel.h.
void humanoid_localization::ObservationModel::setMap | ( | boost::shared_ptr< octomap::OcTree > | map | ) | [virtual] |
Reimplemented in humanoid_localization::EndpointModel.
Definition at line 81 of file ObservationModel.cpp.
boost::shared_ptr<octomap::OcTree> humanoid_localization::ObservationModel::m_map [protected] |
Definition at line 88 of file ObservationModel.h.
boost::shared_ptr<MapModel> humanoid_localization::ObservationModel::m_mapModel [protected] |
Definition at line 84 of file ObservationModel.h.
Definition at line 89 of file ObservationModel.h.
Definition at line 85 of file ObservationModel.h.
Definition at line 86 of file ObservationModel.h.
Definition at line 87 of file ObservationModel.h.
double humanoid_localization::ObservationModel::m_sigmaPitch [protected] |
Definition at line 97 of file ObservationModel.h.
double humanoid_localization::ObservationModel::m_sigmaRoll [protected] |
Definition at line 96 of file ObservationModel.h.
double humanoid_localization::ObservationModel::m_sigmaZ [protected] |
Definition at line 95 of file ObservationModel.h.
Definition at line 99 of file ObservationModel.h.
double humanoid_localization::ObservationModel::m_weightPitch [protected] |
Definition at line 92 of file ObservationModel.h.
double humanoid_localization::ObservationModel::m_weightRoll [protected] |
Definition at line 91 of file ObservationModel.h.
double humanoid_localization::ObservationModel::m_weightZ [protected] |
Definition at line 93 of file ObservationModel.h.