HectorMapTools.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef __HectorMapTools_h_
00030 #define __HectorMapTools_h_
00031 
00032 #include <nav_msgs/OccupancyGrid.h>
00033 #include <nav_msgs/MapMetaData.h>
00034 
00035 #include<Eigen/Core>
00036 
00037 class HectorMapTools{
00038 public:
00039 
00040   template<typename ConcreteScalar>
00041   class CoordinateTransformer{
00042   public:
00043 
00044     CoordinateTransformer()
00045     {
00046 
00047     }
00048 
00049     CoordinateTransformer(const nav_msgs::OccupancyGridConstPtr map)
00050     {
00051       this->setTransforms(*map);
00052     }
00053 
00054 
00055     void setTransforms(const nav_msgs::OccupancyGrid& map)
00056     {
00057       this->setTransforms(map.info);
00058     }
00059 
00060     void setTransforms(const nav_msgs::MapMetaData& meta)
00061     {
00062       origo_ = (Eigen::Matrix<ConcreteScalar, 2, 1>(static_cast<ConcreteScalar>(meta.origin.position.x),static_cast<ConcreteScalar>(meta.origin.position.y)));
00063       scale_ = (static_cast<ConcreteScalar>(meta.resolution));
00064       inv_scale_ = (static_cast<ConcreteScalar>(1.0f / meta.resolution));
00065     }
00066 
00067     void setTransformsBetweenCoordSystems(const Eigen::Matrix<ConcreteScalar, 2, 1>& origoCS1, const Eigen::Matrix<ConcreteScalar, 2, 1>& endCS1,
00068                                           const Eigen::Matrix<ConcreteScalar, 2, 1>& origoCS2, const Eigen::Matrix<ConcreteScalar, 2, 1>& endCS2)
00069     {
00070       Eigen::Matrix<ConcreteScalar, 2, 1> map_t_geotiff_x_factors = getLinearTransform(Eigen::Vector2f(origoCS1[0], endCS1[0]), Eigen::Vector2f(origoCS2[0], endCS2[0]));
00071       Eigen::Matrix<ConcreteScalar, 2, 1> map_t_geotiff_y_factors = getLinearTransform(Eigen::Vector2f(origoCS1[1], endCS1[1]), Eigen::Vector2f(origoCS2[1], endCS2[1]));
00072 
00073       //std::cout << "\n geo_x: \n" << map_t_geotiff_x_factors << "\n geo_y: \n" << map_t_geotiff_y_factors << "\n";
00074 
00075       origo_.x() = map_t_geotiff_x_factors[1];
00076       origo_.y() = map_t_geotiff_y_factors[1];
00077 
00078       scale_ = map_t_geotiff_x_factors[0];
00079       inv_scale_ = 1.0 / scale_;
00080 
00081       //std::cout << "\nscale " << scale_ << "\n";
00082     }
00083 
00084     Eigen::Matrix<ConcreteScalar, 2, 1> getC1Coords(const  Eigen::Matrix<ConcreteScalar, 2, 1>& mapCoords) const
00085     {
00086       return origo_ + (mapCoords * scale_);
00087     }
00088 
00089     Eigen::Matrix<ConcreteScalar, 2, 1> getC2Coords(const  Eigen::Matrix<ConcreteScalar, 2, 1>& worldCoords) const
00090     {
00091       return ((worldCoords - origo_) * inv_scale_);
00092     }
00093 
00094     ConcreteScalar getC1Scale(float c2_scale) const
00095     {
00096       return scale_ * c2_scale;
00097     }
00098 
00099     ConcreteScalar getC2Scale(float c1_scale) const
00100     {
00101       return inv_scale_ * c1_scale;
00102     }
00103 
00104   protected:
00105 
00106     Eigen::Matrix<ConcreteScalar, 2, 1> getLinearTransform(const Eigen::Matrix<ConcreteScalar, 2, 1>& coordSystem1, const Eigen::Matrix<ConcreteScalar, 2, 1>& coordSystem2)
00107     {
00108       ConcreteScalar scaling = (coordSystem2[0] - coordSystem2[1]) / (coordSystem1[0] - coordSystem1[1]);
00109       ConcreteScalar translation = coordSystem2[0] - coordSystem1[0] * scaling;
00110       return Eigen::Vector2f (scaling, translation);
00111     }
00112 
00113     Eigen::Matrix<ConcreteScalar, 2, 1> origo_;
00114     ConcreteScalar scale_;
00115     ConcreteScalar inv_scale_;
00116   };
00117 
00118   class DistanceMeasurementProvider
00119   {
00120   public:
00121     DistanceMeasurementProvider()
00122     {
00123 
00124     }
00125 
00126     void setMap(const nav_msgs::OccupancyGridConstPtr map)
00127     {
00128       map_ptr_ = map;
00129 
00130       world_map_transformer_.setTransforms(*map_ptr_);
00131     }
00132 
00133     float getDist(const Eigen::Vector2f& begin_world, const Eigen::Vector2f& end_world, Eigen::Vector2f* hitCoords = 0)
00134     {
00135       Eigen::Vector2i end_point_map;
00136 
00137       Eigen::Vector2i begin_map (world_map_transformer_.getC2Coords(begin_world).cast<int>());
00138       Eigen::Vector2i end_map (world_map_transformer_.getC2Coords(end_world).cast<int>());
00139       float dist = checkOccupancyBresenhami(begin_map, end_map, &end_point_map);
00140 
00141       if (hitCoords != 0){
00142         *hitCoords = world_map_transformer_.getC1Coords(end_point_map.cast<float>());
00143       }
00144 
00145       return world_map_transformer_.getC1Scale(dist);
00146     }
00147 
00148     inline float checkOccupancyBresenhami( const Eigen::Vector2i& beginMap, const Eigen::Vector2i& endMap, Eigen::Vector2i* hitCoords = 0, unsigned int max_length = UINT_MAX){
00149 
00150       int x0 = beginMap[0];
00151       int y0 = beginMap[1];
00152 
00153       int sizeX = map_ptr_->info.width;
00154       int sizeY = map_ptr_->info.height;
00155 
00156       //check if beam start point is inside map, cancel update if this is not the case
00157       if ((x0 < 0) || (x0 >= sizeX) || (y0 < 0) || (y0 >= sizeY)) {
00158         return -1.0f;
00159       }
00160 
00161       int x1 = endMap[0];
00162       int y1 = endMap[1];
00163 
00164       //std::cout << " x: "<< x1 << " y: " << y1 << " length: " << length << "     ";
00165 
00166       //check if beam end point is inside map, cancel update if this is not the case
00167       if ((x1 < 0) || (x1 >= sizeX) || (y1 < 0) || (y1 >= sizeY)) {
00168         return -1.0f;
00169       }
00170 
00171       int dx = x1 - x0;
00172       int dy = y1 - y0;
00173 
00174       unsigned int abs_dx = abs(dx);
00175       unsigned int abs_dy = abs(dy);
00176 
00177       int offset_dx =  dx > 0 ? 1 : -1;
00178       int offset_dy = (dy > 0 ? 1 : -1) * sizeX;
00179 
00180       unsigned int startOffset = beginMap.y() * sizeX + beginMap.x();
00181 
00182       int end_offset;
00183 
00184       //if x is dominant
00185       if(abs_dx >= abs_dy){
00186         int error_y = abs_dx / 2;
00187         end_offset = bresenham2D(abs_dx, abs_dy, error_y, offset_dx, offset_dy, startOffset,5000);
00188       }else{
00189         //otherwise y is dominant
00190         int error_x = abs_dy / 2;
00191         end_offset = bresenham2D(abs_dy, abs_dx, error_x, offset_dy, offset_dx, startOffset,5000);
00192       }
00193 
00194       if (end_offset != -1){
00195         Eigen::Vector2i end_coords(end_offset % sizeY, end_offset / sizeY);
00196 
00197         int distMap = ((beginMap - end_coords).cast<float>()).norm();
00198 
00199         if (hitCoords != 0){
00200           *hitCoords = end_coords;
00201         }
00202 
00203         return distMap;
00204       }
00205 
00206       return -1.0f;
00207 
00208       //unsigned int endOffset = endMap.y() * sizeX + endMap.x();
00209       //this->bresenhamCellOcc(endOffset);
00210     }
00211 
00212     inline int bresenham2D(unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b,
00213                             unsigned int offset, unsigned int max_length){
00214       unsigned int end = std::min(max_length, abs_da);
00215 
00216       const std::vector<int8_t>& data = map_ptr_->data;
00217 
00218       for(unsigned int i = 0; i < end; ++i){
00219         if (data[offset] == 100){
00220           return static_cast<int>(offset);
00221         }
00222 
00223         offset += offset_a;
00224         error_b += abs_db;
00225         if((unsigned int)error_b >= abs_da){
00226           offset += offset_b;
00227           error_b -= abs_da;
00228         }
00229       }
00230       return -1;
00231       //at(offset);
00232     }
00233 
00234   protected:
00235     CoordinateTransformer<float> world_map_transformer_;
00236     nav_msgs::OccupancyGridConstPtr map_ptr_;
00237 
00238 
00239   };
00240 
00241   static bool getMapExtends(const nav_msgs::OccupancyGrid& map, Eigen::Vector2i& topLeft, Eigen::Vector2i& bottomRight)
00242   {
00243     int lowerStart = -1;
00244     int upperStart = 10000000;
00245 
00246     int xMaxTemp = lowerStart;
00247     int yMaxTemp = lowerStart;
00248     int xMinTemp = upperStart;
00249     int yMinTemp = upperStart;
00250 
00251     int width = map.info.width;
00252     int height = map.info.height;
00253 
00254     for (int y = 0; y < height; ++y){
00255       for (int x = 0; x < width; ++x){
00256 
00257         if ( map.data[x+y*width] != -1){
00258 
00259           if (x > xMaxTemp) {
00260             xMaxTemp = x;
00261           }
00262 
00263           if (x < xMinTemp) {
00264             xMinTemp = x;
00265           }
00266 
00267           if (y > yMaxTemp) {
00268             yMaxTemp = y;
00269           }
00270 
00271           if (y < yMinTemp) {
00272             yMinTemp = y;
00273           }
00274         }
00275       }
00276     }
00277 
00278     if ((xMaxTemp != lowerStart) &&
00279         (yMaxTemp != lowerStart) &&
00280         (xMinTemp != upperStart) &&
00281         (yMinTemp != upperStart)) {
00282 
00283       topLeft = Eigen::Vector2i(xMinTemp,yMinTemp);
00284       bottomRight = Eigen::Vector2i(xMaxTemp+1, yMaxTemp+1);
00285 
00286       return true;
00287     } else {
00288       return false;
00289     }
00290   };
00291 };
00292 
00293 #endif


hector_map_tools
Author(s): Stefan Kohlbrecher
autogenerated on Mon Oct 6 2014 00:32:29