MotionModel.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/MotionModel.h>
00025 
00026 using namespace tf;
00027 using namespace std;
00028 
00029 namespace humanoid_localization{
00030 
00031 MotionModel::MotionModel(ros::NodeHandle* nh, EngineT* rngEngine, tf::TransformListener* tf,
00032                          const std::string& odomFrameId, const std::string& baseFrameId)
00033 : m_tfListener(tf),
00034   m_rngNormal(*rngEngine, NormalDistributionT(0.0, 1.0)),
00035   m_rngUniform(*rngEngine, UniformDistributionT(0.0, 1.0)),
00036   m_odomFrameId(odomFrameId), m_baseFrameId(baseFrameId),
00037   m_firstOdometryReceived(false)
00038 
00039 {
00040 
00041   // motion model noise parameters:
00042   m_odomNoise2D = Eigen::Matrix3d::Zero();
00043   // noise affecting x direction (sq. / variance)
00044   nh->param("motion_noise/xx", m_odomNoise2D(0,0), 0.01);
00045   nh->param("motion_noise/xy", m_odomNoise2D(0,1), 0.01);
00046   nh->param("motion_noise/xt", m_odomNoise2D(0,2), 0.0001);
00047   // noise affecting y direction (sq. / variance)
00048   nh->param("motion_noise/yx", m_odomNoise2D(1,0), 0.01);
00049   nh->param("motion_noise/yy", m_odomNoise2D(1,1), 0.01);
00050   nh->param("motion_noise/yt", m_odomNoise2D(1,2), 0.0001);
00051   // noise affecting orientation (sq. / variance)
00052   nh->param("motion_noise/tx", m_odomNoise2D(2,0), 0.5);
00053   nh->param("motion_noise/ty", m_odomNoise2D(2,1), 0.5);
00054   nh->param("motion_noise/tt", m_odomNoise2D(2,2), 0.01);
00055 
00056   // std. devs for z, roll & pitch (depend on amount of transl.)
00057   nh->param("motion_noise/z", m_odomNoiseZ, 0.01);
00058   nh->param("motion_noise/roll", m_odomNoiseRoll, 0.05);
00059   nh->param("motion_noise/pitch", m_odomNoisePitch, 0.1);
00060 
00061   // old parameters, warn that renamed:
00062   if (nh->hasParam("motion_noise/x"))
00063     ROS_WARN("Parameter motion_noise/x is no longer used, use variances motion_noise/[xx|xy|xt] instead");
00064 
00065   if (nh->hasParam("motion_noise/y"))
00066     ROS_WARN("Parameter motion_noise/y is no longer used, use variances motion_noise/[yx|yy|yt] instead");
00067 
00068   if (nh->hasParam("motion_noise/yaw"))
00069     ROS_WARN("Parameter motion_noise/yaw is no longer used, use variances motion_noise/[tx|ty|tt] instead");
00070 
00071   // odometry calibration (systematic drift correction)
00072   m_odomCalibration2D = Eigen::Matrix3d::Identity();
00073   nh->param("motion_calib/xx", m_odomCalibration2D(0,0), 1.0);
00074   nh->param("motion_calib/xy", m_odomCalibration2D(0,1), 0.0);
00075   nh->param("motion_calib/xt", m_odomCalibration2D(0,2), 0.0);
00076   nh->param("motion_calib/yx", m_odomCalibration2D(1,0), 0.0);
00077   nh->param("motion_calib/yy", m_odomCalibration2D(1,1), 1.0);
00078   nh->param("motion_calib/yt", m_odomCalibration2D(1,2), 0.0);
00079   nh->param("motion_calib/tx", m_odomCalibration2D(2,0), 0.0);
00080   nh->param("motion_calib/ty", m_odomCalibration2D(2,1), 0.0);
00081   nh->param("motion_calib/tt", m_odomCalibration2D(2,2), 1.0);
00082 
00083 
00084   reset();
00085 
00086 }
00087 
00088 MotionModel::~MotionModel() {
00089 
00090 }
00091 
00092 
00093 tf::Transform MotionModel::odomTransformNoise(const tf::Transform& odomTransform){
00094   // vectors (x,y,theta) in 2D for squared motion and variance
00095   Eigen::Vector3d motion2D_sq, motion_variance;
00096   double yaw = tf::getYaw(odomTransform.getRotation());
00097   motion2D_sq(0) = odomTransform.getOrigin().x() * odomTransform.getOrigin().x();
00098   motion2D_sq(1) = odomTransform.getOrigin().y() * odomTransform.getOrigin().y();
00099   motion2D_sq(2) = yaw * yaw;
00100 
00101   motion_variance = m_odomNoise2D * motion2D_sq;
00102 
00103   // use std.normal dev as basis:
00104   // X is normally distributed (mean 0, dev 1)
00105   // => Y = aX + b is also normally distributed with mean b and deviation a
00106 
00107   // absolute amount of translation, used to scale noise in z, roll & pitch
00108   // (about 1-2cm for each update step)
00109   const double d = odomTransform.getOrigin().length();
00110   return tf::Transform(tf::createQuaternionFromRPY(
00111         m_rngNormal() * d * m_odomNoiseRoll,      // roll
00112         m_rngNormal() * d * m_odomNoisePitch,     // pitch
00113         m_rngNormal() * sqrt(motion_variance(2))),// yaw
00114       tf::Vector3(
00115         m_rngNormal() * sqrt(motion_variance(0)), // x
00116         m_rngNormal() * sqrt(motion_variance(1)), // y
00117         m_rngNormal() * d * m_odomNoiseZ));       // z
00118 }
00119 
00120 void MotionModel::reset(){
00121   m_firstOdometryReceived = false;
00122 }
00123 
00124 void MotionModel::applyOdomTransform(tf::Pose& particlePose, const tf::Transform& odomTransform){
00125   particlePose *= calibrateOdometry(odomTransform) * odomTransformNoise(odomTransform);
00126 }
00127 
00128 void MotionModel::applyOdomTransform(Particles& particles, const tf::Transform& odomTransform){
00129   const tf::Transform calibratedOdomTransform = calibrateOdometry(odomTransform);
00130 
00131   for (unsigned i=0; i < particles.size(); ++i){
00132     particles[i].pose *= calibratedOdomTransform * odomTransformNoise(odomTransform);
00133   }
00134 }
00135 
00136 bool MotionModel::applyOdomTransformTemporal(Particles& particles,const ros::Time& t, double dt){
00137   ros::WallTime startTime = ros::WallTime::now();
00138 
00139   // first see if default time is available
00140   tf::Transform odomTransform;
00141   if (!lookupOdomTransform(t, odomTransform))
00142     return false;
00143 
00144   tf::Transform timeSampledTransform;
00145   ros::Duration maxDuration;
00146   if (dt > 0.0){
00147     ros::Time maxTime;
00148     std::string errorString;
00149     m_tfListener->getLatestCommonTime(m_odomFrameId, m_baseFrameId, maxTime, &errorString);
00150     maxDuration = maxTime - t;
00151   }
00152 
00153   for (unsigned i=0; i < particles.size(); ++i){
00154     if (dt > 0.0){
00155       ros::Duration duration(m_rngUniform()*dt -dt/2.0);
00156       // TODO: time t is time of first measurement in scan!
00157       if (duration > maxDuration)
00158         duration = maxDuration;
00159 
00160       if (lookupOdomTransform(t + duration, timeSampledTransform))
00161         applyOdomTransform(particles[i].pose, timeSampledTransform);
00162       else{
00163         ROS_WARN("Could not lookup temporal odomTransform");
00164         applyOdomTransform(particles[i].pose, odomTransform);
00165       }
00166     } else{
00167       applyOdomTransform(particles[i].pose, odomTransform);
00168     }
00169   }
00170 
00171   double dwalltime = (ros::WallTime::now() - startTime).toSec();
00172   ROS_INFO_STREAM("OdomTransformTemporal took " << dwalltime << "s ");
00173 
00174 
00175   return true;
00176 }
00177 
00178 tf::Transform MotionModel::calibrateOdometry(const tf::Transform& odomTransform) const {
00179   Eigen::Vector3d odomPose2D;
00180   double roll, pitch;
00181   odomPose2D(0) = odomTransform.getOrigin().getX();
00182   odomPose2D(1) = odomTransform.getOrigin().getY();
00183   odomPose2D(2) = tf::getYaw(odomTransform.getRotation());
00184   odomTransform.getBasis().getRPY(roll, pitch, odomPose2D(2));
00185 
00186   odomPose2D = m_odomCalibration2D * odomPose2D;
00187 
00188   return tf::Transform(tf::createQuaternionFromRPY(roll, pitch, odomPose2D(2)),
00189                        tf::Vector3(odomPose2D(0), odomPose2D(1), odomTransform.getOrigin().getZ()));
00190 
00191 
00192 }
00193 
00194 bool MotionModel::lookupOdomTransform(const ros::Time& t, tf::Transform& odomTransform) const{
00195   tf::Stamped<tf::Pose> odomPose;
00196 
00197   if (t <= m_lastOdomPose.stamp_){
00198     ROS_WARN("Looking up OdomTransform that is %f ms older than the last odomPose!",
00199              (m_lastOdomPose.stamp_ - t).toSec()/1000.0);
00200   }
00201 
00202   if (!lookupOdomPose(t, odomPose))
00203     return false;
00204 
00205   odomTransform = computeOdomTransform(odomPose);
00206   return true;
00207 }
00208 
00209 tf::Transform MotionModel::computeOdomTransform(const tf::Transform currentPose) const{
00210   if (m_firstOdometryReceived){
00211     return m_lastOdomPose.inverse() * currentPose;
00212   } else{
00213     return tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,0));
00214   }
00215 
00216 }
00217 
00218 void MotionModel::storeOdomPose(const tf::Stamped<tf::Pose>& odomPose){
00219   m_firstOdometryReceived = true;
00220   if (odomPose.stamp_ <= m_lastOdomPose.stamp_){
00221     ROS_WARN("Trying to store an OdomPose that is older or equal than the current in the MotionModel, ignoring!");
00222   } else {
00223     m_lastOdomPose = odomPose;
00224   }
00225 
00226 }
00227 
00228 
00229 bool MotionModel::lookupOdomPose(const ros::Time& t, tf::Stamped<tf::Pose>& odomPose) const
00230 {
00231   tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
00232                                              tf::Vector3(0,0,0)), t, m_baseFrameId);
00233 
00234   try
00235   {
00236     m_tfListener->transformPose(m_odomFrameId, ident, odomPose);
00237   }
00238   catch(tf::TransformException& e)
00239   {
00240     ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
00241     return false;
00242   }
00243 
00244   return true;
00245 }
00246 
00247 
00248 bool MotionModel::lookupLocalTransform(const std::string& targetFrame, const ros::Time& t,
00249                                        tf::StampedTransform& localTransform) const
00250 {
00251   try
00252   {
00253     m_tfListener->lookupTransform(targetFrame, m_baseFrameId, t, localTransform);
00254   }
00255   catch(tf::TransformException& e)
00256   {
00257     ROS_WARN("Failed to lookup local transform (%s)", e.what());
00258     return false;
00259   }
00260 
00261   return true;
00262 }
00263 
00264 bool MotionModel::getLastOdomPose(tf::Stamped<tf::Pose>& lastOdomPose) const{
00265   if (m_firstOdometryReceived){
00266     lastOdomPose = m_lastOdomPose;
00267     return true;
00268   } else{
00269     return false;
00270   }
00271 
00272 }
00273 
00274 }
00275 


humanoid_localization
Author(s): Armin Hornung, Stefan Osswald, Daniel Maier
autogenerated on Mon Oct 6 2014 00:41:01