-
-
Save anonymous/49a7d91d327d4bf225661b34c705b1c1 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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; | |
} | |
} | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment