-
-
Save anonymous/9c4249e5b5299a82914a to your computer and use it in GitHub Desktop.
Some function used for feature detection, explanation per function is in the comments
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
| /* 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