-
-
Save Gengoz/d7f42a33a0955dd340015b658c8e9a2b 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 DeadEnd::isDeadEnd(Corners corners, int * DeadEndFound, float * DeadEndCoords, float * distTODE) | |
{ | |
// start of the real functions for dead end detection. | |
// variable declarations. | |
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 | |
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; | |
} | |
} | |
// ===================================================================================== // | |
// 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 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); | |
} | |
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