gistfile1.txt

/* The HoughLinesP() function returns multple lines which represent the same wall. Therefore
 * we want to filter these lines and add the remaining lines to a new vector. We compare the first
 * line of a vector with all the other lines in two ways: if r and theta should are the same
 * and one if the extremes of the shortest line are in between the two extremes of the largest
 * line. If so, the line is the same and the shortest line will not be added to the new vector.
 * If not so, the line will be added to the new vector and will also be checked with all the other
 * remaining lines in the old vector.
 * */

//Filter the lines, add create a new vector which contains the remaining lines
new_lines filter_lines(old_lines &line, new_lines &new_line)
{
    double ti, tk;
    int x0i, y0i, x1i, y1i, ri, x0k, y0k, x1k, y1k, rk ;

    //Empty the new_lines class
    new_line.x0.erase(new_line.x0.begin(), new_line.x0.end());
    new_line.y0.erase(new_line.y0.begin(), new_line.y0.end());
    new_line.x1.erase(new_line.x1.begin(), new_line.x1.end());
    new_line.y1.erase(new_line.y1.begin(), new_line.y1.end());
    new_line.r.erase(new_line.r.begin(), new_line.r.end());
    new_line.t.erase(new_line.t.begin(), new_line.t.end());

    //Compare line i with line k, which is a line added to the new_line vector
    for ( unsigned i = 0 ; i < line.r.size(); i++)
    {
        //If the new_line vector is still empty, first add a line to this vector
        if (new_line.r.size() == 0 )
        {
            add_line(line, new_line, i);
        }

        else
        {
            x0i = line.x0[i];
            x1i = line.x1[i];
            y0i = line.y0[i];
            y1i = line.y1[i];
            ri  = line.r[i];
            ti  = line.t[i];

            //Loop over the 'line' vector and compare the lines with the i-th line of the new vector 'new_line'
            for ( unsigned k = 0 ; k < new_line.r.size(); k++ )
            {
                x0k = new_line.x0[k];
                x1k = new_line.x1[k];
                y0k = new_line.y0[k];
                y1k = new_line.y1[k];
                rk  = new_line.r[k];
                tk  = new_line.t[k];

                //Check if the r and theta of both lines are the same (within a tolerance)
                if ( ((abs(ri - rk) <= TOL_R) &&  (fabs(ti - tk) <= TOL_T)) || (abs(ri - rk) <= TOL_R && fabs(fabs(ti - tk) - 2*M_PI) <= TOL_T))
                {
                    //Check if k is smaller then i
                    if (length_new_line(new_line, k) < length_line(line, i))
                    {
                        //Check if one of the extremes of line k is in between the extremes of line i
                        //If yes, overwrite line k with line i
                        //If not, add line i to the new_line vector if k was the last line in the new vector
                        if(   ( (x0k > x0i) && (x0k < x1i) ) || ( (x1k > x0i) && (x1k < x1i) )
                           || ( (x0k > x1i) && (x0k < x0i) ) || ( (x1k > x1i) && (x1k < x0i) )
                           || ( (y0k > y0i) && (y0k < y1i) ) || ( (y1k > y0i) && (y1k < y1i) )
                           || ( (y0k > y1i) && (y0k < y0i) ) || ( (y1k > y1i) && (y1k < y0i) ) )
                        {
                            overwrite_line(line, new_line, k, i);
                            break;
                        }

                        else
                        {
                            if ( k == new_line.r.size()-1 )
                            {
                                add_line(line, new_line, i);
                                break;
                            }
                        }
                    }

                    //Check if i is smaller then k
                    else if (length_new_line(new_line, k) >= length_line(line, i))
                    {
                        //Check if one of the extremes of i is in between the extremes of line k
                        //If yes, do nothing (lines are the same, but the longest line is already added to the new vector
                        //If not, add line i to the new vector if k was the last line in the new vector
                        if (    ( (x0i > x0k) && (x0i < x1k) ) || ( (x1i > x0k) && (x1i < x1k) )
                             || ( (x0i > x1k) && (x0i < x0k) ) || ( (x1i > x1k) && (x1i < x0k) )
                             || ( (y0i > y0k) && (y0i < y1k) ) || ( (y1i > y0k) && (y1i < y1k) )
                             || ( (y0i > y1k) && (y0i < y0k) ) || ( (y1i > y1k) && (y1i < y0k) ) )
                        {
                            break;
                        }

                        else
                        {
                            if ( k == new_line.r.size()-1 )
                            {
                                add_line(line, new_line, i);
                                break;
                            }
                        }
                    }
                }

                //If r and t are different, add the line to the new vector if k is the last line in the new vector
                else
                {
                    if ( k == new_line.r.size()-1 )
                    {
                        add_line(line, new_line, i);
                        break;
                    }
                }
            }
        }
    }

    return new_line;
}