/*
Adds new toy coordinates to knownToys vector
Updates the coordinates if toy already known
Input: float x == x
float y == y
float z == toy_type (wooden == 1, small = 0)
*/
void update_toys(float x, float y, float z)
{
bool found = false;
float dist = 9999;
//Check if the new toy coordinate is inside the radius of any of the known toys
for (float2DVector::size_type i = 0; i < knownToys.size(); i++) {
dist = sqrt(pow(x - knownToys[i][0],2) + pow(y - knownToys[i][1],2));
if (dist < SAME_TOY_MAX_R) {
//inside toy radius, update the toy coordinate
knownToys[i][0] = (knownToys[i][0] + x) / 2;
knownToys[i][1] = (knownToys[i][1] + y) / 2;
found = true;
break;
}
}
if (!found) {
ROS_INFO("A new toy!");
vector<float> pos;
pos.push_back(x);
pos.push_back(y);
pos.push_back(z);
knownToys.push_back(pos);
}
}
/*
Filters input coordinates
Called by subscriber to toy coordinates published by image processing
Calls update_toys when trust treshold for coordinate is reached
Obstacles are filtered out
Input: input_coord->x: float x
input_coord->y: float y
input_coord->z: float toy_type (wooden == 1, small = 0)
*/
void checkInput(const geometry_msgs::Point::ConstPtr& input_coord) {
bool found = false;
float dist = 9999; //initialise distance between new and old toy coordinates
float x = input_coord->x;
float y = input_coord->y;
//Map area points for ignoring toy coordinates in obstacles:
//Outside
float map_x_begin = -1.00;
float map_x_end = 2.63;
float map_y_begin = -1.62;
float map_y_end = 1.35;
//Box
float box_x_begin = 0.44;
float box_x_end = 1.10;
float box_y_begin = 0.24;
float box_y_end = 0.80;
//Pillar, currently not needed
float pillar_x_begin = 999.0;
float pillar_x_end = 999.0;
float pillar_y_begin = 999.0;
float pillar_y_end = 999.0;
//Middle wall
float wall_x_begin = 0.768;
float wall_x_end = 1.215;
float wall_y_begin = -1.814;
float wall_y_end = -0.307;
//White basket
float white_x_begin = -0.379;
float white_x_end = 0.004;
float white_y_begin = -1.772;
float white_y_end = -1.539;
//Blue basket, currently not needed
float blue_x_begin = 999.0;
float blue_x_end = 999.0;
float blue_y_begin = 999.0;
float blue_y_end = 999.0;
// NOT (outside the map OR in box OR in pillar OR in middle wall OR in white box OR in blue box)
if (!(x < map_x_begin || x > map_x_end || y < map_y_begin || y > map_y_end
|| (x > box_x_begin && x < box_x_end && y > box_y_begin && y < box_y_end)
|| (x > pillar_x_begin && x < pillar_x_end && y > pillar_y_begin && y < pillar_y_end)
|| (x > wall_x_begin && x < wall_x_end && y > wall_y_begin && y < wall_y_end)
|| (x > white_x_begin && x < white_x_end && y > white_y_begin && y < white_y_end)
|| (x > blue_x_begin && x < blue_x_end && y > blue_y_begin && y < blue_y_end) ))
{
//Check if the new coordinate is inside the radius of any of the filtering table coordinates
for (float2DVector::size_type i = 0; i < filteringTable.size(); i++) {
dist = sqrt(pow(x - filteringTable[i][0],2) + pow(y - filteringTable[i][1],2));
if (dist < SAME_TOY_MAX_R) {
//Inside coordinate radius, update the filtering table coordinate
filteringTable[i][0] = (filteringTable[i][0] + x) / 2;
filteringTable[i][1] = (filteringTable[i][1] + y) / 2;
filteringTable[i][3] = filteringTable[i][3] + 1;
if (filteringTable[i][3] >= INPUT_TRUST_TRESHOLD) {
//coordinate has reached it input trust treshold, update toys
update_toys(filteringTable[i][0], filteringTable[i][1],filteringTable[i][2]);
}
found = true;
break;
}
}
if (!found) {
//no matching toy, add a new toy to filtering table
vector<float> pos;
pos.push_back(x);
pos.push_back(y);
pos.push_back(input_coord->z);
pos.push_back(1);
filteringTable.push_back(pos);
//If too much coordinates in filtering table, clear first ones
while (filteringTable.size() > 50) {
filteringTable.erase(filteringTable.begin());
}
}
}
}
int HolisticFeatureExtractor::extractFeatures(const LTKTraceGroup& traceGroup, const LTKCaptureDevice& captureDevice, const LTKScreenContext& screenContext, LTKPreprocessorInterface *ltkShapeRecPtr, float2DVector& featureVector)
{
LOG( LTKLogger::LTK_LOGLEVEL_DEBUG) <<
"Entered HolisticFeatureExtractor::extractFeatures" << endl;
LTKTrace preprocessedTrace; // a trace of the trace group
LTKTrace preprocessedTrace2; // a trace of the trace group
LTKTrace preprocessedTrace3; // a trace of the trace group
LTKTraceGroup preprocessedTraceGroup;
LTKTraceGroup preprocessedTraceGroup2;
LTKTraceGroup preprocessedTraceGroup3;
int traceIndex; // variable to loop over all traces of the trace group
// preprocessing the traceGroup in 3 ways to extract 3 kinds of features
preprocess(traceGroup, preprocessedTraceGroup, captureDevice, screenContext, ltkShapeRecPtr);
preprocess2(traceGroup, preprocessedTraceGroup2, captureDevice, screenContext, ltkShapeRecPtr);
preprocess3(traceGroup, preprocessedTraceGroup3, captureDevice, screenContext, ltkShapeRecPtr);
// extracting the feature vector
for(traceIndex = 0; traceIndex < traceGroup.getNumTraces(); ++traceIndex)
{
preprocessedTrace = preprocessedTraceGroup.getTraceAt(traceIndex);
preprocessedTrace2 = preprocessedTraceGroup2.getTraceAt(traceIndex);
preprocessedTrace3 = preprocessedTraceGroup3.getTraceAt(traceIndex);
// calling the compute features methods
floatVector features;
// calculating features with preprocessedTrace2
features.push_back(computeEER(preprocessedTrace));
features.push_back(computeOrientation(preprocessedTrace));
// calculating features with preprocessedTrace2
float TCL = computeTCL(preprocessedTrace2);
TCL /= calculateBBoxPerimeter(screenContext); // normalizing using the perimeter
features.push_back(TCL);
features.push_back(computePII(preprocessedTrace2, screenContext));
// calculating features with preprocessedTrace3
float swAng = computeSweptAngle(preprocessedTrace3);
// normalizing the swept angle with swAngNormFactor x 360 degrees
swAng /= (2*PI*m_swAngNormFactor);
features.push_back(swAng);
featureVector.push_back(features);
}//traceIndex
LOG( LTKLogger::LTK_LOGLEVEL_DEBUG) <<
"Exiting HolisticFeatureExtractor::extractFeatures" << endl;
return SUCCESS;
}