19 #include <ISM/utility/GeometryHelper.hpp> 34 PointPtr absPosition = GeometryHelper::getSourcePoint(referencePose,
35 specifier->refToObjectQuat,
37 PosePtr absPose = GeometryHelper::getSourcePose(referencePose,
39 specifier->refToObjectPoseQuat);
40 for(
unsigned int i = 0; i < weight; i++)
45 std::string patternName,
double numberOfSpecifiers)
58 ISM::VoteSpecifierPtr specifier,
59 ISM::PosePtr &referencePose,
61 unsigned int specifiersSize,
66 PointPtr absPosition = GeometryHelper::getSourcePoint(referencePose,
67 specifier->refToObjectQuat,
69 PosePtr absPose = GeometryHelper::getSourcePose(referencePose,
71 specifier->refToObjectPoseQuat);
72 unsigned int nextLevel = level + 1;
73 if(GeometryHelper::poseEqual(referencePose, absPose))
75 ROS_DEBUG(
"Object %s is identical to the reference of the ism in which it is object", objectType.c_str());
76 unsigned int numberOfHypotheses = specifiersSize * weight;
96 unsigned int posesSize = it->second.size();
98 unsigned int threshold = round(numberOfSpecifiers * specifiersSize);
99 for (
unsigned int i = 0; i < threshold; ++i)
101 unsigned int index = rand() % posesSize;
116 std::string patternName,
121 for (
IsmObject object : objectsInPattern)
123 std::vector<ISM::VoteSpecifierPtr> specifiers =
votes_.at(patternName).at(
object.first).at(
object.second);
126 for (
unsigned int i = 0; i < specifiers.size(); i++)
134 if (skipOtherSpecifiers){
135 ROS_DEBUG_STREAM(
"Skipping further votes for object " <<
"(" <<
object.first <<
"," <<
object.second <<
")" <<
".");
144 for (
unsigned int i = 0; i < specifiers.size(); i++)
147 referencePose, weight);
ISM::PatternToObjectToVoteMap votes_
void addPointToPointCloud(ISM::PosePtr pose_to_add, std::string type, std::string identifier)
RecognizerPredictionISM::addPointToPointCloud Adds an ISM Pose with object type and identifier as att...
bool referenceObjectProcessing(std::string objectType, ISM::VoteSpecifierPtr specifier, ISM::PosePtr &referencePose, unsigned int weight, unsigned int specifiersSize, unsigned int level)
referenceObjectProcessing Processes a reference object. Processes the following steps.
AttributedPointCloud attributed_point_cloud_
std::set< IsmObject > IsmObjectSet
~PaperPredictionNormalized()
void createAttributedPointCloud(ISM::PosePtr reference_pose_ptr, double percentage_of_records_for_prediction)
#define ROS_DEBUG_STREAM(args)
std::pair< std::string, std::string > IsmObject
void calculateRecursiveUnfoundPoses(ISM::PosePtr &referencePose, std::string patternName, unsigned int weight, unsigned int level)
bool isReferenceObject(IsmObject object)
void createAttributedPointCloudFromMap(double numberOfSpecifiers)
PaperPredictionNormalized(std::string dbfileName)
std::map< IsmObject, std::vector< ISM::PosePtr > > objectPoseMap
AttributedPointCloud predictUnfoundPoses(ISM::PosePtr &referencePose, std::string patternName, double percentage_of_records_for_prediction)
RecognizerPredictionISM::calcUnfoundPoses Processes the following steps for each object in the object...
IsmObjectSet getObjectTypesAndIdsBelongingToPattern(std::string type)
void nonReferenceObjectProcessing(IsmObject object, ISM::VoteSpecifierPtr specifier, ISM::PosePtr &referencePose, unsigned int weight)
asr_msgs::AsrAttributedPointCloud AttributedPointCloud