GridMapLogOdds.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 __GridMapLogOdds_h_
00030 #define __GridMapLogOdds_h_
00031 
00032 #include <cmath>
00033 
00037 class LogOddsCell
00038 {
00039 public:
00040 
00041   /*
00042   void setOccupied()
00043   {
00044     logOddsVal += 0.7f;
00045   };
00046 
00047   void setFree()
00048   {
00049     logOddsVal -= 0.4f;
00050   };
00051   */
00052 
00053 
00058   void set(float val)
00059   {
00060     logOddsVal = val;
00061   }
00062 
00067   float getValue() const
00068   {
00069     return logOddsVal;
00070   }
00071 
00076   bool isOccupied() const
00077   {
00078     return logOddsVal > 0.0f;
00079   }
00080 
00081   bool isFree() const
00082   {
00083     return logOddsVal < 0.0f;
00084   }
00085 
00089   void resetGridCell()
00090   {
00091     logOddsVal = 0.0f;
00092     updateIndex = -1;
00093   }
00094 
00095   //protected:
00096 
00097 public:
00098 
00099   float logOddsVal; 
00100   int updateIndex;
00101 
00102 
00103 };
00104 
00108 class GridMapLogOddsFunctions
00109 {
00110 public:
00111 
00115   GridMapLogOddsFunctions()
00116   {
00117     this->setUpdateFreeFactor(0.4f);
00118     this->setUpdateOccupiedFactor(0.6f);
00119     /*
00120     //float probOccupied = 0.6f;
00121     float probOccupied = 0.9f;
00122     float oddsOccupied = probOccupied / (1.0f - probOccupied);
00123     logOddsOccupied = log(oddsOccupied);
00124 
00125     float probFree = 0.4f;
00126     float oddsFree = probFree / (1.0f - probFree);
00127     logOddsFree = log(oddsFree);
00128     */
00129   }
00130 
00135   void updateSetOccupied(LogOddsCell& cell) const
00136   {
00137     if (cell.logOddsVal < 50.0f){
00138       cell.logOddsVal += logOddsOccupied;
00139     }
00140   }
00141 
00146   void updateSetFree(LogOddsCell& cell) const
00147   {
00148 
00149     cell.logOddsVal += logOddsFree;
00150 
00151   }
00152 
00153   void updateUnsetFree(LogOddsCell& cell) const
00154   {
00155     cell.logOddsVal -= logOddsFree;
00156   }
00157 
00163   float getGridProbability(const LogOddsCell& cell) const
00164   {
00165     float odds = exp(cell.logOddsVal);
00166     return odds / (odds + 1.0f);
00167 
00168     /*
00169     float val = cell.logOddsVal;
00170 
00171     //prevent #IND when doing exp(large number).
00172     if (val > 50.0f) {
00173       return 1.0f;
00174     } else {
00175       float odds = exp(val);
00176       return odds / (odds + 1.0f);
00177     }
00178     */
00179     //return 0.5f;
00180   }
00181 
00182   //void getGridValueMap( const LogOddsCell& cell) const{};
00183   //void isOccupied(LogOddsCell& cell) {};
00184 
00185   //void resetGridCell() {};
00186 
00187   void setUpdateFreeFactor(float factor)
00188   {
00189     logOddsFree = probToLogOdds(factor);
00190   }
00191 
00192   void setUpdateOccupiedFactor(float factor)
00193   {
00194     logOddsOccupied = probToLogOdds(factor);
00195   }
00196 
00197 protected:
00198 
00199   float probToLogOdds(float prob)
00200   {
00201     float odds = prob / (1.0f - prob);
00202     return log(odds);
00203   }
00204 
00205   float logOddsOccupied; 
00206   float logOddsFree;     
00207 
00208 
00209 };
00210 
00211 
00212 #endif


hector_mapping
Author(s): Stefan Kohlbrecher
autogenerated on Thu Jun 6 2019 20:12:30