00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <boost/foreach.hpp>
00038
00039 #include "probabilistic_grasp_planner/distribution_evaluator.h"
00040 #include "probabilistic_grasp_planner/probabilistic_planner_tools.h"
00041 #include "probabilistic_grasp_planner/grasp_success_probability_computer.h"
00042 #include "probabilistic_grasp_planner/grasp_regression_evaluator.h"
00043
00044 namespace probabilistic_grasp_planner {
00045
00046 double GraspRegressionEvaluator::estimateProbability(const GraspWithMetadata &grasp) const
00047 {
00048 NormalDistributionEvaluator evaluator(position_bandwidth_, orientation_bandwidth_);
00049 evaluator.reset_normalization_term();
00050
00051 double probability = 0.0;
00052
00053 const GraspWithMetadata* closest_grasp = NULL;
00054 double closest_dist = 0.0;
00055
00056 double cartesian_dist, rotation_dist;
00057
00058 BOOST_FOREACH(const GraspWithMetadata &grasp_for_object, grasps_)
00059 {
00060
00061 if (closest_grasp==NULL)
00062 {
00063 closest_grasp = &grasp_for_object;
00064
00065 grasp.getDistance(grasp_for_object, cartesian_dist, rotation_dist);
00066
00067 closest_dist = sqrt(pow(position_bandwidth_*cartesian_dist,2) +
00068 pow(orientation_bandwidth_*rotation_dist,2));
00069 }
00070
00071 grasp.getDistance(grasp_for_object, cartesian_dist, rotation_dist);
00072
00073 double this_dist = sqrt(pow(position_bandwidth_*cartesian_dist,2) +
00074 pow(orientation_bandwidth_*rotation_dist,2));
00075
00076 if (this_dist < closest_dist)
00077 {
00078 closest_grasp = &grasp_for_object;
00079 closest_dist = this_dist;
00080 }
00081
00082
00083 double p_dens = evaluator.evaluate(grasp_for_object, grasp);
00084 double grasp_probability = simple_computer_->getProbability(grasp_for_object);
00085 probability += p_dens*grasp_probability;
00086 }
00087
00088
00089 if (evaluator.get_normalization_term() != 0.0)
00090 {
00091 probability /= evaluator.get_normalization_term();
00092
00093 double p_dens = evaluator.evaluate(*closest_grasp, grasp);
00094 double grasp_probability = simple_computer_->getProbability(*closest_grasp);
00095 double probability_from_closest_grasp = p_dens*grasp_probability;
00096
00097 return std::min(probability, probability_from_closest_grasp);
00098 }
00099 else {
00100 return 0.0;
00101 }
00102 }
00103
00104 }