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_RAYCASTINGMODEL_H_ 00025 #define HUMANOID_LOCALIZATION_RAYCASTINGMODEL_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 00035 #include <humanoid_localization/ObservationModel.h> 00036 00037 #include <octomap/octomap.h> 00038 00039 00040 namespace humanoid_localization{ 00041 class RaycastingModel : public ObservationModel { 00042 public: 00043 RaycastingModel(ros::NodeHandle* nh, boost::shared_ptr<MapModel> mapModel, EngineT * rngEngine); 00044 virtual ~RaycastingModel(); 00045 virtual void integrateMeasurement(Particles& particles, const PointCloud& pc, const std::vector<float>& ranges, float max_range, const tf::Transform& baseToSensor); 00046 00047 protected: 00048 bool getHeightError(const Particle& p, const tf::StampedTransform& footprintToBase, double& heightError) const; 00049 // laser parameters: 00050 double m_zHit; 00051 double m_zRand; 00052 double m_zShort; 00053 double m_zMax; 00054 double m_sigmaHit; 00055 double m_lambdaShort; 00056 00057 bool m_filterPointCloudGround; 00058 double m_groundFilterDistance; 00059 double m_groundFilterAngle; 00060 double m_groundFilterPlaneDistance; 00061 int m_numFloorPoints; 00062 int m_numNonFloorPoints; 00063 }; 00064 00065 } 00066 00067 #endif