DeadEnd_detection.cpp

#include "DeadEnd.h"
#include "Config.h"

#define toDeg  180.0f/3.14159265f;
#define toRad  3.14159265f/180.0f;

using namespace std;

template <typename DerivedA>
void getSegments(const DerivedA& sections, const DerivedA& segments)
{
    segments = sections;
}

void isPerpendicular()
{

}

//template <typename DerivedA>
//void isDeadEnd(const DerivedA sections, const DerivedA segments, const DerivedA leftright, const DerivedA * deadends, int *nDE)
//{
//    *nDE = 5;
//}


void DeadEnd::isDeadEnd(Corners corners, int * DeadEndFound, float * DeadEndCoords, float * distTODE)
{
    // start of the real functions for dead end detection.
    //
    // variable declarations.
    //int nRowsin = 100;
    //int nColsin = 3;
    int nSections = 0;                          // number of sections found by wall detection.
    int nDE = 0, nDE_dummy = 0;                                // number of dead ends detected.
    int nearestDE = 0;                          // index of the nearest dead end.
    float compSegments = 1.0;                   // constant to identify segments
    float angleTol = 5.0f*toRad;                // corner perpendicularity tolerance
    float angleSetPoint = 90.0f*toRad;          // Corner detection goal (we want corners to be 90 degrees)
    float angle;                                // angle between segments
    float DElength = 0.3;                       // Dead end template parameter: minimum depth of a dead end
    float DEwidth_lb = 0.5;                     // Dead end template parameter: width lower bound
    float DEwidth_ub = 1.5;                     // Dead end template parameter: width upper bound
    float closestDE = 30;                        // distance to the nearest dead end. initialized at max sensor range.
    float distToDEThreashold = 1.0f;            // distance to dead end threashold. If a dead end is within this range, it is returned.

    MatrixXf cornersMat, segments, vect;
    MatrixXf RL(2,2), RR(2,2);                           // Rotation matrix to determine the corner direction
    VectorXf v1(2), v2(2), v3(2), v4(2);                      // dummy vectors to calculate angle between segments
    MatrixXf perp;                              // matrix containing the perpendicularity between segments
    MatrixXf leftright;                         // matrix containing turn direction information. 0 = left; 1 = right; 2 = no turn is being made;
    MatrixXf DE;                                // matrix containing the dead end corner coordinates.
    MatrixXf lleft_DE_vect(1,2), lright_DE_vect(1,2), widht_DE_vect(1,2);   // vectors containing the dead end dimension vectors.
    MatrixXf DeadEnds;                          // The wall locations of the detected dead end
    MatrixXf DEmidpoint, v_toMidPoint(1,2);                        // midpoint of the dead end;
    vect.resize(1,2);                           // dummy vector used be segment extraction.
    RL << 0.0 ,1.0,
          -1.0 ,0.0;
    RR << 0,-1,
          1,0;

    // extract segment data from CornerSections
    //int cont = 1;
    while( corners[nSections][1] != 0)
    {
        nSections++;
    }
    cornersMat.resize(nSections,3);
    for(int i=0; i < nSections; ++i)
    {
        for(int j=0; j < 3; ++j)
        {
            cornersMat(i,j) = corners[i][j];
        }
    }



    //   =====================================================================================  //
    // Segment extraction routine.
    {
    segments.resize((nSections-1),2);

    for(int i=0; i < nSections - 1; ++i)
    {
        if(cornersMat((i+1),2) == compSegments )
        {

            // valid segment found
            vect(0,0) = cornersMat(i+1,1) - cornersMat(i,1);
            vect(0,1) = cornersMat(i+1,0) - cornersMat(i,0);
            vect = vect/vect.norm();


        }
        else
        {
            vect << 0,0;
        }

        segments.row(i) = vect.row(0);
    }

    }

    //   =====================================================================================  //
    // Perpendicularity routine.

    perp.resize(segments.rows()-1, 1);                          // set the size of the perpendicularity matrix
    leftright.resize(segments.rows()-1, 1);                     // set the size of the leftright matrix

    for (int i = 0; i < segments.rows()-1; ++i)
    {
        if(segments.row(i+1).norm() > 0.01f)
        {
            // valid segment found
            v1 = segments.row(i);
            v2 = segments.row(i+1);
            angle = acos(v1.dot(v2));

            if( abs(abs(angle) - angleSetPoint) < angleTol )
            {
                // The angle between segment i and i+1 is perpendicular within 5 degrees.

                perp(i) = 1.0f;

                v3 = RL*v1;
                v4 = RR*v1;

                if( v3.dot(v2) > 0.0f )
                {
                    // turning v1 CCW aligns v1 and v2. Left turn identified
                    leftright(i) = 0.0f;
                }
                else if (v4.dot(v2) > 0.0f)
                {
                    // a right turn is found
                    leftright(i) = 1.0f;
                }
                else
                {
                    // a turn is detected, but is not a left turn: so it has to be a right turn
                    leftright(i) = 2.0f;
                }

            }
            else
            {
                // no turn is being made
                perp(i) = 0.0f;
                leftright(i) = 2.0f;
            }
        }
        else
        {
            // A segment of 0 length is found
            perp(i) = 2.0f;
            leftright(i) = 2.0f;
        }
    }       // subfunction end.

//    cout << "All segments have been evaluated, turns are: " << endl;
//    for (int i=0;i < leftright.rows(); ++i)
//    {
//        if (leftright(i) == 0.0f) {cout << "Turn " << i << " is left" << endl;}
//        else if(leftright(i) == 1.0f) {cout << "Turn " << i << " is right" << endl;}
//        else {{cout << "Turn " << i << " is no turn" << endl;}}
//    }


    //   =====================================================================================  //
    // Dead end detection routine.

    // find the number of dead ends to construct the deadends vector.
    for(int i = 0; i < leftright.rows()-1; ++i)
    {
        // look for a dead end sequence, indecated by two consecutave left turns.
        if ( (leftright(i) == 0.0f) && (leftright(i+1) == 0.0f) )
        {
            nDE++;
        }
    }
    if( nDE == 0 )
    {
        DeadEnds.resize(1,2);
        DeadEnds.setZero();
        DEmidpoint.resize(1,2);
        DEmidpoint.setZero();
    }
    else
    {
        DeadEnds.resize(4*nDE,2);
        DEmidpoint.resize(nDE,2);
    }

    DE.resize(4,2);
    for(int i = 0; i < leftright.rows()-1; ++i)
    {
        // look for a dead end sequenvoid initDElocations(float *DEcoords, int nDEdesired);ce, indecated by two consecutave left turns.
        if ( (leftright(i) == 0.0f) && (leftright(i+1) == 0.0f) )
        {
            // possible dead end detected.
            // now perform template matching

            // first make a vector containing the dead end sections (so the 4 points from the wall detection function)
            DE.row(0) = cornersMat.block<1,2>(i,0);
            DE.row(1) = cornersMat.block<1,2>(i+1,0);
            DE.row(2) = cornersMat.block<1,2>(i+2,0);
            DE.row(3) = cornersMat.block<1,2>(i+3,0);

            // make vectors of the dead end walls.
            widht_DE_vect(0,0) = DE(2,0) - DE(1,0); widht_DE_vect(0,1) = DE(2,1) - DE(1,1);
            lleft_DE_vect(0,0) = DE(3,0) - DE(2,0); lleft_DE_vect(0,1) = DE(3,1) - DE(2,1);
            lright_DE_vect(0,0) = DE(1,0) - DE(0,0); lright_DE_vect(0,1) = DE(1,1) - DE(0,1);

            // compare dead end to template
            if ( (widht_DE_vect.norm() < DEwidth_ub) && (widht_DE_vect.norm() > DEwidth_lb))
            {
                // The width matches the template
                if ( (lleft_DE_vect.norm() > DElength) && (lright_DE_vect.norm() > DElength) )
                {
                    // the side walls are also long enough
                    DeadEnds.block<4,2>(nDE_dummy,0) = DE;

                    v_toMidPoint = (widht_DE_vect/widht_DE_vect.norm() * DISTANCE_DOOR_REFERENCE_POINT) * RL;
                    DEmidpoint.row(nDE_dummy) = widht_DE_vect*0.5 + v_toMidPoint + DE.row(1);

                    nDE_dummy++;


                }
            }


        }
    }       // function end

    //   =====================================================================================  //
    // function return routine.

    // always give back the closest dead end.
    if(nDE_dummy >= 1)
    {
        // multiple dead ends found
        // find nearest dead end
        for( int i = 0; i < nDE_dummy; ++i)
        {
            if( DEmidpoint.row(i).norm() < closestDE) {closestDE = DEmidpoint.row(i).norm(); nearestDE = i;}
        }
    }
    else {nearestDE = 0;}

    if (closestDE <= distToDEThreashold){ *distTODE = closestDE; }
    else
    {
        // dead end is to far away.
        nDE = 0; nDE_dummy = 0;
        *distTODE = 30.0f;
    }
    *DeadEndFound = nDE_dummy;

    DeadEndCoords[0] = DEmidpoint(nearestDE,0);
    DeadEndCoords[1] = DEmidpoint(nearestDE,1);

    // return all other dead ends for debugging purposes
//    int count1 = 1;
//    for(int i =0; i < nDE; ++i)
//    {
//        if(i != nearestDE)
//        {
//            // this DE is not the closest, so append to list of DE's
//            DeadEndCoords[count1*2] = DEmidpoint(i,0);
//            DeadEndCoords[(count1*2 + 1)] = DEmidpoint(i,1);
//            count1++;
//        }
//    }


}

void DeadEnd::initDElocations(float *DEcoords, int nDEdesired)
{
    // initialize all DE coordinates to 0.
    for ( int i = 0; i < 2*nDEdesired; ++i)
    {
        DEcoords[i] = 0.0f;
    }
}