ObservationModel.h
Go to the documentation of this file.
00001 // SVN $HeadURL$
00002 // SVN $Id$
00003 
00004 /*
00005  * 6D localization for humanoid robots
00006  *
00007  * Copyright 2009-2012 Armin Hornung, University of Freiburg
00008  * http://www.ros.org/wiki/humanoid_localization
00009  *
00010  *
00011  * This program is free software: you can redistribute it and/or modify
00012  * it under the terms of the GNU General Public License as published by
00013  * the Free Software Foundation, version 3.
00014  *
00015  * This program is distributed in the hope that it will be useful,
00016  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  * GNU General Public License for more details.
00019  *
00020  * You should have received a copy of the GNU General Public License
00021  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
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   //static double logLikelihoodSimple(double x, double sigma){
00070   //    return  -((x * x) / (2* sigma * sigma));
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 /* OBSERVATIONMODEL_H_ */


humanoid_localization
Author(s): Armin Hornung, Stefan Osswald, Daniel Maier
autogenerated on Wed Aug 26 2015 11:54:40