/*
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());
            }
        }
    }
    
}
Esempio n. 3
0
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;

}