$search
00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/humanoid_localization/src/RaycastingModel.cpp $ 00002 // SVN $Id: RaycastingModel.cpp 3856 2013-01-31 17:24:18Z 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/RaycastingModel.h> 00025 00026 #include <pcl/point_types.h> 00027 #include <pcl/ros/conversions.h> 00028 #include <pcl_ros/transforms.h> 00029 #include <octomap_ros/conversions.h> 00030 00031 namespace humanoid_localization{ 00032 00033 RaycastingModel::RaycastingModel(ros::NodeHandle* nh, boost::shared_ptr<MapModel> mapModel, EngineT * rngEngine) 00034 : ObservationModel(nh, mapModel, rngEngine) 00035 { 00036 // params: 00037 nh->param("raycasting/z_hit", m_zHit, 0.8); 00038 nh->param("raycasting/z_short", m_zShort, 0.1); 00039 nh->param("raycasting/z_max", m_zMax, 0.05); 00040 nh->param("raycasting/z_rand", m_zRand, 0.05); 00041 nh->param("raycasting/sigma_hit", m_sigmaHit, 0.02); 00042 nh->param("raycasting/lambda_short", m_lambdaShort, 0.1); 00043 00044 if (m_zMax <= 0.0){ 00045 ROS_ERROR("raycasting/z_max needs to be > 0.0"); 00046 } 00047 00048 if (m_zRand <= 0.0){ 00049 ROS_ERROR("raycasting/z_rand needs to be > 0.0"); 00050 } 00051 } 00052 00053 RaycastingModel::~RaycastingModel(){ 00054 00055 } 00056 00057 void RaycastingModel::integrateMeasurement(Particles& particles, const PointCloud& pc, const std::vector<float>& ranges, float max_range, const tf::Transform& base_to_laser){ 00058 assert(pc.size() == ranges.size()); 00059 00060 if (!m_map){ 00061 ROS_ERROR("Map file is not set in raycasting"); 00062 return; 00063 } 00064 // iterate over samples, multi-threaded: 00065 #pragma omp parallel for 00066 for (unsigned i=0; i < particles.size(); ++i){ 00067 Eigen::Matrix4f globalLaserOrigin; 00068 tf::Transform globalLaserOriginTf = particles[i].pose * base_to_laser; 00069 pcl_ros::transformAsMatrix(globalLaserOriginTf, globalLaserOrigin); 00070 00071 // raycasting origin 00072 octomap::point3d originP(globalLaserOriginTf.getOrigin().x(), 00073 globalLaserOriginTf.getOrigin().y(), 00074 globalLaserOriginTf.getOrigin().z()); 00075 PointCloud pc_transformed; 00076 pcl::transformPointCloud(pc, pc_transformed, globalLaserOrigin); 00077 00078 // iterate over beams: 00079 PointCloud::const_iterator pc_it = pc_transformed.begin(); 00080 std::vector<float>::const_iterator ranges_it = ranges.begin(); 00081 for ( ; pc_it != pc_transformed.end(); ++pc_it, ++ranges_it){ 00082 00083 double p = 0.0; // probability for weight 00084 00085 if (*ranges_it <= max_range){ 00086 00087 // direction of ray in global (map) coords 00088 octomap::point3d direction(pc_it->x , pc_it->y, pc_it->z); 00089 direction = direction - originP; 00090 00091 // TODO: check first if endpoint is within map? 00092 octomap::point3d end; 00093 // raycast in OctoMap, we need to cast a little longer than max_range 00094 // to correct for particle drifts away from obstacles 00095 if(m_map->castRay(originP,direction, end, true, 1.5*max_range)){ 00096 assert(m_map->isNodeOccupied(m_map->search(end))); 00097 // TODO: use squared distances? 00098 float raycastRange = (originP - end).norm(); 00099 float z = raycastRange - *ranges_it; 00100 00101 // obstacle hit: 00102 p = m_zHit / (SQRT_2_PI * m_sigmaHit) * exp(-(z * z) / (2 * m_sigmaHit * m_sigmaHit)); 00103 00104 // short range: 00105 if (*ranges_it <= raycastRange) 00106 p += m_zShort * m_lambdaShort * exp(-m_lambdaShort* (*ranges_it)) / (1-exp(-m_lambdaShort*raycastRange)); 00107 00108 // random measurement: 00109 p += m_zRand / max_range; 00110 } else { // racasting did not hit, but measurement is no maxrange => random? 00111 p = m_zRand / max_range; 00112 } 00113 00114 } else{ // maximum range 00115 p = m_zMax; 00116 } 00117 00118 // add log-likelihood 00119 // (note: likelihood can be larger than 1!) 00120 assert(p > 0.0); 00121 particles[i].weight += log(p); 00122 00123 } // end of loop over scan 00124 00125 } // end of loop over particles 00126 00127 00128 } 00129 00130 bool RaycastingModel::getHeightError(const Particle& p, const tf::StampedTransform& footprintToBase, double& heightError) const{ 00131 00132 octomap::point3d direction = octomap::pointTfToOctomap(footprintToBase.inverse().getOrigin()); 00133 octomap::point3d origin = octomap::pointTfToOctomap(p.pose.getOrigin()); 00134 octomap::point3d end; 00135 // cast ray to bottom: 00136 if (!m_map->castRay(origin, direction, end, true, 2*direction.norm())) 00137 return false; 00138 00139 heightError = std::max(0.0, std::abs((origin-end).z() - p.pose.getOrigin().getZ()) - m_map->getResolution()); 00140 // ROS_INFO("Height error: %f", heightError); 00141 00142 return true; 00143 } 00144 00145 }