Skip to content

Instantly share code, notes, and snippets.

/Hough detection Secret

Created Jun 26, 2015
Embed
What would you like to do?
Some function used for feature detection, explanation per function is in the comments
/* lineaPToPolar()
* ACT: This function converts a line segment to a polar representation
* of an infinite line. Like this:
*
* y|+
* | +
* | +
* | +
* | * +
* | r* +
* | * +
* |* theta +
* |\______________ +
* x
*
* r is the shortest distance to the line and theta is the angle of the line
* with the x axes. This is needed to compare all the created hough lines, to
* check if duplicate lines exist. Polar coordinates are used because y=ax+b can't
* represent vertical lines
*
* IN: - vector with [x0,y0,x1,y1]; integers
*
* OUT: - array with [r,theta,x0,y0,x1,y1]; floats
*/
float* linearPToPolar(std::vector<int> points)
{
float dx = points[2] - points[0];
float dy = points[3] - points[1];
// calculate shortest distance from center of robot to (infinite) line
float r = (abs(points[2]*points[1] - points[3]*points[0]))/(sqrt(pow((points[3]-points[1]),2)+pow((points[2]-points[0]),2)));
// exception when the line is horizontal
if(dy == 0)
{
if (points[1] < 0)
{
Ppolar[1] = - 0.5 * CV_PI;
}
else
{
Ppolar[1] = 0.5 * CV_PI;
}
Ppolar[0] = abs(points[1]);
Ppolar[2] = points[0] + canvas_center.x;
Ppolar[3] = (points[1] - canvas_center.y)*-1;
Ppolar[4] = points[2] + canvas_center.x;
Ppolar[5] = (points[3] - canvas_center.y)*-1;
return Ppolar;
}
//exception when the line is vertical
else if(dx == 0)
{
if (points[0] < 0)
{
Ppolar[1] = CV_PI;
}
else
{
Ppolar[1] = 0;
}
Ppolar[0] = abs(points[0]);
Ppolar[2] = points[0] + canvas_center.x;
Ppolar[3] = (points[1] - canvas_center.y)*-1;
Ppolar[4] = points[2] + canvas_center.x;
Ppolar[5] = (points[3] - canvas_center.y)*-1;
return Ppolar;
}
// calculate theta of line r
else
{
// make a cartesian representation of the line y=ax+b
float a = (dy/dx);
float b = points[1]-a*points[0];
float theta;
//angle is between 0 and 0.5 pi when a is negative and b is positive
if (a < 0 && b > 0){
theta = 0.5*CV_PI - acos(r/b);
}
//angle is between 1 and 1.5 pi when a is negative and b is negative
else if (a < 0 && b < 0)
{
theta = 1.5*CV_PI - acos(r/-b);
}
//angle is between 0.5 and 1 pi when a is positive and b is positive
else if (a > 0 && b > 0)
{
theta = 0.5*CV_PI + acos(r/b);
}
//angle is between 0 and -0.5 pi when a is negative and b is negative
else if (a > 0 && b < 0)
{
theta = 0 - (0.5*CV_PI - acos(r/-b));
}
else
{
std::cout << "error\n";
}
Ppolar[0] = r;
Ppolar[1] = theta;
Ppolar[2] = points[0] + canvas_center.x;
Ppolar[3] = (points[1] - canvas_center.y)*-1;
Ppolar[4] = points[2] + canvas_center.x;
Ppolar[5] = (points[3] - canvas_center.y)*-1;
return Ppolar;
}
}
/* post_hough()
* ACT: The hough detection sends out many duplicate lines
* normally you could change the resolution of the hough detect function
* , but this didn't give the desired effect. So a filter function was created.
* Lines are converted to polar represantations with lineaPToPolar()
* and when a duplicate exists only the longest line is saved. This way we
* don't have to average over time to get the best result and we have faster code.
* Because the lineaPToPolar() function returns ifinite lines an extra functionality
* is build in this function to check if lines that have the same polar represantation
* are in fact the same lines. Like in this example:
*
*
* | |
* | | <-- same polar represantation but different line
* | |
* |
* |
* | |
* | | <-- compared line
* | |
*
*
* IN: - vector of lines with element 4 channels deep line1:[x0,y0,x1,y1]; integers
* line2:[x0,y0,x1,y1]
* of the lines detected by the HoughLinesP() function
*
* OUT: - Matrix with only the non duplicate longest lines, so the correct lines.
* with rows of the lines and colomns of the coordinates of the points
* that make up the line.; cv::Mat
*
*/
cv::Mat post_hough(std::vector<cv::Vec4i> lines)
{
cv::Mat post_lines;
//iterate over all the lines detected by HoughLinesP()
for( size_t i = 0; i < lines.size(); i++ )
{
//convert to usable vector
std::vector<int> convert;
for( int k = 0; k < 4; k++ )
{
int y = ((lines[i][k]) * pow(-1,k)) - (canvas_center.x * pow(-1,k)) ;
convert.push_back(y);
}
//convert to infinte polar represantation
float *polarlocal = linearPToPolar(convert);
convert.clear();
//always write the first line
if(post_lines.empty())
{
float p[6] = {polarlocal[0], polarlocal[1], polarlocal[2], polarlocal[3], polarlocal[4], polarlocal[5]};
cv::Mat m(1,6,CV_32FC1, &p);
post_lines.push_back(m);
}
else
{
//iterate over al the lines in the post processed matrix (so without duplicates)
for (int j = 0; j < post_lines.rows;j++)
{
//check if the new line already has an existing duplicate in the post processed matrix
if ( fabs(polarlocal[0] - post_lines.at<float>(j,0)) <= LibConst::told && ( fabs(polarlocal[1] - post_lines.at<float>(j,1)) <= LibConst::ttold || fabs(fabs(polarlocal[1] - post_lines.at<float>(j,1)) - 2*CV_PI) <= LibConst::ttold) )
{
float dx_post = post_lines.at<float>(j,4)-post_lines.at<float>(j,2);
float dy_post = post_lines.at<float>(j,5)-post_lines.at<float>(j,3);
float dx_loc = polarlocal[4] - polarlocal[2];
float dy_loc = polarlocal[5] - polarlocal[3];
//check if the duplicate line is longer
if( sqrt(dx_post*dx_post+dy_post*dy_post) <= sqrt(dx_loc*dx_loc + dy_loc*dy_loc))
{
//check if one of the points of the shorter line falls between the points of the longer line.
//if this is not the fact the line is actually a different line (see description of function).
/* | |
* | | <-- same polar represantation but different line
* | |
* |
* |
* | |
* | | <-- compared line
* | |
* */
float ExP1x = post_lines.at<float>(j,2);
float ExP1y = post_lines.at<float>(j,3);
float ExP2x = post_lines.at<float>(j,4);
float ExP2y = post_lines.at<float>(j,5);
float NewP1x = polarlocal[2];
float NewP1y = polarlocal[3];
float NewP2x = polarlocal[4];
float NewP2y = polarlocal[5];
if ( (ExP1x - LibConst::told <= NewP2x && ExP1x + LibConst::told >= NewP1x || ExP1x - LibConst::told <= NewP1x && ExP1x + LibConst::told >= NewP2x
|| ExP2x - LibConst::told <= NewP2x && ExP2x + LibConst::told >= NewP1x || ExP2x - LibConst::told <= NewP1x && ExP2x + LibConst::told >= NewP2x )
&&
( ExP1y - LibConst::told <= NewP2y && ExP1y + LibConst::told >= NewP1y || ExP1y - LibConst::told <= NewP1y && ExP1y + LibConst::told >= NewP2y
|| ExP2y - LibConst::told <= NewP2y && ExP2y + LibConst::told >= NewP1y || ExP2y - LibConst::told <= NewP1y && ExP2y + LibConst::told >= NewP2y ) )
{
// overwrite the existing shorter line and break so a new line can be processed
for(int l = 0; l < 6; l++)
{
post_lines.at<float>(j,l) = polarlocal[l];
}
break;
}
}
// the line is shorter so we check if it is in fact the same line, the new line already exists but is longer so this new line doesn't need to be saved.
// break if this is true, so a new line can be processed.
else
{
float ExP1x = polarlocal[2];
float ExP1y = polarlocal[3];
float ExP2x = polarlocal[4];
float ExP2y = polarlocal[5];
float NewP1x = post_lines.at<float>(j,2);
float NewP1y = post_lines.at<float>(j,3);
float NewP2x = post_lines.at<float>(j,4);
float NewP2y = post_lines.at<float>(j,5);
if ( (ExP1x - LibConst::told <= NewP2x && ExP1x + LibConst::told >= NewP1x || ExP1x - LibConst::told <= NewP1x && ExP1x + LibConst::told >= NewP2x
|| ExP2x - LibConst::told <= NewP2x && ExP2x + LibConst::told >= NewP1x || ExP2x - LibConst::told <= NewP1x && ExP2x + LibConst::told >= NewP2x )
&&
( ExP1y - LibConst::told <= NewP2y && ExP1y + LibConst::told >= NewP1y || ExP1y - LibConst::told <= NewP1y && ExP1y + LibConst::told >= NewP2y
|| ExP2y - LibConst::told <= NewP2y && ExP2y + LibConst::told >= NewP1y || ExP2y - LibConst::told <= NewP1y && ExP2y + LibConst::told >= NewP2y ) )
{
break;
}
}
}
// if all the lines in the post processed matrix are checked and the line doesn yet exist in the post processed matrix add it to the bottom of the post processed matrix
// and again break, otherwise the line will be added indefinitly
if(j == post_lines.rows - 1)
{
float p[6] = {polarlocal[0], polarlocal[1], polarlocal[2], polarlocal[3], polarlocal[4], polarlocal[5]};
cv::Mat m(1,6,CV_32FC1, &p);
post_lines.push_back(m);
break;
}
}
}
}
return post_lines;
}
/* houghdetect()
* ACT: Hough detection, to fit a line over the laserdata points
* see http://docs.opencv.org/doc/tutorials/imgproc/imgtrans/hough_lines/hough_lines.html
* for more information. For preprocessing we added a gaussian blur to apply some noise reduction
* on the laserdata.
*
* IN: - LRF data points; emc::LaserData
* - Size of detetion area; cv::Point2i
*
* OUT: - vector with [x0,y0,x1,y1,r,theta]; doubles
*
*/
std::vector<cv::Vec6d> houghdetect(emc::LaserData &scan, cv::Point2i size)
{
// create matrix of the laserdata
cv::Mat canvas(size.x, size.y, CV_8UC3, cv::Scalar(0, 0, 0));
canvas_center = cv::Point2d(canvas.rows / 2, canvas.cols / 2);
// Draw the LRF data points in the matrix
double a = scan.angle_min;
for(unsigned int i = 0; i < scan.ranges.size(); ++i)
{
double x = cos(a) * scan.ranges[i];
double y = sin(a) * scan.ranges[i];
cv::Point2d p = worldToCanvas(x, y);
if (p.x >= 0 && p.y >= 0 && p.x < canvas.cols && p.y < canvas.rows)
canvas.at<cv::Vec3b>(p) = cv::Vec3b(0, 255, 0);
a += scan.angle_increment;
}
// Preprocessing for hough detection. First we blur the image of the laserdata to do some noise reduction
// and then we make a binary image (grayscale)
cv:: Mat dst, post, dst2;
cv::GaussianBlur( canvas, dst, cv::Size(5,5), 0);
cv::cvtColor(dst, dst2, CV_BGR2GRAY);
// Probabilistic Hough line transform (input matrix, output vector of found lines, resolution of r, resolution of theta, threshold before a line is accepted,
// minimum line length, maximum distance between points to be counted as one line)
std::vector<cv::Vec4i> lines;
cv::HoughLinesP( dst2, lines, 1, CV_PI/180, 40, 10, 20 );
//post processing of the hough lines (remove duplicates)
post = post_hough(lines);
std::vector<cv::Vec6d> clean_lines;
for (int i = 0; i < post.rows; i++)
{
double* colines = canvasToWorld4(post.at<float>(i,2), post.at<float>(i,3), post.at<float>(i,4), post.at<float>(i,5));
clean_lines.push_back(cv::Vec6d(colines[0],colines[1],colines[2],colines[3],post.at<float>(i,0),post.at<float>(i,1)));
}
return clean_lines;
}
/* calc_junction()
* ACT: Function that returns coordinates of the gaps in the maze.
* It uses the lines detected by houghdetect and checks for three
* different situations.
* 1. Crossing:
* look for 2 lines with the same polar represenation which means
* they are in line with each other. The middle of the two closest
* points is then calculated and stored as a possible gap setpoint.
* 2. T-junction or turn:
* Look for 2 lines that are perpendicular to each other.
* If they are a intersection point is calculated and it is checked
* if this intersection is on one of the lines. This makes sure no false
* turns of T-junctions are detected. The midpoint between the intersection
* and the endpoint of the other lane is returned as a possible gap setpoint.
* 3. Corridor:
* Look for 2 lines that are parallel to each other.
* the mid point between the two furthest endpoints of the lines is stored
* as the possible gap setpoint.
*
* IN: - LRF data points; emc::LaserData
* - Size of detetion area; cv::Point2i
*
*
* OUT: - Vector with setpoints of in a vector [a b c]
* with a left, b middle and c right.
*
*/
std::vector<cv::Vec2d> calc_junction(emc::LaserData &scan, cv::Point2i size)
{
std::vector<cv::Vec6d> lines;
lines = houghdetect(scan, size);
std::vector<cv::Vec2d> gapcoor;
std::vector<cv::Vec2d> gapcoor_clean;
std::vector<cv::Vec2d> Veca;
std::vector<cv::Vec2d> Vecb;
std::vector<cv::Vec2d> Vecc;
for (size_t i = 0; i < lines.size(); i++)
{
for (size_t j = 0; j < lines.size(); j++)
{
if (i !=j)
{
// Check for crossing, so check if two lines have the same polar represantation
if ( fabs(lines[i][4] - lines[j][4]) <= LibConst::told && fabs(lines[i][5] - lines[j][5]) <= LibConst::ttold )
{
double distance = 9999.99;
double new_distance = 0;
double midx;
double midy;
double dy;
double dx;
// If two lines are in lines are in line with each other, check for the end
// points closest to each other and calculate the midpoint
for (int k = 0; k < 2; k++)
{
for (int l = k+l; l < 2; l++)
{
dy = lines[i][1 + k*2] - lines[j][1 + l*2];
dx = lines[i][0 + k*2] - lines[j][0 + l*2];
new_distance = sqrt(pow(dy,2) + pow(dx,2));
if (new_distance < distance)
{
distance = new_distance;
midx = (lines[i][0 + k*2] + lines[j][0 + l*2])/2;
midy = (lines[i][1 + k*2] + lines[j][1 + l*2])/2;
}
}
}
// If the setpoint is not 0, push it to the vector with possible gap setpoints
if(new_distance >= LibConst::told*resolution)
{
gapcoor.push_back(cv::Vec2d(midx,midy));
}
break;
}
}
}
for (size_t j = 0; j < lines.size(); j++)
{
if (i !=j)
{
// Check for a right/left turn or T-junction (perpendicular lines)
if ( ( ( lines [i][5] + LibConst::ttold + 0.5*CV_PI) >= lines[j][5] && ( lines [i][5] - LibConst::ttold + 0.5*CV_PI ) <= lines[j][5]
|| ( ( lines [j][5] + LibConst::ttold + 0.5*CV_PI) >= lines[i][5] && ( lines [j][5] - LibConst::ttold + 0.5*CV_PI ) <= lines[i][5]) ) )
{
int x;
float y;
// Exceptions for horizontal or vertical lines, because else
// when we calculate the intersection point we divide by 0
// and no valid intersection point is returned
if(fabs(lines[i][5]) < 0.1)
{
x = lines[i][4];
y = lines[j][4];
}
else if (fabs(lines[i][5] - CV_PI) < 0.1 )
{
x = -1 * lines[i][4];
y = lines[j][4];
}
else if(fabs(lines[j][5]) < 0.1)
{
x = lines[j][4];
y = lines[i][4];
}
else if (fabs(lines[j][5] - CV_PI) < 0.1 )
{
x = -1 * lines[j][4];
y = lines[i][4];
}
else
{
for(x = -250; x <= 250; x ++)
{
// Calculate the intersection coordinates bij extending the lines and check
// when they have the same y value. Break when this happens so the right x value is stored
if ( ( ((-1*cos(lines[i](5)))/(sin(lines[i][5]))) * x + (lines[i][4]/sin(lines[i][5])) + LibConst::told ) >= ( ((-1*cos(lines[j](5)))/(sin(lines[j][5]))) * x + (lines[j][4]/sin(lines[j][5])) )
&& ( ((-1*cos(lines[i](5)))/(sin(lines[i][5]))) * x + (lines[i][4]/sin(lines[i][5])) - LibConst::told ) <= ( ((-1*cos(lines[j](5)))/(sin(lines[j][5]))) * x + (lines[j][4]/sin(lines[j][5])) ) )
{
y = ((-1*cos(lines[i](5)))/(sin(lines[i][5]))) * x + (lines[i][4]/sin(lines[i][5]));
break;
}
}
// When no intersection is found break.
if (x == 251)
{
break;
}
}
// Check if point lies on line i
if ( (((x + LibConst::told)*resolution >= lines[i][0] && (x - LibConst::told)*resolution <= lines[i][2]) || ((x + LibConst::told)*resolution >= lines[i][2] && (x - LibConst::told)*resolution <= lines[i][0]))
&& (((y + LibConst::told)*resolution >= lines[i][1] && (y - LibConst::told)*resolution <= lines[i][3]) || ((y + LibConst::told)*resolution >= lines[i][3] && (y - LibConst::told)*resolution <= lines[i][1])) )
{
double distance = 9999.99;
double new_distance = 0;
double midx;
double midy;
double dy;
double dx;
// If so check which endpoint of line j is closest and calculate the midpoint
for (int k = 0; k < 2; k++)
{
dy = y*resolution - lines[j][1 + k*2];
dx = x*resolution - lines[j][0 + k*2];
new_distance = sqrt(pow(dy,2) + pow(dx,2));
if (new_distance < distance)
{
distance = new_distance;
midx = (x*resolution + lines[j][0 + k*2])/2;
midy = (y*resolution + lines[j][1 + k*2])/2;
}
}
// If the setpoint is not 0, push it to the vector with possible gap setpoints
if(distance >= LibConst::told*resolution)
{
gapcoor.push_back(cv::Vec2d(midx,midy));
}
break;
}
// Check if point lies on line j
else if ( (((x + LibConst::told)*resolution >= lines[j][0] && (x - LibConst::told)*resolution <= lines[j][2]) || ((x + LibConst::told)*resolution >= lines[j][2] && (x - LibConst::told)*resolution <= lines[j][0]))
&& (((y + LibConst::told)*resolution >= lines[j][1] && (y - LibConst::told)*resolution <= lines[j][3]) || ((y + LibConst::told)*resolution >= lines[j][3] && (y - LibConst::told)*resolution <= lines[j][1])) )
{
double distance = 9999.99;
double new_distance = 0;
double midx;
double midy;
double dy;
double dx;
// If so check which endpoint of line i is closest and calculate the midpoint
for (int k = 0; k < 2; k++)
{
dy = y*resolution - lines[i][1 + k*2];
dx = x*resolution - lines[i][0 + k*2];
new_distance = sqrt(pow(dy,2) + pow(dx,2));
if (new_distance < distance)
{
distance = new_distance;
midx = (x*resolution + lines[i][0 + k*2])/2;
midy = (y*resolution + lines[i][1 + k*2])/2;
}
}
// If the setpoint is not 0, push it to the vector with possible gap setpoints
if(distance >= LibConst::told*resolution)
{
gapcoor.push_back(cv::Vec2d(midx,midy));
}
break;
}
}
}
}
for (size_t j = 0; j < lines.size(); j++)
{
if (i !=j)
{
// Check for straight corridor (parallel walls)
if ( ( ( lines [i][5] + LibConst::ttold + CV_PI) >= lines[j][5] && ( lines [i][5] - LibConst::ttold + CV_PI ) <= lines[j][5]
|| ( ( lines [j][5] + LibConst::ttold + CV_PI) >= lines[i][5] && ( lines [j][5] - LibConst::ttold + CV_PI ) <= lines[i][5]) ) )
{
cv::Point2d P1;
cv::Point2d P2;
// Take the highest points of the line
if (lines[i][1] >= lines[i][3])
{
P1 = cv::Point2d(lines[i][0],lines[i][1]);
}
else if (lines[i][1] < lines[i][3])
{
P1 = cv::Point2d(lines[i][2],lines[i][3]);
}
if (lines[j][1] >= lines[j][3])
{
P2 = cv::Point2d(lines[j][0],lines[j][1]);
}
else if (lines[j][1] < lines[j][3])
{
P2 = cv::Point2d(lines[j][2],lines[j][3]);
}
double midx = (P1.x + P2.x)/2;
double midy = (P1.y + P2.y)/2;
gapcoor.push_back(cv::Vec2d(midx,midy));
break;
}
}
}
}
//divide in sections: a left, b middle, c right
for (size_t i = 0; i < gapcoor.size(); i++)
{
if (gapcoor[i][1] >= 0.01)
{
if (gapcoor[i][1]/gapcoor[i][0] >= -4 && gapcoor[i][1]/gapcoor[i][0] < 0)
{
//left
Veca.push_back(cv::Vec2d(gapcoor[i][0],gapcoor[i][1]));
}
else if (gapcoor[i][1]/gapcoor[i][0] <= 4 && gapcoor[i][1]/gapcoor[i][0] > 0)
{
//right
Vecc.push_back(cv::Vec2d(gapcoor[i][0],gapcoor[i][1]));
}
else
{
//middle
Vecb.push_back(cv::Vec2d(gapcoor[i][0],gapcoor[i][1]));
}
}
}
cv::Vec2d a;
cv::Vec2d b;
cv::Vec2d c;
// Filter a, only hold the closest setpoint
double distance = 9999.99;
for (size_t i = 0; i < Veca.size(); i++)
{
double dy = Veca[i][1];
double dx = Veca[i][0];
double new_distance = sqrt(pow(dy,2) + pow(dx,2));
if (new_distance < distance)
{
distance = new_distance;
a = Veca[i];
}
}
gapcoor_clean.push_back(a);
// Filter b, only hold the furthest setpoint
distance = 0;
for (size_t i = 0; i < Vecb.size(); i++)
{
double dy = Vecb[i][1];
double dx = Vecb[i][0];
double new_distance = sqrt(pow(dy,2) + pow(dx,2));
if (new_distance > distance && ( a[1] + LibConst::told*resolution <= Vecb[i][1] && c[1] + LibConst::told*resolution <= Vecb[i][1] ) )
{
distance = new_distance;
b = Vecb[i];
}
}
gapcoor_clean.push_back(b);
// Filter c, only hold the closest setpoint
distance = 9999.99;
for (size_t i = 0; i < Vecc.size(); i++)
{
double dy = Vecc[i][1];
double dx = Vecc[i][0];
double new_distance = sqrt(pow(dy,2) + pow(dx,2));
if (new_distance < distance)
{
distance = new_distance;
c = Vecc[i];
}
}
gapcoor_clean.push_back(c);
return gapcoor_clean;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment