OccGridMapBase.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 __OccGridMapBase_h_
00030 #define __OccGridMapBase_h_
00031 
00032 #include "GridMapBase.h"
00033 
00034 #include "../scan/DataPointContainer.h"
00035 #include "../util/UtilFunctions.h"
00036 
00037 #include <Eigen/Geometry>
00038 
00039 namespace hectorslam {
00040 
00041 template<typename ConcreteCellType, typename ConcreteGridFunctions>
00042 class OccGridMapBase
00043   : public GridMapBase<ConcreteCellType>
00044 {
00045 
00046 public:
00047 
00048   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00049 
00050   OccGridMapBase(float mapResolution, const Eigen::Vector2i& size, const Eigen::Vector2f& offset)
00051     : GridMapBase<ConcreteCellType>(mapResolution, size, offset)
00052     , currUpdateIndex(0)
00053     , currMarkOccIndex(-1)
00054     , currMarkFreeIndex(-1)
00055   {}
00056 
00057   virtual ~OccGridMapBase() {}
00058 
00059   void updateSetOccupied(int index)
00060   {
00061     concreteGridFunctions.updateSetOccupied(this->getCell(index));
00062   }
00063 
00064   void updateSetFree(int index)
00065   {
00066     concreteGridFunctions.updateSetFree(this->getCell(index));
00067   }
00068 
00069   void updateUnsetFree(int index)
00070   {
00071     concreteGridFunctions.updateUnsetFree(this->getCell(index));
00072   }
00073 
00074   float getGridProbabilityMap(int index) const
00075   {
00076     return concreteGridFunctions.getGridProbability(this->getCell(index));
00077   }
00078 
00079   bool isOccupied(int xMap, int yMap) const
00080   {
00081     return (this->getCell(xMap,yMap).isOccupied());
00082   }
00083 
00084   bool isFree(int xMap, int yMap) const
00085   {
00086     return (this->getCell(xMap,yMap).isFree());
00087   }
00088 
00089   bool isOccupied(int index) const
00090   {
00091     return (this->getCell(index).isOccupied());
00092   }
00093 
00094   bool isFree(int index) const
00095   {
00096     return (this->getCell(index).isFree());
00097   }
00098 
00099   float getObstacleThreshold() const
00100   {
00101     ConcreteCellType temp;
00102     temp.resetGridCell();
00103     return concreteGridFunctions.getGridProbability(temp);
00104   }
00105 
00106   void setUpdateFreeFactor(float factor)
00107   {
00108     concreteGridFunctions.setUpdateFreeFactor(factor);
00109   }
00110 
00111   void setUpdateOccupiedFactor(float factor)
00112   {
00113     concreteGridFunctions.setUpdateOccupiedFactor(factor);
00114   }
00115 
00121   void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld)
00122   {
00123     currMarkFreeIndex = currUpdateIndex + 1;
00124     currMarkOccIndex = currUpdateIndex + 2;
00125 
00126     //Get pose in map coordinates from pose in world coordinates
00127     Eigen::Vector3f mapPose(this->getMapCoordsPose(robotPoseWorld));
00128 
00129     //Get a 2D homogenous transform that can be left-multiplied to a robot coordinates vector to get world coordinates of that vector
00130     Eigen::Affine2f poseTransform((Eigen::Translation2f(
00131                                         mapPose[0], mapPose[1]) * Eigen::Rotation2Df(mapPose[2])));
00132 
00133     //Get start point of all laser beams in map coordinates (same for alle beams, stored in robot coords in dataContainer)
00134     Eigen::Vector2f scanBeginMapf(poseTransform * dataContainer.getOrigo());
00135 
00136     //Get integer vector of laser beams start point
00137     Eigen::Vector2i scanBeginMapi(scanBeginMapf[0] + 0.5f, scanBeginMapf[1] + 0.5f);
00138 
00139     //Get number of valid beams in current scan
00140     int numValidElems = dataContainer.getSize();
00141 
00142     //std::cout << "\n maxD: " << maxDist << " num: " << numValidElems << "\n";
00143 
00144     //Iterate over all valid laser beams
00145     for (int i = 0; i < numValidElems; ++i) {
00146 
00147       //Get map coordinates of current beam endpoint
00148       Eigen::Vector2f scanEndMapf(poseTransform * (dataContainer.getVecEntry(i)));
00149       //std::cout << "\ns\n" << scanEndMapf << "\n";
00150 
00151       //add 0.5 to beam endpoint vector for following integer cast (to round, not truncate)
00152       scanEndMapf.array() += (0.5f);
00153 
00154       //Get integer map coordinates of current beam endpoint
00155       Eigen::Vector2i scanEndMapi(scanEndMapf.cast<int>());
00156 
00157       //Update map using a bresenham variant for drawing a line from beam start to beam endpoint in map coordinates
00158       if (scanBeginMapi != scanEndMapi){
00159         updateLineBresenhami(scanBeginMapi, scanEndMapi);
00160       }
00161     }
00162 
00163     //Tell the map that it has been updated
00164     this->setUpdated();
00165 
00166     //Increase update index (used for updating grid cells only once per incoming scan)
00167     currUpdateIndex += 3;
00168   }
00169 
00170   inline void updateLineBresenhami( const Eigen::Vector2i& beginMap, const Eigen::Vector2i& endMap, unsigned int max_length = UINT_MAX){
00171 
00172     int x0 = beginMap[0];
00173     int y0 = beginMap[1];
00174 
00175     //check if beam start point is inside map, cancel update if this is not the case
00176     if ((x0 < 0) || (x0 >= this->getSizeX()) || (y0 < 0) || (y0 >= this->getSizeY())) {
00177       return;
00178     }
00179 
00180     int x1 = endMap[0];
00181     int y1 = endMap[1];
00182 
00183     //std::cout << " x: "<< x1 << " y: " << y1 << " length: " << length << "     ";
00184 
00185     //check if beam end point is inside map, cancel update if this is not the case
00186     if ((x1 < 0) || (x1 >= this->getSizeX()) || (y1 < 0) || (y1 >= this->getSizeY())) {
00187       return;
00188     }
00189 
00190     int dx = x1 - x0;
00191     int dy = y1 - y0;
00192 
00193     unsigned int abs_dx = abs(dx);
00194     unsigned int abs_dy = abs(dy);
00195 
00196     int offset_dx = util::sign(dx);
00197     int offset_dy = util::sign(dy) * this->sizeX;
00198 
00199     unsigned int startOffset = beginMap.y() * this->sizeX + beginMap.x();
00200 
00201     //if x is dominant
00202     if(abs_dx >= abs_dy){
00203       int error_y = abs_dx / 2;
00204       bresenham2D(abs_dx, abs_dy, error_y, offset_dx, offset_dy, startOffset);
00205     }else{
00206       //otherwise y is dominant
00207       int error_x = abs_dy / 2;
00208       bresenham2D(abs_dy, abs_dx, error_x, offset_dy, offset_dx, startOffset);
00209     }
00210 
00211     unsigned int endOffset = endMap.y() * this->sizeX + endMap.x();
00212     this->bresenhamCellOcc(endOffset);
00213 
00214   }
00215 
00216   inline void bresenhamCellFree(unsigned int offset)
00217   {
00218     ConcreteCellType& cell (this->getCell(offset));
00219 
00220     if (cell.updateIndex < currMarkFreeIndex) {
00221       concreteGridFunctions.updateSetFree(cell);
00222       cell.updateIndex = currMarkFreeIndex;
00223     }
00224   }
00225 
00226   inline void bresenhamCellOcc(unsigned int offset)
00227   {
00228     ConcreteCellType& cell (this->getCell(offset));
00229 
00230     if (cell.updateIndex < currMarkOccIndex) {
00231 
00232       //if this cell has been updated as free in the current iteration, revert this
00233       if (cell.updateIndex == currMarkFreeIndex) {
00234         concreteGridFunctions.updateUnsetFree(cell);
00235       }
00236 
00237       concreteGridFunctions.updateSetOccupied(cell);
00238       //std::cout << " setOcc " << "\n";
00239       cell.updateIndex = currMarkOccIndex;
00240     }
00241   }
00242 
00243   inline void bresenham2D( unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset){
00244 
00245     this->bresenhamCellFree(offset);
00246 
00247     unsigned int end = abs_da-1;
00248 
00249     for(unsigned int i = 0; i < end; ++i){
00250       offset += offset_a;
00251       error_b += abs_db;
00252 
00253       if((unsigned int)error_b >= abs_da){
00254         offset += offset_b;
00255         error_b -= abs_da;
00256       }
00257 
00258       this->bresenhamCellFree(offset);
00259     }
00260   }
00261 
00262 protected:
00263 
00264   ConcreteGridFunctions concreteGridFunctions;
00265   int currUpdateIndex;
00266   int currMarkOccIndex;
00267   int currMarkFreeIndex;
00268 };
00269 
00270 
00271 }
00272 
00273 #endif


hector_mapping
Author(s): Stefan Kohlbrecher
autogenerated on Wed Aug 26 2015 11:45:03