$search
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