RaycastingModel.cpp
Go to the documentation of this file.
1 /*
2  * 6D localization for humanoid robots
3  *
4  * Copyright 2009-2012 Armin Hornung, University of Freiburg
5  * http://www.ros.org/wiki/humanoid_localization
6  *
7  *
8  * This program is free software: you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation, version 3.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program. If not, see <http://www.gnu.org/licenses/>.
19  */
20 
22 
23 #include <pcl/point_types.h>
24 #include <pcl/conversions.h>
25 #include <pcl_ros/transforms.h>
27 
28 namespace humanoid_localization{
29 
31 : ObservationModel(nh, mapModel, rngEngine)
32 {
33  // params:
34  nh->param("raycasting/z_hit", m_zHit, 0.8);
35  nh->param("raycasting/z_short", m_zShort, 0.1);
36  nh->param("raycasting/z_max", m_zMax, 0.05);
37  nh->param("raycasting/z_rand", m_zRand, 0.05);
38  nh->param("raycasting/sigma_hit", m_sigmaHit, 0.02);
39  nh->param("raycasting/lambda_short", m_lambdaShort, 0.1);
40 
41  if (m_zMax <= 0.0){
42  ROS_ERROR("raycasting/z_max needs to be > 0.0");
43  }
44 
45  if (m_zRand <= 0.0){
46  ROS_ERROR("raycasting/z_rand needs to be > 0.0");
47  }
48  #pragma omp parallel
49  #pragma omp critical
50  {
51  if (omp_get_thread_num() == 0){
52  ROS_INFO("Using %d threads in RaycastingModel", omp_get_num_threads());
53  }
54  }
55 }
56 
58 
59 }
60 
61 void RaycastingModel::integrateMeasurement(Particles& particles, const PointCloud& pc, const std::vector<float>& ranges, float max_range, const tf::Transform& base_to_laser){
62  assert(pc.size() == ranges.size());
63 
64  if (!m_map){
65  ROS_ERROR("Map file is not set in raycasting");
66  return;
67  }
68  // iterate over samples, multi-threaded:
69 #pragma omp parallel for
70  for (unsigned i=0; i < particles.size(); ++i){
71  Eigen::Matrix4f globalLaserOrigin;
72  tf::Transform globalLaserOriginTf = particles[i].pose * base_to_laser;
73  pcl_ros::transformAsMatrix(globalLaserOriginTf, globalLaserOrigin);
74 
75  // raycasting origin
76  octomap::point3d originP(globalLaserOriginTf.getOrigin().x(),
77  globalLaserOriginTf.getOrigin().y(),
78  globalLaserOriginTf.getOrigin().z());
79  PointCloud pc_transformed;
80  pcl::transformPointCloud(pc, pc_transformed, globalLaserOrigin);
81 
82  // iterate over beams:
83  PointCloud::const_iterator pc_it = pc_transformed.begin();
84  std::vector<float>::const_iterator ranges_it = ranges.begin();
85  for ( ; pc_it != pc_transformed.end(); ++pc_it, ++ranges_it){
86 
87  double p = 0.0; // probability for weight
88 
89  if (*ranges_it <= max_range){
90 
91  // direction of ray in global (map) coords
92  octomap::point3d direction(pc_it->x , pc_it->y, pc_it->z);
93  direction = direction - originP;
94 
95  // TODO: check first if endpoint is within map?
96  octomap::point3d end;
97  // raycast in OctoMap, we need to cast a little longer than max_range
98  // to correct for particle drifts away from obstacles
99  if(m_map->castRay(originP,direction, end, true, 1.5*max_range)){
100  assert(m_map->isNodeOccupied(m_map->search(end)));
101  float raycastRange = (originP - end).norm();
102  float z = raycastRange - *ranges_it;
103  float sigma_scaled = m_sigmaHit;
105  sigma_scaled = (*ranges_it) * (*ranges_it) * (m_sigmaHit);
106 
107  // obstacle hit:
108  p = m_zHit / (SQRT_2_PI * sigma_scaled) * exp(-(z * z) / (2 * sigma_scaled * sigma_scaled));
109 
110  // short range:
111  if (*ranges_it <= raycastRange)
112  p += m_zShort * m_lambdaShort * exp(-m_lambdaShort* (*ranges_it)) / (1-exp(-m_lambdaShort*raycastRange));
113 
114  // random measurement:
115  p += m_zRand / max_range;
116  } else { // racasting did not hit, but measurement is no maxrange => random?
117  p = m_zRand / max_range;
118  }
119 
120  } else{ // maximum range
121  p = m_zMax;
122  }
123 
124  // add log-likelihood
125  // (note: likelihood can be larger than 1!)
126  assert(p > 0.0);
127  particles[i].weight += log(p);
128 
129  } // end of loop over scan
130 
131  } // end of loop over particles
132 
133 
134 }
135 
136 bool RaycastingModel::getHeightError(const Particle& p, const tf::StampedTransform& footprintToBase, double& heightError) const{
137 
138  octomap::point3d direction = octomap::pointTfToOctomap(footprintToBase.inverse().getOrigin());
140  octomap::point3d end;
141  // cast ray to bottom:
142  if (!m_map->castRay(origin, direction, end, true, 2*direction.norm()))
143  return false;
144 
145  heightError = std::max(0.0, std::abs((origin-end).z() - footprintToBase.getOrigin().z()) - m_map->getResolution());
146  //ROS_INFO("Height error: %f", heightError);
147 
148  return true;
149 }
150 
151 }
boost::shared_ptr< octomap::OcTree > m_map
boost::mt19937 EngineT
Boost RNG engine:
double norm() const
void transformAsMatrix(const tf::Transform &bt, Eigen::Matrix4f &out_mat)
Particle consists of a pose and a weight.
static octomap::point3d pointTfToOctomap(const tf::Point &ptTf)
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
#define ROS_INFO(...)
static const double SQRT_2_PI
sqrt(2*pi)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
TFSIMD_FORCE_INLINE const tfScalar & y() const
Transform inverse() const
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
TFSIMD_FORCE_INLINE const tfScalar & z() const
virtual void integrateMeasurement(Particles &particles, const PointCloud &pc, const std::vector< float > &ranges, float max_range, const tf::Transform &baseToSensor)
bool getHeightError(const Particle &p, const tf::StampedTransform &footprintToBase, double &heightError) const
RaycastingModel(ros::NodeHandle *nh, boost::shared_ptr< MapModel > mapModel, EngineT *rngEngine)
std::vector< Particle > Particles
pcl::PointCloud< pcl::PointXYZ > PointCloud
#define ROS_ERROR(...)


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