probabilistic_grasp_planner: recognition_probability

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 // Author(s): Peter Brook
00036 
00037 #include <boost/foreach.hpp>
00038 
00039 #include "probabilistic_grasp_planner/recognition_probability_computer.h"
00040 #include "probabilistic_grasp_planner/probabilistic_planner_tools.h"
00041 
00042 namespace probabilistic_grasp_planner {
00043 
00044 void TopHitProbabilityComputer::computeRepresentationProbabilities(std::vector<ObjectRepresentation> &representations)
00045 {
00046   if (representations.empty())
00047   {
00048     ROS_ERROR("Cannot prepare probabilities list for empty representation list");
00049     return;
00050   }
00051 
00052   bool top_found = false;
00053   BOOST_FOREACH(ObjectRepresentation &representation, representations)
00054   {
00055     if (!top_found)
00056     {
00057       representation.probability = 1.0;
00058       top_found = true;
00059     } else 
00060     {
00061       representation.probability = 0.0;
00062     }
00063   }
00064 }
00065 
00069 double CompositeProbabilityComputer::getProbabilityForRecognitionScore(const double &score)
00070 {
00071   const double mean_correct = 0.002335; const double std_correct = 0.000622;
00072   const double mean_incorrect = 0.003524; const double std_incorrect = 0.000769;
00073   double p_correct = std::exp(-0.5*std::pow(score-mean_correct,2)/std::pow(std_correct,2));
00074   double p_incorrect = std::exp(-0.5*std::pow(score-mean_incorrect,2)/std::pow(std_incorrect,2));
00075   
00076   if (p_incorrect + p_correct < 1e-10) return 0.;
00077   return 0.8 * p_correct / (p_incorrect+p_correct);
00078 }
00079 
00090 void CompositeProbabilityComputer::computeRepresentationProbabilities(std::vector<ObjectRepresentation> &representations)
00091 {
00092   if (representations.empty())
00093   {
00094     ROS_ERROR("Cannot prepare probabilities list for empty representation list");
00095     return;
00096   }
00097 
00098   double best_recognition_probability = 0.0;
00099   double sum_of_probabilities = 0.0;
00100   BOOST_FOREACH(const ObjectRepresentation &representation, representations)
00101   {
00102     if (!representation.object.potential_models.empty())
00103     {
00104       double prob = getProbabilityForRecognitionScore(representation.object.potential_models[0].confidence);
00105       sum_of_probabilities += prob;
00106       best_recognition_probability = std::max(best_recognition_probability, prob);
00107     }
00108   }
00109 
00110   BOOST_FOREACH(ObjectRepresentation &representation, representations)
00111   {
00112     double probability;
00113     if (db_only_)
00114     {
00115       if (!representation.object.potential_models.empty())
00116       {
00117                 if (sum_of_probabilities > 1e-10){
00118                   probability = getProbabilityForRecognitionScore(
00119                                   representation.object.potential_models[0].confidence) / sum_of_probabilities;
00120                 }
00121                 else probability = 0.;
00122                 representation.probability = probability;
00123       }
00124       else
00125       {
00126         representation.probability = 0.0;
00127       }
00128     }
00129     else
00130     {
00131       if (!representation.object.potential_models.empty())
00132       {
00133                 if (sum_of_probabilities > 1e-10){
00134                   probability = best_recognition_probability * getProbabilityForRecognitionScore(
00135                                    representation.object.potential_models[0].confidence) / sum_of_probabilities;
00136                 }
00137                 else probability = 0.;
00138         representation.probability = probability;
00139       }
00140       else
00141       {
00143         representation.probability = 1.0-best_recognition_probability;
00144       }
00145     }
00146   }
00147 }
00148 
00155 double InverseCurveRecognitionProbabilityComputer::getProbabilityForRecognitionScore(const double &score)
00156 {
00157   double q = 1.0/curvature_;
00158   double val = (1.0/(q+(score/recognition_threshold_ * (curvature_ - q))) - q)*(1.0/(curvature_-q));
00159   return std::max(0.0,val);
00160 }
00161 
00162 void LearnedProbabilityComputer::computeRepresentationProbabilities(std::vector<ObjectRepresentation> &representations)
00163 {
00164   int this_case = CLUSTER_ONLY;
00165   //Figure out which case we are dealing with
00166   bool cluster_found = false;
00167   int num_db_objects = 0;
00168   double best_db_score = 100; //Need to figure out which db model is the best in case they aren't sorted
00169   size_t best_db_idx = 0;
00170 
00171   size_t idx = 0;
00172 
00173   ObjectRepresentation* cluster_ptr = NULL;
00174   ObjectRepresentation* best_db_model = NULL;
00175   std::vector<ObjectRepresentation*> other_db_models;
00176   BOOST_FOREACH(ObjectRepresentation &representation,representations)
00177   {
00178     if (!representation.object.potential_models.empty())
00179     {
00180       ++num_db_objects;
00181       if (representation.object.potential_models[0].confidence < best_db_score)
00182       {
00183         best_db_score = representation.object.potential_models[0].confidence;
00184         best_db_idx = idx;
00185       }
00186     }
00187     else
00188     {
00189       cluster_found = true;
00190       cluster_ptr = &representation;
00191     }
00192     ++idx;
00193   }
00194 
00195   best_db_model = &representations[best_db_idx];
00196   for (size_t i=0; i < representations.size(); ++i)
00197   {
00198     if (i != best_db_idx && !representations[i].object.potential_models.empty())
00199     {
00200       other_db_models.push_back(&representations[i]);
00201     }
00202   }
00203 
00204   if (cluster_found)
00205   {
00206     if (num_db_objects == 0)
00207     {
00208       this_case = CLUSTER_ONLY;
00209     }
00210     else if (num_db_objects == 1)
00211     {
00212       this_case = CLUSTER_DB;
00213     }
00214     else
00215     {
00216       this_case = CLUSTER_DB_MULTIPLE;
00217     }
00218   }
00219   else
00220   {
00221     if (num_db_objects == 0)
00222     {
00223       ROS_ERROR("No objects given");
00224       return;
00225     }
00226     else if (num_db_objects == 1)
00227     {
00228       this_case = DB_ONLY;
00229     }
00230     else
00231     {
00232       this_case = DB_MULTIPLE;
00233     }
00234   }
00235 
00236   //Assign probabilities accordingly
00237   switch (this_case)
00238   {
00239     case CLUSTER_ONLY:
00240     {
00241       cluster_ptr->probability = 1.0;
00242       break;
00243     }
00244     case CLUSTER_DB:
00245     {
00246       cluster_ptr->probability = 0.333;
00247       best_db_model->probability = 0.667;
00248       break;
00249     }
00250     case CLUSTER_DB_MULTIPLE:
00251     {
00252       cluster_ptr->probability = 0.25;
00253       best_db_model->probability = 0.50;
00254       int num_other_models = other_db_models.size();
00255       BOOST_FOREACH(ObjectRepresentation* representation,other_db_models)
00256       {
00257         representation->probability = 0.25/num_other_models;
00258       }
00259       break;
00260     }
00261     case DB_ONLY:
00262     {
00263       best_db_model->probability = 1.0;
00264       break;
00265     }
00266     case DB_MULTIPLE:
00267     {
00268       best_db_model->probability = 0.667;
00269       int num_other_models = other_db_models.size();
00270       BOOST_FOREACH(ObjectRepresentation* representation,other_db_models)
00271       {
00272         representation->probability = 0.333/num_other_models;
00273       }
00274       break;
00275     }
00276   }
00277 }
00278 
00279 }
recognition_probability_computer.cpp
