Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #ifndef HUMANOID_LOCALIZATION_OBSERVATIONMODEL_H_
00025 #define HUMANOID_LOCALIZATION_OBSERVATIONMODEL_H_
00026
00027 #include <limits>
00028 #include<cmath>
00029
00030 #include <omp.h>
00031
00032 #include <ros/ros.h>
00033 #include <tf/transform_datatypes.h>
00034 #include <pcl_ros/transforms.h>
00035
00036 #include <humanoid_localization/humanoid_localization_defs.h>
00037 #include <humanoid_localization/MapModel.h>
00038 #include <octomap/octomap.h>
00039
00040 #include <sensor_msgs/PointCloud2.h>
00041
00042
00043 namespace humanoid_localization{
00044
00046 const static double SQRT_2_PI = 2.506628274;
00047
00049 const static double LOG_SQRT_2_PI = 0.91893853320467274178;
00050
00051 class ObservationModel {
00052 public:
00053 ObservationModel(ros::NodeHandle* nh, boost::shared_ptr<MapModel> mapModel, EngineT * rngEngine);
00054 virtual ~ObservationModel();
00055
00057 static inline double logLikelihood(double x, double sigma){
00058 assert(!isnan(x));
00059 return -1.0*( LOG_SQRT_2_PI) - log(sigma) - ((x * x) / (2* sigma * sigma));
00060 }
00061
00063 static inline double logLikelihoodSq(double x_sq, double sigma){
00064 assert(!isnan(x_sq));
00065 return -1.0*( LOG_SQRT_2_PI) - log(sigma) - ((x_sq) / (2* sigma * sigma));
00066 }
00067
00068
00069
00070
00071
00072
00077 virtual void integrateMeasurement(Particles& particles, const PointCloud& pc, const std::vector<float>& ranges, float max_range, const tf::Transform& baseToSensor) = 0;
00078
00079 virtual void integratePoseMeasurement(Particles& particles, double roll, double pitch, const tf::StampedTransform& footprintToTorso);
00080
00081 virtual void setMap(boost::shared_ptr<octomap::OcTree> map);
00082 protected:
00083 virtual bool getHeightError(const Particle& p, const tf::StampedTransform& footprintToBase, double& heightError) const = 0;
00084 boost::shared_ptr<MapModel> m_mapModel;
00085 EngineT m_rngEngine;
00086 NormalGeneratorT m_rngNormal;
00087 UniformGeneratorT m_rngUniform;
00088 boost::shared_ptr<octomap::OcTree> m_map;
00089 ros::Publisher m_pc_pub;
00090
00091 double m_weightRoll;
00092 double m_weightPitch;
00093 double m_weightZ;
00094
00095 double m_sigmaZ;
00096 double m_sigmaRoll;
00097 double m_sigmaPitch;
00098
00099 bool m_use_squared_error;
00100
00101 };
00102 }
00103
00104 #endif