ObservationModel.cpp
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 #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   // TODO: move to HumanoidLocalization, skip individual parts if z/rp constrained
00060   double poseHeight = footprintToTorso.getOrigin().getZ();
00061   ROS_DEBUG("Pose measurement z=%f R=%f P=%f", poseHeight, poseRoll, posePitch);
00062   // TODO cluster xy of particles => speedup
00063 #pragma omp parallel for
00064   for (unsigned i=0; i < particles.size(); ++i){
00065     // integrate IMU meas.:
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     // integrate height measurement (z)
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 }


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