$search
00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/humanoid_localization/src/EndpointModel.cpp $ 00002 // SVN $Id: EndpointModel.cpp 3426 2012-10-17 13:33:04Z hornunga@informatik.uni-freiburg.de $ 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/EndpointModel.h> 00025 00026 namespace humanoid_localization{ 00027 00028 EndpointModel::EndpointModel(ros::NodeHandle* nh, boost::shared_ptr<MapModel> mapModel, EngineT * rngEngine) 00029 : ObservationModel(nh, mapModel, rngEngine), m_sigma(0.2), m_maxObstacleDistance(0.5) 00030 { 00031 ROS_INFO("Using Endpoint observation model"); 00032 00033 nh->param("endpoint/sigma", m_sigma, m_sigma); 00034 nh->param("endpoint/max_obstacle_distance", m_maxObstacleDistance, m_maxObstacleDistance); 00035 00036 if (m_sigma <= 0.0){ 00037 ROS_ERROR("Sigma (std.dev) needs to be > 0 in EndpointModel"); 00038 } 00039 00040 initDistanceMap(); 00041 00042 00043 } 00044 00045 EndpointModel::~EndpointModel(){ 00046 00047 } 00048 00049 void EndpointModel::integrateMeasurement(Particles& particles, const PointCloud& pc, const std::vector<float>& ranges, float max_range, const tf::Transform& baseToSensor){ 00050 00051 // iterate over samples, multithreaded: 00052 #pragma omp parallel for 00053 for (unsigned i=0; i < particles.size(); ++i){ 00054 Eigen::Matrix4f globalLaserOrigin; 00055 pcl_ros::transformAsMatrix(particles[i].pose * baseToSensor, globalLaserOrigin); 00056 PointCloud pc_transformed; 00057 pcl::transformPointCloud(pc, pc_transformed, globalLaserOrigin); 00058 00059 // iterate over beams: 00060 for (PointCloud::const_iterator it = pc_transformed.begin(); it != pc_transformed.end(); ++it){ 00061 // search only for endpoint in tree 00062 octomap::point3d endPoint(it->x, it->y, it->z); 00063 float dist = m_distanceMap->getDistance(endPoint); 00064 if (dist > 0.0){ // endpoint is inside map: 00065 //std::cout << dist << " "; 00066 particles[i].weight += logLikelihood(dist, m_sigma); 00067 } else { //assign weight of max.distance: 00068 particles[i].weight += logLikelihood(m_maxObstacleDistance, m_sigma); 00069 } 00070 } 00071 // TODO: handle max range measurements 00072 //std::cout << "\n"; 00073 } 00074 00075 } 00076 00077 bool EndpointModel::getHeightError(const Particle& p, const tf::StampedTransform& footprintToBase, double& heightError) const{ 00078 tf::Vector3 xyz = p.pose.getOrigin(); 00079 double poseHeight = xyz.getZ(); 00080 std::vector<double> heights; 00081 m_mapModel->getHeightlist(xyz.getX(), xyz.getY(), 0.6, heights); 00082 if (heights.size() == 0) 00083 return false; 00084 00085 00086 // find nearest z-level: 00087 heightError = std::numeric_limits<double>::max(); 00088 for (unsigned i = 0; i< heights.size(); i++){ 00089 double dist = std::abs((heights[i] + poseHeight) - xyz.getZ()); 00090 if (dist < heightError) 00091 heightError = dist; 00092 00093 } 00094 00095 return true; 00096 } 00097 00098 void EndpointModel::setMap(boost::shared_ptr<octomap::OcTree> map){ 00099 m_map = map; 00100 initDistanceMap(); 00101 } 00102 00103 void EndpointModel::initDistanceMap(){ 00104 double x,y,z; 00105 m_map->getMetricMin(x,y,z); 00106 octomap::point3d min(x,y,z); 00107 m_map->getMetricMax(x,y,z); 00108 octomap::point3d max(x,y,z); 00109 m_distanceMap = boost::shared_ptr<DynamicEDTOctomap>(new DynamicEDTOctomap(float(m_maxObstacleDistance), &(*m_map), min, max, false)); 00110 m_distanceMap->update(); 00111 ROS_INFO("Distance map for endpoint model completed"); 00112 } 00113 00114 } 00115