MotionModel.h
Go to the documentation of this file.
1 // SVN $HeadURL$
2 // SVN $Id$
3 
4 /*
5  * 6D localization for humanoid robots
6  *
7  * Copyright 2009-2012 Armin Hornung, University of Freiburg
8  * http://www.ros.org/wiki/humanoid_localization
9  *
10  *
11  * This program is free software: you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation, version 3.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License
21  * along with this program. If not, see <http://www.gnu.org/licenses/>.
22  */
23 
24 #ifndef HUMANOID_LOCALIZATION_MOTIONMODEL_H_
25 #define HUMANOID_LOCALIZATION_MOTIONMODEL_H_
26 
27 #include <ros/ros.h>
28 #include <tf/transform_listener.h>
29 #include <tf/transform_datatypes.h>
30 #include <eigen3/Eigen/Cholesky>
32 
33 namespace humanoid_localization{
34 class MotionModel {
35 public:
36  MotionModel(ros::NodeHandle* nh, EngineT* rngEngine, tf::TransformListener* tf, const std::string& odomFrameId, const std::string& baseFrameId);
37  virtual ~MotionModel();
38  void reset();
40  bool lookupOdomPose(const ros::Time& t, tf::Stamped<tf::Pose>& odomPose) const;
41 
43  bool lookupOdomTransform(const ros::Time& t, tf::Transform& odomTransform) const;
44 
47  tf::Transform computeOdomTransform(const tf::Transform currentPose) const;
48 
50  bool lookupLocalTransform(const std::string& targetFrame, const ros::Time& t, tf::StampedTransform& localTransform) const;
51 
53  bool lookupPoseHeight(const ros::Time& t, double& poseHeight) const;
54 
56  void applyOdomTransform(Particles& particles, const tf::Transform& odomTransform);
57 
60  bool applyOdomTransformTemporal(Particles& particles, const ros::Time& t, double dt);
61 
63  void storeOdomPose(const tf::Stamped<tf::Pose>& odomPose);
64 
67  bool getLastOdomPose(tf::Stamped<tf::Pose>& lastOdomPose) const;
68 
69 
70  // aligns fixed-size members in memory
71  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
72 
73 protected:
75  void applyOdomTransform(tf::Pose& particlePose, const tf::Transform& odomTransform);
76 
78  void transformPose(tf::Pose& particlePose, const tf::Transform& odomTransform);
79 
82  tf::Transform odomTransformNoise(const tf::Transform& odomTransform);
83 
85  tf::Transform calibrateOdometry(const tf::Transform& odomTransform) const;
86 
88 
89  NormalGeneratorT m_rngNormal; // standard normal-distributed noise
91  // parameters:
93  Eigen::Matrix3d m_odomNoise2D;
95  Eigen::Matrix3d m_odomCalibration2D;
97  double m_odomNoiseZ;
102 
103 
104  std::string m_odomFrameId;
105  std::string m_baseFrameId;
106  std::string m_footprintFrameId;
107 
110 
111 
112 };
113 }
114 
115 #endif
void storeOdomPose(const tf::Stamped< tf::Pose > &odomPose)
store odomPose as m_lastOdomPose to compute future odom transforms
tf::Transform odomTransformNoise(const tf::Transform &odomTransform)
Definition: MotionModel.cpp:93
double m_odomNoisePitch
std.dev param of noise in pitch (depending on distance traveled)
Definition: MotionModel.h:101
bool lookupLocalTransform(const std::string &targetFrame, const ros::Time &t, tf::StampedTransform &localTransform) const
lookup the local target frame in the base frame (local transform)
boost::variate_generator< EngineT &, NormalDistributionT > NormalGeneratorT
standard normal-distributed noise
boost::mt19937 EngineT
Boost RNG engine:
tf::TransformListener * m_tfListener
Definition: MotionModel.h:87
Eigen::Matrix3d m_odomCalibration2D
systematic calibration parameters for odometry drift
Definition: MotionModel.h:95
double m_odomNoiseZ
std.dev param of noise in Z (depending on distance traveled)
Definition: MotionModel.h:97
MotionModel(ros::NodeHandle *nh, EngineT *rngEngine, tf::TransformListener *tf, const std::string &odomFrameId, const std::string &baseFrameId)
Definition: MotionModel.cpp:31
bool getLastOdomPose(tf::Stamped< tf::Pose > &lastOdomPose) const
bool lookupOdomPose(const ros::Time &t, tf::Stamped< tf::Pose > &odomPose) const
look up the odom pose at a certain time through tf
void transformPose(tf::Pose &particlePose, const tf::Transform &odomTransform)
Transform a particle&#39;s pose with the relative odomTransform with added random noise.
Eigen::Matrix3d m_odomNoise2D
variance parameters for calibrated odometry noise
Definition: MotionModel.h:93
tf::Transform computeOdomTransform(const tf::Transform currentPose) const
boost::variate_generator< EngineT &, UniformDistributionT > UniformGeneratorT
uniform noise in range [0,1)
tf::Transform calibrateOdometry(const tf::Transform &odomTransform) const
bool lookupOdomTransform(const ros::Time &t, tf::Transform &odomTransform) const
looks up the odometry pose at time t and then calls computeOdomTransform()
bool applyOdomTransformTemporal(Particles &particles, const ros::Time &t, double dt)
std::vector< Particle > Particles
double m_odomNoiseRoll
std.dev param of noise in roll (depending on distance traveled)
Definition: MotionModel.h:99
bool lookupPoseHeight(const ros::Time &t, double &poseHeight) const
lookup the height of the torso, based on tf between the base and footprint frames ...
void applyOdomTransform(Particles &particles, const tf::Transform &odomTransform)
apply odomTransform to all particles (with random noise and calibration)
tf::Stamped< tf::Pose > m_lastOdomPose
Definition: MotionModel.h:109


humanoid_localization
Author(s): Armin Hornung, Stefan Osswald, Daniel Maier
autogenerated on Mon Jun 10 2019 13:38:31