GridMapLogOdds.h

Go to the documentation of this file.

//=================================================================================================
// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Simulation, Systems Optimization and Robotics
//       group, TU Darmstadt nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================

#ifndef __GridMapLogOdds_h_
#define __GridMapLogOdds_h_

#include <cmath>

class LogOddsCell
{
public:

  /*
  void setOccupied()
  {
    logOddsVal += 0.7f;
  };

  void setFree()
  {
    logOddsVal -= 0.4f;
  };
  */


  void set(float val)
  {
    logOddsVal = val;
  }

  float getValue() const
  {
    return logOddsVal;
  }

  bool isOccupied() const
  {
    return logOddsVal > 0.0f;
  }

  bool isFree() const
  {
    return logOddsVal < 0.0f;
  }

  void resetGridCell()
  {
    logOddsVal = 0.0f;
    updateIndex = -1;
  }

  //protected:

public:

  float logOddsVal; 
  int updateIndex;


};

class GridMapLogOddsFunctions
{
public:

  GridMapLogOddsFunctions()
  {
    this->setUpdateFreeFactor(0.4f);
    this->setUpdateOccupiedFactor(0.6f);
    /*
    //float probOccupied = 0.6f;
    float probOccupied = 0.9f;
    float oddsOccupied = probOccupied / (1.0f - probOccupied);
    logOddsOccupied = log(oddsOccupied);

    float probFree = 0.4f;
    float oddsFree = probFree / (1.0f - probFree);
    logOddsFree = log(oddsFree);
    */
  }

  void updateSetOccupied(LogOddsCell& cell) const
  {
    if (cell.logOddsVal < 50.0f){
      cell.logOddsVal += logOddsOccupied;
    }
  }

  void updateSetFree(LogOddsCell& cell) const
  {

    cell.logOddsVal += logOddsFree;

  }

  void updateUnsetFree(LogOddsCell& cell) const
  {
    cell.logOddsVal -= logOddsFree;
  }

  float getGridProbability(const LogOddsCell& cell) const
  {
    float odds = exp(cell.logOddsVal);
    return odds / (odds + 1.0f);

    /*
    float val = cell.logOddsVal;

    //prevent #IND when doing exp(large number).
    if (val > 50.0f) {
      return 1.0f;
    } else {
      float odds = exp(val);
      return odds / (odds + 1.0f);
    }
    */
    //return 0.5f;
  }

  //void getGridValueMap( const LogOddsCell& cell) const{};
  //void isOccupied(LogOddsCell& cell) {};

  //void resetGridCell() {};

  void setUpdateFreeFactor(float factor)
  {
    logOddsFree = probToLogOdds(factor);
  }

  void setUpdateOccupiedFactor(float factor)
  {
    logOddsOccupied = probToLogOdds(factor);
  }

protected:

  float probToLogOdds(float prob)
  {
    float odds = prob / (1.0f - prob);
    return log(odds);
  }

  float logOddsOccupied; 
  float logOddsFree;     


};


#endif