00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <humanoid_localization/ObservationModel.h>
00025
00026 using namespace std;
00027 using namespace tf;
00028
00029 namespace humanoid_localization{
00030
00031 ObservationModel::ObservationModel(ros::NodeHandle* nh, boost::shared_ptr<MapModel> mapModel, EngineT* rngEngine )
00032 : m_mapModel(mapModel),
00033 m_rngNormal(*rngEngine, NormalDistributionT(0.0, 1.0)),
00034 m_rngUniform(*rngEngine, UniformDistributionT(0.0, 1.0)),
00035 m_weightRoll(1.0), m_weightPitch(1.0), m_weightZ(1.0),
00036 m_sigmaZ(0.02), m_sigmaRoll(0.05), m_sigmaPitch(0.05),
00037 m_use_squared_error(false)
00038 {
00039
00040 m_map = m_mapModel->getMap();
00041
00042 nh->param("weight_factor_roll", m_weightRoll, m_weightRoll);
00043 nh->param("weight_factor_pitch", m_weightPitch, m_weightPitch);
00044 nh->param("weight_factor_z", m_weightZ, m_weightZ);
00045 nh->param("motion_sigma_z", m_sigmaZ, m_sigmaZ);
00046 nh->param("motion_sigma_roll", m_sigmaRoll, m_sigmaRoll);
00047 nh->param("motion_sigma_pitch", m_sigmaPitch, m_sigmaPitch);
00048 nh->param("obs_squared_distance", m_use_squared_error, m_use_squared_error);
00049
00050 if (m_sigmaZ <= 0.0 || m_sigmaRoll <= 0.0 || m_sigmaPitch <= 0.0){
00051 ROS_ERROR("Sigma (std.dev) needs to be > 0 in ObservationModel");
00052 }
00053 }
00054
00055 ObservationModel::~ObservationModel() {
00056 }
00057
00058 void ObservationModel::integratePoseMeasurement(Particles& particles, double poseRoll, double posePitch, const tf::StampedTransform& footprintToTorso){
00059
00060 double poseHeight = footprintToTorso.getOrigin().getZ();
00061 ROS_DEBUG("Pose measurement z=%f R=%f P=%f", poseHeight, poseRoll, posePitch);
00062
00063 #pragma omp parallel for
00064 for (unsigned i=0; i < particles.size(); ++i){
00065
00066 double roll, pitch, yaw;
00067 particles[i].pose.getBasis().getRPY(roll, pitch, yaw);
00068 particles[i].weight += m_weightRoll * logLikelihood(poseRoll - roll, m_sigmaRoll);
00069 particles[i].weight += m_weightPitch * logLikelihood(posePitch - pitch, m_sigmaPitch);
00070
00071
00072 double heightError;
00073 if (getHeightError(particles[i],footprintToTorso, heightError))
00074 particles[i].weight += m_weightZ * logLikelihood(heightError, m_sigmaZ);
00075
00076
00077 }
00078
00079 }
00080
00081 void ObservationModel::setMap(boost::shared_ptr<octomap::OcTree> map){
00082 m_map = map;
00083 }
00084
00085 }