text: /***************************************************************************
   *   Software License Agreement (BSD License)                              *
   *   Copyright (C) 2017 by Florian Beck <florian.beck@tuwien.ac.at>        *
   *                                                                         *
   *   Redistribution and use in source and binary forms, with or without    *
   *   modification, are permitted provided that the following conditions    *
   *   are met:                                                              *
   *                                                                         *
   *   1. Redistributions of source code must retain the above copyright     *
   *      notice, this list of conditions and the following disclaimer.      *
   *   2. 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.                                                      *
   *   3. Neither the name of the copyright holder 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 OR CONTRIBUTORS 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 HEATMAP_SYSTEM_MODEL_INV_H
  #define HEATMAP_SYSTEM_MODEL_INV_H
  
  #include "common.h"
  #include "meas_model.h"
  #include <grid_map_ros/grid_map_ros.hpp>
  #include <random>
  
  class HeatMapSystemModelInv : public MeasModel
  {
  public:
    HeatMapSystemModelInv(double sigma_theta, int angle_partitions, grid_map::GridMap& heat_map, grid_map::Matrix* layer);
  
    double getProbability(const Eigen::Ref<const Eigen::VectorXd>& curr_state, const Eigen::Ref<const Eigen::VectorXd>& meas, const Eigen::Ref<const Eigen::MatrixXd>& meas_cov = Eigen::MatrixXd(), double dt = 0) override;
  
  private:
    std::mt19937 generator_;                                /// random number generator
    std::normal_distribution<double> normal_distribution_;  /// normal distribution for generic use
    double sigma_theta_;
    grid_map::GridMap heat_map_;
    grid_map::Matrix* layer_;
    int angle_partitions_;
    double probability_;
    std::vector<double> weight_factor_;
    double sum_for_weight_factor_;
  };
  
  #endif  // HEATMAP_SYSTEM_MODEL_INV_H
  