MapModel.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/MapModel.h>
00025 
00026 namespace humanoid_localization{
00027 MapModel::MapModel(ros::NodeHandle* nh)
00028 : m_motionMeanZ(0.0),
00029   m_motionRangeZ(-1.0), m_motionRangeRoll(-1.0), m_motionRangePitch(-1.0),
00030   m_motionObstacleDist(0.2)
00031 {
00032 
00033   // motion model max ranges (particles have to stay within range)
00034   nh->param("motion_mean_z", m_motionMeanZ, m_motionMeanZ);
00035   nh->param("motion_range_z", m_motionRangeZ, m_motionRangeZ);
00036   nh->param("motion_range_roll", m_motionRangeRoll, m_motionRangeRoll);
00037   nh->param("motion_range_pitch", m_motionRangePitch, m_motionRangePitch);
00038   // this is not correctly used at the moment:
00039   //nh->param("motion_occupied_radius", m_motionObstacleDist, m_motionObstacleDist);
00040 
00041 }
00042 
00043 MapModel::~MapModel(){
00044 
00045 }
00046 
00047 
00048 boost::shared_ptr<octomap::OcTree> MapModel::getMap() const{
00049   return m_map;
00050 }
00051 
00052 void MapModel::verifyPoses(Particles& particles){
00053   double minX, minY, minZ, maxX, maxY, maxZ;
00054   m_map->getMetricMin(minX, minY, minZ);
00055   m_map->getMetricMax(maxX, maxY, maxZ);
00056 
00057   // find min. particle weight:
00058   double minWeight = std::numeric_limits<double>::max();
00059   for (Particles::iterator it = particles.begin(); it != particles.end(); ++it) {
00060     if (it->weight < minWeight)
00061       minWeight = it->weight;
00062 
00063   }
00064 
00065   minWeight -= 200;
00066 
00067   unsigned numWall = 0;
00068   unsigned numOut = 0;
00069   unsigned numMotion = 0;
00070 
00071 
00072   // TODO possible speedup: cluster particles by grid voxels first?
00073   // iterate over samples, multi-threaded:
00074 #pragma omp parallel for
00075   for (unsigned i = 0; i < particles.size(); ++i){
00076 
00077     octomap::point3d position(particles[i].pose.getOrigin().getX(),
00078                               particles[i].pose.getOrigin().getY(),
00079                               particles[i].pose.getOrigin().getZ());
00080 
00081     // see if outside of map bounds:
00082     if (position(0) < minX || position(0) > maxX
00083         ||      position(1) < minY || position(1) > maxY
00084         ||      position(2) < minZ || position(2) > maxZ)
00085     {
00086       particles[i].weight = minWeight;
00087 #pragma omp atomic
00088       numOut++;
00089     } else {
00090 
00091       // see if occupied cell:
00092       if (this->isOccupied(position)){
00093         particles[i].weight = minWeight;
00094 #pragma omp atomic
00095         numWall++;
00096       } else {
00097         // see if current pose is has a valid walking height:
00098         if (m_motionRangeZ >= 0.0 &&
00099             (std::abs(particles[i].pose.getOrigin().getZ() - getFloorHeight(particles[i].pose) - m_motionMeanZ)
00100               > m_motionRangeZ))
00101         {
00102           particles[i].weight = minWeight;
00103 #pragma omp atomic
00104           numMotion++;
00105         } else if (m_motionRangePitch >= 0.0 || m_motionRangeRoll >= 0.0){
00106 
00107           double yaw, pitch, roll;
00108           particles[i].pose.getBasis().getRPY(roll, pitch, yaw);
00109 
00110           if ((m_motionRangePitch >= 0.0 && std::abs(pitch) > m_motionRangePitch)
00111               || (m_motionRangeRoll >= 0.0 && std::abs(roll) > m_motionRangeRoll))
00112           {
00113             particles[i].weight = minWeight;
00114 #pragma omp atomic
00115             numMotion++;
00116           }
00117         }
00118       }
00119     }
00120   } // end loop over particles
00121 
00122   if (numWall > 0 || numOut > 0 || numMotion > 0){
00123     ROS_INFO("Particle weights minimized: %d out of map, %d in obstacles, %d out of motion range", numOut, numWall, numMotion);
00124   }
00125 
00126   if (numOut + numWall >= particles.size()){
00127     ROS_WARN("All particles are out of the valid map area or in obstacles!");
00128   }
00129 
00130 }
00131 
00132 
00133 void MapModel::initGlobal(Particles& particles, double z, double roll, double pitch,
00134                           const Vector6d& initNoise,
00135                           UniformGeneratorT& rngUniform, NormalGeneratorT& rngNormal){
00136   double sizeX,sizeY,sizeZ, minX, minY, minZ;
00137   m_map->getMetricSize(sizeX,sizeY,sizeZ);
00138   m_map->getMetricMin(minX, minY, minZ);
00139 
00140   double weight = 1.0 / particles.size();
00141   Particles::iterator it = particles.begin();
00142   while (true){
00143     if (it == particles.end())
00144       break;
00145     // obtain a pose hypothesis:
00146     double x = minX + sizeX * rngUniform();
00147     double y = minY + sizeY * rngUniform();
00148     std::vector<double> z_list;
00149     getHeightlist(x, y, 0.6,z_list);
00150 
00151     for (unsigned zIdx = 0; zIdx < z_list.size(); zIdx++){
00152       if (it == particles.end())
00153         break;
00154 
00155       // not needed => we already know that z contains valid poses
00156       // distance map: used distance from obstacles:
00157       //std::abs(node->getLogOdds()) < 0.1){
00158       //                        if (!isOccupied(octomap::point3d(x, y, z[zIdx]))){
00159 
00160       it->pose.getOrigin().setX(x);
00161       it->pose.getOrigin().setY(y);
00162       // TODO: sample z, roll, pitch
00163       it->pose.getOrigin().setZ(z_list.at(zIdx) + z + rngNormal() * initNoise(2));
00164       double yaw = rngUniform() * 2 * M_PI  -M_PI;
00165       it->pose.setRotation(tf::createQuaternionFromRPY(roll, pitch, yaw));
00166       it->weight = weight;
00167       it++;
00168     }
00169   }
00170 
00171 }
00172 
00173 void MapModel::getHeightlist(double x, double y, double totalHeight, std::vector<double>& heights){
00174   double minX, minY, minZ, maxX, maxY, maxZ;
00175   m_map->getMetricMin(minX, minY, minZ);
00176   m_map->getMetricMax(maxX, maxY, maxZ);
00177 
00178   double res = m_map->getResolution();
00179 
00180   double z =  maxZ-res/2.0;
00181   double lastZ = z + res;
00182 
00183   while (z >= minZ){
00184     if (isOccupied(octomap::point3d(x, y, z))){
00185       if (lastZ - z >= totalHeight + res){
00186         heights.push_back(z+ res/2.0);
00187       }
00188       lastZ = z;
00189     }
00190 
00191     z -= res;
00192   }
00193 }
00194 
00195 bool MapModel::isOccupied(const octomap::point3d& position) const{
00196   octomap::OcTreeNode* mapNode = m_map->search(position);
00197   if (mapNode)
00198     return isOccupied(mapNode);
00199   else return false;
00200 }
00201 
00202 
00204 // Distance Map (Endpoint Model)
00206 
00207 DistanceMap::DistanceMap(ros::NodeHandle* nh)
00208 : MapModel(nh)
00209 {
00210   ROS_ERROR("Distance map implementation is currently not supported");
00211   std::string mapFileName;
00212   nh->getParam("map_file_dist", mapFileName);
00213 
00214 // TODO: use FileIO, try octree<float>
00215 //  octomap::AbstractOcTree* tree = octomap_msgs::fullMsgDataToMap(resp.map.data);
00216 //  if (tree){
00217 //    ROS_INFO("Received tree type %s",tree->getTreeType().c_str());
00218 //    //octree = dynamic_cast<OcTree*>(tree);
00219 //  }
00220 
00221   octomap::OcTree* tree = dynamic_cast<octomap::OcTree*>(octomap::AbstractOcTree::read(mapFileName));
00222   if (tree){
00223     m_map.reset(tree);
00224   }
00225 
00226   if (!m_map|| m_map->size() <= 1){
00227     ROS_ERROR("Distance map file loaded from \"%s\" is erroneous, exiting...", mapFileName.c_str());
00228     exit(-1);
00229   }
00230   double x,y,z;
00231   m_map->getMetricSize(x,y,z);
00232   ROS_INFO("Distance map initialized with %zd nodes (%.2f x %.2f x %.2f m)", m_map->size(), x,y,z);
00233 
00234 }
00235 
00236 DistanceMap::~DistanceMap(){
00237 
00238 }
00239 
00240 bool DistanceMap::isOccupied(octomap::OcTreeNode* node) const{
00241   if (std::abs(node->getLogOdds()) < m_map->getResolution())
00242     return true;
00243   else
00244     return false;
00245 }
00246 
00247 double DistanceMap::getFloorHeight(const tf::Transform& pose) const{
00248   // TODO:
00249   ROS_ERROR("DistanceMap::getFloorHeight not implemented yet!");
00250 
00251   return 0.0;
00252 }
00253 
00255 // Occupancy Map (Raycasting)
00257 
00258 
00259 OccupancyMap::OccupancyMap(ros::NodeHandle* nh)
00260 : MapModel(nh)
00261 {
00262   std::string servname = "octomap_binary";
00263   ROS_INFO("Requesting the map from %s...", nh->resolveName(servname).c_str());
00264   octomap_msgs::GetOctomap::Request req;
00265   octomap_msgs::GetOctomap::Response resp;
00266   while(nh->ok() && !ros::service::call(servname, req, resp))
00267   {
00268     ROS_WARN("Request to %s failed; trying again...", nh->resolveName(servname).c_str());
00269     usleep(1000000);
00270   }
00271 
00272 
00273 // Groovy:
00274 #if ROS_VERSION_MINIMUM(1, 9, 0)
00275   m_map.reset(octomap_msgs::binaryMsgToMap(resp.map));
00276 #else  // Fuerte:
00277   m_map.reset(octomap_msgs::binaryMsgDataToMap(resp.map.data));
00278 #endif
00279 
00280   if (!m_map || m_map->size() <= 1){
00281     ROS_ERROR("Occupancy map is erroneous, exiting...");
00282     exit(-1);
00283   }
00284   double x,y,z;
00285   m_map->getMetricSize(x,y,z);
00286   ROS_INFO("Occupancy map initialized with %zd nodes (%.2f x %.2f x %.2f m), %f m res.", m_map->size(), x,y,z, m_map->getResolution());
00287 
00288 }
00289 
00290 OccupancyMap::~OccupancyMap(){
00291 
00292 }
00293 
00294 bool OccupancyMap::isOccupied(octomap::OcTreeNode* node) const{
00295   return m_map->isNodeOccupied(node);
00296 }
00297 
00298 
00299 double OccupancyMap::getFloorHeight(const tf::Transform& pose)const {
00300   octomap::point3d end;
00301   if (m_map->castRay(octomap::pointTfToOctomap(pose.getOrigin()), octomap::point3d(0.0, 0.0, -1.0), end, false)){
00302     // add resolution/2 so height is above voxel boundary:
00303     return end.z()+m_map->getResolution()/2.0;
00304   } else {
00305     ROS_WARN("getFloorHeight raycast did not succeed, using 0.0");
00306     return 0.0;
00307   }
00308 
00309 }
00310 
00311 }
00312 


humanoid_localization
Author(s): Armin Hornung, Stefan Osswald, Daniel Maier
autogenerated on Wed Aug 26 2015 11:54:40