RaycastingModel.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/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           float raycastRange = (originP - end).norm();
00098           float z = raycastRange - *ranges_it;
00099           float sigma_scaled = m_sigmaHit;
00100           if (m_use_squared_error)
00101              sigma_scaled = (*ranges_it) * (*ranges_it) * (m_sigmaHit);
00102 
00103           // obstacle hit:
00104           p = m_zHit / (SQRT_2_PI * sigma_scaled) * exp(-(z * z) / (2 * sigma_scaled * sigma_scaled));
00105 
00106           // short range:
00107           if (*ranges_it <= raycastRange)
00108             p += m_zShort * m_lambdaShort * exp(-m_lambdaShort* (*ranges_it)) / (1-exp(-m_lambdaShort*raycastRange));
00109 
00110           // random measurement:
00111           p += m_zRand / max_range;
00112         } else { // racasting did not hit, but measurement is no maxrange => random?
00113           p = m_zRand / max_range;
00114         }
00115 
00116       } else{ // maximum range
00117         p = m_zMax;
00118       }
00119 
00120       // add log-likelihood
00121       // (note: likelihood can be larger than 1!)
00122       assert(p > 0.0);
00123       particles[i].weight += log(p);
00124 
00125     } // end of loop over scan
00126 
00127   } // end of loop over particles
00128 
00129 
00130 }
00131 
00132 bool RaycastingModel::getHeightError(const Particle& p, const tf::StampedTransform& footprintToBase, double& heightError) const{
00133 
00134   octomap::point3d direction = octomap::pointTfToOctomap(footprintToBase.inverse().getOrigin());
00135   octomap::point3d origin = octomap::pointTfToOctomap(p.pose.getOrigin());
00136   octomap::point3d end;
00137   // cast ray to bottom:
00138   if (!m_map->castRay(origin, direction, end, true, 2*direction.norm()))
00139     return false;
00140 
00141   heightError =  std::max(0.0, std::abs((origin-end).z() - footprintToBase.getOrigin().z()) - m_map->getResolution());
00142   //ROS_INFO("Height error: %f", heightError);
00143 
00144   return true;
00145 }
00146 
00147 }


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