probability_distribution.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #ifndef PROBABILITY_DISTRIBUTION_H
00036 #define PROBABILITY_DISTRIBUTION_H
00037 
00038 namespace bayesian_grasp_planner {
00039 
00040 class ProbabilityDistribution
00041 {
00042 public:
00043   virtual double evaluate(double value) const = 0;
00044 };
00045 
00046 class IdentityProbabilityDistribution : public ProbabilityDistribution
00047 {
00048 public:
00049   virtual double evaluate(double value) const {return value;}
00050 };
00051  
00052 
00054 class GaussianProbabilityDistribution : public ProbabilityDistribution
00055 {
00056 protected:
00057   double mean_;    //mean of the shifted distribution
00058   double std_dev_; //std of the shifted distribution 
00059   double min_;     //raw value min for shifting
00060   double max_;     //raw value max for shifting
00061   bool   flip_;    //whether the raw value should be flipped so that higher is better
00062 public:
00063   GaussianProbabilityDistribution(double mean, double std_dev, double min, double max, bool flip) : 
00064                       mean_(mean),std_dev_(std_dev),min_(min),max_(max),flip_(flip){}
00065 
00066   virtual double evaluate(double value) const 
00067   {
00068         //flip and shift the raw value
00069         double clipped_value = value;
00070         if (value > max_+1e-6)
00071         {
00072           clipped_value = max_;
00073           //ROS_WARN("clipping value %.3f to max of %.3f", value, max_);
00074         }
00075         if (value < min_-1e-6) 
00076         {
00077           clipped_value = min_;
00078           //ROS_WARN("clipping value %.3f to min of %.3f", value, min_);
00079         }
00080         double shifted_value;
00081         if (flip_)
00082         {
00083           shifted_value = (max_ - clipped_value) / (max_ - min_);
00084           //printf("flipping, max_=%.3f, min_=%.3f ", max_, min_);
00085         }
00086         else
00087         {
00088           shifted_value = (clipped_value - min_) / (max_ - min_);
00089           //printf("not flipping, max_=%.3f, min_=%.3f ", max_, min_);
00090         }
00091         //printf("mean: %.3f, std_dev: %.3f, value: %.3f, shifted_value: %.3f, prob: %.3f\n", mean_, std_dev_, value, shifted_value, 1/sqrt(2*3.14159*pow(std_dev_, 2)) * exp(-.5*pow(shifted_value - mean_, 2)/pow(std_dev_, 2)));
00092         return 1/sqrt(2*3.14159*pow(std_dev_, 2)) * exp(-.5*pow(shifted_value - mean_, 2)/pow(std_dev_, 2));
00093   }
00094 };
00095 
00096 
00098 class BimodalGaussianProbabilityDistribution : public ProbabilityDistribution
00099 {
00100 protected:
00101   double mean_;      //mean of the shifted distribution (for all but zero values)
00102   double std_dev_;   //std of the shifted distribution (for all but zero values)
00103   double zero_prob_; //probability of getting a zero value
00104 public:
00105   BimodalGaussianProbabilityDistribution(double mean, double std_dev, double zero_prob) : 
00106         mean_(mean),std_dev_(std_dev),zero_prob_(zero_prob){}
00107 
00108   virtual double evaluate(double value) const 
00109   {
00110         if (value == 0) return zero_prob_;
00111         return (1-zero_prob_)*(1/sqrt(2*3.14159*pow(std_dev_, 2)) * exp(-.5*pow(value - mean_, 2)/pow(std_dev_, 2)));
00112   }
00113 };
00114 
00115 
00116 }
00117 
00118 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


bayesian_grasp_planner
Author(s): Kaijen Hsiao and Matei Ciocarlie
autogenerated on Fri Jan 25 2013 15:02:36