EndpointModel.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/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 (precomputing...)");
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     std::vector<float>::const_iterator ranges_it = ranges.begin();
00060     // iterate over beams:
00061     for (PointCloud::const_iterator it = pc_transformed.begin(); it != pc_transformed.end(); ++it, ++ranges_it){
00062       // search only for endpoint in tree
00063       octomap::point3d endPoint(it->x, it->y, it->z);
00064       float dist = m_distanceMap->getDistance(endPoint);
00065       float sigma_scaled = m_sigma;
00066       if (m_use_squared_error)
00067          sigma_scaled = (*ranges_it) * (*ranges_it) * (m_sigma);
00068       if (dist > 0.0){ // endpoint is inside map:
00069         particles[i].weight += logLikelihood(dist, sigma_scaled);
00070       } else { //assign weight of max.distance:
00071         particles[i].weight += logLikelihood(m_maxObstacleDistance, sigma_scaled);
00072       }
00073     }
00074     // TODO: handle max range measurements
00075     //std::cout << "\n";
00076   }
00077 
00078 }
00079 
00080 bool EndpointModel::getHeightError(const Particle& p, const tf::StampedTransform& footprintToBase, double& heightError) const{
00081   tf::Vector3 xyz = p.pose.getOrigin();
00082   double poseHeight = footprintToBase.getOrigin().getZ();
00083   std::vector<double> heights;
00084   m_mapModel->getHeightlist(xyz.getX(), xyz.getY(), 0.6, heights);
00085   if (heights.size() == 0)
00086     return false;
00087 
00088 
00089   // TODO: verify this!
00090   // find nearest z-level:
00091   heightError = std::numeric_limits<double>::max();
00092   for (unsigned i = 0; i< heights.size(); i++){
00093     double dist = std::abs((heights[i] + poseHeight) - xyz.getZ());
00094     if (dist < heightError)
00095       heightError = dist;
00096 
00097   }
00098 
00099   return true;
00100 }
00101 
00102 void EndpointModel::setMap(boost::shared_ptr<octomap::OcTree> map){
00103   m_map = map;
00104   initDistanceMap();
00105 }
00106 
00107 void EndpointModel::initDistanceMap(){
00108   double x,y,z;
00109   m_map->getMetricMin(x,y,z);
00110   octomap::point3d min(x,y,z);
00111   m_map->getMetricMax(x,y,z);
00112   octomap::point3d max(x,y,z);
00113   m_distanceMap = boost::shared_ptr<DynamicEDTOctomap>(new DynamicEDTOctomap(float(m_maxObstacleDistance), &(*m_map), min, max, false));
00114   m_distanceMap->update();
00115   ROS_INFO("Distance map for endpoint model completed");
00116 }
00117 
00118 }
00119 


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