/*
* Returns a vector of the lines obtained from the Hough Transform.
* @param useMinimum if enabled, the voting threshold is lowered until HOUGH_MIN_LINES can be obtained.
*/
vector<Line> RoadDetection::getHoughLines(Mat frame, bool useMinimum)
{
vector<Vec2f> lines;
vector<Line> filteredLines;
// if using minimum, repeat hough process while lowering the voting until we have HOUGH_MIN_LINES
if (useMinimum)
{
int thresh = HOUGH_MIN_THRESH;
while ((int)lines.size() < HOUGH_MIN_LINES && thresh > 0)
{
HoughLines(frame, lines, 1, CV_PI / 180, thresh, 0, 0);
thresh -= 5;
}
}
// otherwise, run hough process once using a default voting value
else
{
HoughLines(frame, lines, 1, CV_PI / 180, HOUGH_DEFAULT_THRESH, 0, 0);
}
// convert rho and theta to actual points
for (size_t i = 0; i < lines.size(); i++)
{
float rho = lines[i][0];
float theta = lines[i][1];
if (theta < HOUGH_MIN_ANGLE || theta > HOUGH_MAX_ANGLE)
{
Point pt1, pt2;
double a = cos(theta);
double b = sin(theta);
double x0 = a*rho;
double y0 = b*rho;
pt1.x = cvRound(x0 + 1000 * (-b));
pt1.y = cvRound(y0 + 1000 * (a));
pt2.x = cvRound(x0 - 1000 * (-b));
pt2.y = cvRound(y0 - 1000 * (a));
Line line = Line(pt1, pt2);
filteredLines.push_back(line);
}
}
return filteredLines;
}
ResultLines lineDetection(Mat origin,int w1, int w2) {
vector<Vec2f> lines;
ResultLines resultlines;
Mat canny;
Mat test = origin.clone();
Canny(origin,canny,100,500);
HoughLines(canny, lines, 1, CV_PI / 180, w1, w2, 0);
setLineSize(lines.size());
for (size_t i = 0; i < lines.size(); i++) {
float rho = lines[i][0], theta = lines[i][1];
Point pt1, pt2;
double a = cos(theta), b = sin(theta);
double x0 = a * rho, y0 = b * rho;
pt1.x = cvRound(x0 + 1000 * (-b));
pt1.y = cvRound(y0 + 1000 * (a));
pt2.x = cvRound(x0 - 1000 * (-b));
pt2.y = cvRound(y0 - 1000 * (a));
Vec4i vec4i = Vec4i(pt1.x,pt1.y,pt2.x,pt2.y);
double degree = theta/CV_PI*180;
line(test,pt1,pt2,Scalar(255,255,255),3);
degreeChecking(degree,vec4i,pt1,pt2,&resultlines,origin);
resultlines.allLines.push_back(vec4i);
}
return resultlines;
}
// ********** HOUGH LINE STUFF ************//
ImageUtils::HoughDataNonP houghLinesNonP(cv::Mat& im_src) {
cv::Mat im_copy;
im_src.copyTo(im_copy);
ImageUtils::HoughDataNonP hough;
HoughLines(im_src, hough.lines,1,5*3.14/180.0,100,0,0);
//HOUGH_RES_RHO_PIXEL, HOUGH_RES_THETA_RAD,
//HOUGH_MIN_VOTES, 0, 0);
return hough;
}
///////////////////////////////////////////////////////////////////
// Panel::CannyDetection()
// Description: This is the debug version of CannyDetection(). As
// mentioned in DetectEdges() description, we know this is bad
// coding to have similar code in different functions but we
// were on a short schedule. The differences in this function and
// CannyDetection are that this one contains sliders which the
// Canny parameters can be adjusted and the intersections are not
// computed after Hough lines. See description of CannyDetection()
// for more information on the image processing used in this
// function.
///////////////////////////////////////////////////////////////////
Mat Panel::CannyDetectionDebug(Mat image, bool showImg)
{
Mat greyImage;
cvtColor(image, greyImage, CV_BGR2GRAY);
Mat eroded, dilated, thresh, blurredThresh, edges, edgesGray;
vector<Vec2f> lines;
// This code is for testing different values of various functions
// used with Canny, Blurring, and Hough Lines
namedWindow("Sliders", CV_WINDOW_KEEPRATIO);
cvCreateTrackbar("low_threshold", "Sliders", &m_lowCannyThreshold, 255);
cvCreateTrackbar("blur_sigma_x", "Sliders", &m_sigmaX, 100);
cvCreateTrackbar("blur_sigma_y", "Sliders", &m_sigmaY, 100);
cvCreateTrackbar("canny_low", "Sliders", &m_cannyLow, 500);
cvCreateTrackbar("canny_ratio", "Sliders", &m_ratio, 5);
cvCreateTrackbar("min_hough_length", "Sliders", &m_houghLength, 1000);
while (true)
{
threshold(greyImage, thresh, m_lowCannyThreshold, 255, THRESH_BINARY);
erode(thresh, eroded, Mat());
dilate(eroded, dilated, Mat());
GaussianBlur(thresh, blurredThresh, Size(7, 7), m_sigmaX, m_sigmaY);
Canny(blurredThresh, edges, m_cannyLow, m_cannyLow*m_ratio, 3);
HoughLines(edges, lines, 1, CV_PI / 180, m_houghLength, 0, 0);
cvtColor(edges, edgesGray, CV_GRAY2BGR);
for (size_t i = 0; i < lines.size(); i++)
{
float rho = lines[i][0], theta = lines[i][1];
Point pt1, pt2;
double a = cos(theta), b = sin(theta);
double x0 = a*rho, y0 = b*rho;
pt1.x = cvRound(x0 + 1000 * (-b));
pt1.y = cvRound(y0 + 1000 * (a));
pt2.x = cvRound(x0 - 1000 * (-b));
pt2.y = cvRound(y0 - 1000 * (a));
line(edgesGray, pt1, pt2, Scalar(0, 0, 255), 3, CV_AA);
}
namedWindow("Threshhold", CV_WINDOW_KEEPRATIO);
imshow("Threshhold", thresh);
namedWindow("Dilated", CV_WINDOW_KEEPRATIO);
imshow("Dilated", dilated);
namedWindow("Blurred", CV_WINDOW_KEEPRATIO);
imshow("Blurred", blurredThresh);
namedWindow("Canny Edges", CV_WINDOW_KEEPRATIO);
imshow("Canny Edges", edges);
namedWindow("Hough Lines", CV_WINDOW_KEEPRATIO);
imshow("Hough Lines", edgesGray);
if (waitKey(30) == 27)
break;
}
return edges;
}
void calcAndOrDraw(Mat& img, Rect& roi, bool drawResult)
{
// Crop the image
Mat gray;
cvtColor(img, gray, CV_BGR2GRAY); //Grayscale
Mat detectedEdges;
blur(gray, detectedEdges, Size(3, 3)); //Blur
int val2 = 30;
Canny(detectedEdges, detectedEdges, val2, val2 * 3, 3); //Canny
vector<Vec2f> lines;
HoughLines(detectedEdges, lines, 1, CV_PI / 180, 150); //Hough Line
vector<Point2f> pts;
calcCrop(img.size(), lines, pts, roi);
if (drawResult)
drawCrop(img, pts, roi);
}
Mat HoughLineTransform::StandardHoughLineTransform(int, void*)
{
vector<Vec2f> lines;
HoughLines(dst, lines, 1, CV_PI / 180, 100, 0, 0);
for (size_t i = 0; i < lines.size(); i++)
{
float rho = lines[i][0], theta = lines[i][1];
Point pt1, pt2;
double a = cos(theta), b = sin(theta);
double x0 = a*rho, y0 = b*rho;
pt1.x = cvRound(x0 + 1000 * (-b));
pt1.y = cvRound(y0 + 1000 * (a));
pt2.x = cvRound(x0 - 1000 * (-b));
pt2.y = cvRound(y0 - 1000 * (a));
line(cdst, pt1, pt2, Scalar(0, 0, 255), 3, CV_AA);
}
return cdst;
}
std::vector<Line> Hough::calculate(cv::InputArray _image)
{
cv::Mat image = _image.getMat();
std::vector<Line> result;
cv::vector<cv::Vec2f> lines;
if(!image.data)
return result;
imageDiagonal = sqrt( pow(image.rows, 2) + pow(image.cols, 2));
cv::Mat cannyImage;
getCannyImage(cannyImage, image);
HoughLines(cannyImage, lines, 1, math::m_pi/180, minimalHoughVotes);
compareHoughLinesWithCannyEdges(result, lines, cannyImage);
return result;
}
Mat RoadWatcher::Calculate_And_Draw_RoadLines(Mat frame)
{
Mat half_frame, dst, cdst, editedFrame, blackHueg, blackProb;
int lowerFrameloc;
editedFrame = frame.clone();
half_frame = frame( Range( frame.rows/2 +50, frame.rows - 1 ), Range( 0, frame.cols - 1 ) );
//copy the half frame into these 2 frames, afterwards make them completely black. So that we can use these frames to bitwise and.
blackHueg = half_frame.clone();
blackProb = half_frame.clone();
blackHueg.setTo(Scalar(0,0,0));
blackProb.setTo(Scalar(0,0,0));
//define where the lanes should be located on the main frame.
lowerFrameloc = frame.rows/2 +50;
//convert the color + canny to prepare for (probablistic) houghlines
cvtColor(dst, cdst, CV_GRAY2BGR);
Canny(half_frame, dst, 50, 200, 3);
threshold(dst,cdst,128,255,THRESH_BINARY_INV);
vector<Vec2f> houghlines;
vector<Vec4i> probLines;
// detect lines
HoughLines(dst, houghlines, 1, CV_PI/180, Threshold, 0, 0 );
HoughLinesP(dst, probLines, 1, CV_PI/180, Threshold, 50, 10 );
blackHueg = Draw_HoughLines(blackHueg, houghlines);
blackProb = Draw_Probablistic_HoughLines(blackProb, probLines);
Mat linesToDraw = bitwise_And(blackHueg, blackProb);
Mat returner = Draw_RoadLines_On_Matrix(editedFrame, linesToDraw, lowerFrameloc);
return returner;
}
void feature_detection::lines(float &rho, float &theta, int &threshold)
{
HoughLines(edge_map, hough_lines, rho, theta, threshold, 0, 0);
}
vector<PlateLine> PlateLines::getLines(Mat edges, float sensitivityMultiplier, bool vertical)
{
if (this->debug)
cout << "PlateLines::getLines" << endl;
static int HORIZONTAL_SENSITIVITY = pipelineData->config->plateLinesSensitivityHorizontal;
static int VERTICAL_SENSITIVITY = pipelineData->config->plateLinesSensitivityVertical;
vector<Vec2f> allLines;
vector<PlateLine> filteredLines;
int sensitivity;
if (vertical)
sensitivity = VERTICAL_SENSITIVITY * (1.0 / sensitivityMultiplier);
else
sensitivity = HORIZONTAL_SENSITIVITY * (1.0 / sensitivityMultiplier);
HoughLines( edges, allLines, 1, CV_PI/180, sensitivity, 0, 0 );
for( size_t i = 0; i < allLines.size(); i++ )
{
float rho = allLines[i][0], theta = allLines[i][1];
Point pt1, pt2;
double a = cos(theta), b = sin(theta);
double x0 = a*rho, y0 = b*rho;
double angle = theta * (180 / CV_PI);
pt1.x = cvRound(x0 + 1000*(-b));
pt1.y = cvRound(y0 + 1000*(a));
pt2.x = cvRound(x0 - 1000*(-b));
pt2.y = cvRound(y0 - 1000*(a));
if (vertical)
{
if (angle < 20 || angle > 340 || (angle > 160 && angle < 210))
{
// good vertical
LineSegment line;
if (pt1.y <= pt2.y)
line = LineSegment(pt2.x, pt2.y, pt1.x, pt1.y);
else
line = LineSegment(pt1.x, pt1.y, pt2.x, pt2.y);
// Get rid of the -1000, 1000 stuff. Terminate at the edges of the image
// Helps with debugging/rounding issues later
LineSegment top(0, 0, edges.cols, 0);
LineSegment bottom(0, edges.rows, edges.cols, edges.rows);
Point p1 = line.intersection(bottom);
Point p2 = line.intersection(top);
PlateLine plateLine;
plateLine.line = LineSegment(p1.x, p1.y, p2.x, p2.y);
plateLine.confidence = (1.0 - MIN_CONFIDENCE) * ((float) (allLines.size() - i)) / ((float)allLines.size()) + MIN_CONFIDENCE;
filteredLines.push_back(plateLine);
}
}
else
{
if ( (angle > 70 && angle < 110) || (angle > 250 && angle < 290))
{
// good horizontal
LineSegment line;
if (pt1.x <= pt2.x)
line = LineSegment(pt1.x, pt1.y, pt2.x, pt2.y);
else
line =LineSegment(pt2.x, pt2.y, pt1.x, pt1.y);
// Get rid of the -1000, 1000 stuff. Terminate at the edges of the image
// Helps with debugging/ rounding issues later
int newY1 = line.getPointAt(0);
int newY2 = line.getPointAt(edges.cols);
PlateLine plateLine;
plateLine.line = LineSegment(0, newY1, edges.cols, newY2);
plateLine.confidence = (1.0 - MIN_CONFIDENCE) * ((float) (allLines.size() - i)) / ((float)allLines.size()) + MIN_CONFIDENCE;
filteredLines.push_back(plateLine);
}
}
}
return filteredLines;
}
int main( int argc, char** argv ) {
unsigned long nframes = 0, t_start, t_end;
if (argc == 1)
printf ("For options use --help\n\n");
for (int i = 1; i < argc; i++) {
if (!strcmp(argv[i],"1") || !strcmp(argv[i],"2"))
CAM_NUMBER = atoi (argv[i]);
else if (!strcmp (argv[i], "--test"))
TEST = 1;
else if (!strcmp (argv[i], "--grad"))
GRAD = 1;
else if (!strcmp (argv[i], "--watershed"))
WATERSHED = 1;
else if (!strcmp (argv[i], "--write"))
WRITE = 1;
else if (!strcmp (argv[i], "--break"))
BREAK = 1;
else if (!strcmp (argv[i], "--line"))
LINE = 1;
else if (!strcmp (argv[i], "--circle"))
CIRCLE = 1;
else if (!strcmp (argv[i], "--rect"))
RECT = 1;
else if (!strcmp (argv[i], "--gate"))
GATE = 1;
else if (!strcmp (argv[i], "--path"))
PATH = 1;
else if (!strcmp (argv[i], "--buoy"))
BUOY = 1;
else if (!strcmp (argv[i], "--frame"))
FRAME = 1;
else if (!strcmp (argv[i], "--timeout"))
TIMEOUT = atoi(argv[++i]);
else if (!strcmp (argv[i], "--pos"))
POS = atoi(argv[++i]);
else if (!strcmp (argv[i], "--load"))
LOAD = ++i; // put the next argument index into LOAD
else if (!strcmp (argv[i], "--help")) {
printf ("OpenCV based webcam program. Hit 'q' to exit. Defaults to cam0, writes to \"webcam.avi\"\n");
printf ("Put any integer as an argument (without --) to use that as camera number\n\n");
printf (" --write\n Write captured video to file.\n\n");
printf (" --load\n Use a video file (following --load) instead of a webcam.\n\n");
printf (" --line\n Run line finding code.\n\n");
printf (" --circle\n Run circle finding code.\n\n");
printf (" --break\n Pause the input (press any key to go to the next frame).\n\n");
printf (" Example: `./webcam 1 --write` will use cam1, and will write to disk\n\n");
return 0;
}
}
/// initialization
// init camera
mvCamera* camera = NULL;
if (LOAD == 0) {
camera = new mvCamera (CAM_NUMBER);
}
else {
camera = new mvCamera (argv[LOAD]);
}
if (camera != 0 && POS > 0) {
camera->set_relative_position(static_cast<double>(POS)/1000);
}
mvVideoWriter* frame_writer = NULL;
mvVideoWriter* filter_writer_1 = NULL;
mvVideoWriter* filter_writer_2 = NULL;
if (WRITE) {
frame_writer = new mvVideoWriter ("frames.avi");
filter_writer_1 = new mvVideoWriter ("filtered_1.avi");
filter_writer_2 = new mvVideoWriter ("filtered_2.avi");
}
// init windows
mvWindow* win1 = new mvWindow ("webcam");
mvWindow* win2 = new mvWindow ("win2");
mvWindow* win3 = new mvWindow ("win3");
// declare filters we need
mvHSVFilter HSVFilter ("test_settings.csv"); // color filter
mvBinaryMorphology Morphology7 (9,9, MV_KERN_ELLIPSE);
mvBinaryMorphology Morphology5 (5,5, MV_KERN_ELLIPSE);
mvHoughLines HoughLines ("test_settings.csv");
mvLines lines; // data struct to store lines
mvKMeans kmeans;
mvWatershedFilter watershed_filter;
mvContours contour_filter;
MDA_VISION_MODULE_GATE* gate=GATE? new MDA_VISION_MODULE_GATE : 0;
MDA_VISION_MODULE_PATH* path=PATH? new MDA_VISION_MODULE_PATH : 0;
MDA_VISION_MODULE_BUOY* buoy=BUOY? new MDA_VISION_MODULE_BUOY : 0;
MDA_VISION_MODULE_FRAME* frame_task=FRAME? new MDA_VISION_MODULE_FRAME : 0;
// declare images we need
IplImage* scratch_color = mvCreateImage_Color();
IplImage* scratch_color_2 = mvCreateImage_Color();
IplImage* filter_img = mvCreateImage ();
IplImage* filter_img_2 = mvCreateImage ();
/// execution
char c = 0;
IplImage* frame;
t_start = clock();
for (;;) {
frame = camera->getFrameResized(); // read frame from cam
if (LOAD) {
usleep(17000);
}
if (TIMEOUT > 0) {
if ((clock() - t_start)/CLOCKS_PER_SEC > TIMEOUT) {
break;
}
}
if (!frame) {
printf ("Video Finished.\n");
break;
}
if (nframes < 10) {
nframes++;
continue;
}
cvCopy (frame, scratch_color);
win1->showImage (scratch_color);
if (TEST) {
}
else if (GATE) {
if (gate->filter (frame) == FULL_DETECT) {
cvWaitKey(200);
}
}
else if (PATH) {
if (path->filter (frame) == FULL_DETECT) {
cvWaitKey(00);
}
}
else if (BUOY) {
if (buoy->filter (frame) == FULL_DETECT) {
cvWaitKey(200);
}
}
else if (FRAME) {
if (frame_task->filter (frame) == FULL_DETECT) {
cvWaitKey(200);
}
}
else if (WATERSHED) {
watershed_filter.watershed(frame, filter_img);
win1->showImage (frame);
win2->showImage (filter_img);
COLOR_TRIPLE color;
MvCircle circle;
MvCircleVector circle_vector;
MvRotatedBox rbox;
MvRBoxVector rbox_vector;
while ( watershed_filter.get_next_watershed_segment(filter_img_2, color) ) {
if (CIRCLE) {
contour_filter.match_circle(filter_img_2, &circle_vector, color);
}
else if (RECT) {
contour_filter.match_rectangle(filter_img_2, &rbox_vector, color, 2, 6);
}
/*
win3->showImage(filter_img_2);
cvWaitKey(200);
*/
}
if (CIRCLE && circle_vector.size() > 0) {
MvCircleVector::iterator iter = circle_vector.begin();
MvCircleVector::iterator iter_end = circle_vector.end();
int index = 0;
printf ("%d Circles Detected:\n", static_cast<int>(circle_vector.size()));
for (; iter != iter_end; ++iter) {
printf ("\tCircle #%d: (%3d,%3d), Rad=%5.1f, <%3d,%3d,%3d>\n", ++index,
iter->center.x, iter->center.y, iter->radius, iter->m1, iter->m2, iter->m3);
iter->drawOntoImage(filter_img_2);
}
win3->showImage (filter_img_2);
cvWaitKey(200);
}
else if (RECT && rbox_vector.size() > 0) {
MvRBoxVector::iterator iter = rbox_vector.begin();
MvRBoxVector::iterator iter_end = rbox_vector.end();
int index = 0;
printf ("%d Rectangles Detected:\n", static_cast<int>(rbox_vector.size()));
for (; iter != iter_end; ++iter) {
printf ("\tRect #%d: (%3d,%3d), Len=%5.1f, Width=%5.1f, Angle=%5.1f <%3d,%3d,%3d>\n", ++index,
iter->center.x, iter->center.y, iter->length, iter->width, iter->angle, iter->m1, iter->m2, iter->m3);
iter->drawOntoImage(filter_img_2);
}
win3->showImage (filter_img_2);
cvWaitKey(200);
}
}
if (GRAD) {
Morphology5.gradient(filter_img, filter_img);
}
if (LINE) {
HoughLines.findLines (filter_img, &lines);
kmeans.cluster_auto (1, 8, &lines, 1);
//lines.drawOntoImage (filter_img);
kmeans.drawOntoImage (filter_img);
lines.clearData(); // erase line data and reuse allocated mem
//kmeans.clearData();
win3->showImage (filter_img);
}
/*
else if (CIRCLE) {
CvPoint centroid;
float radius;
contour_filter.match_circle(filter_img, centroid, radius);
contour_filter.drawOntoImage(filter_img);
win3->showImage (filter_img);
}
else if (RECT) {
CvPoint centroid;
float length, angle;
contour_filter.match_rectangle(filter_img, centroid, length, angle);
contour_filter.drawOntoImage(filter_img);
win3->showImage (filter_img);
}
*/
if (WRITE) {
frame_writer->writeFrame (frame);
cvCvtColor (filter_img, scratch_color, CV_GRAY2BGR);
filter_writer_1->writeFrame (scratch_color);
cvCvtColor (filter_img_2, scratch_color, CV_GRAY2BGR);
filter_writer_2->writeFrame (scratch_color);
}
nframes++;
if (BREAK)
c = cvWaitKey(0);
else if (LOAD)
c = cvWaitKey(3); // go for about 15 frames per sec
else
c = cvWaitKey(5);
if (c == 'q')
break;
else if (c == 'w') {
mvDumpPixels (frame, "webcam_img_pixel_dump.csv");
mvDumpHistogram (frame, "webcam_img_histogram_dump.csv");
}
}
t_end = clock ();
printf ("\nAverage Framerate = %f\n", (float)nframes/(t_end - t_start)*CLOCKS_PER_SEC);
cvReleaseImage (&scratch_color);
cvReleaseImage (&scratch_color_2);
cvReleaseImage (&filter_img);
cvReleaseImage (&filter_img_2);
delete camera;
delete frame_writer;
delete filter_writer_1;
delete filter_writer_2;
delete win1;
delete win2;
delete win3;
return 0;
}
bool LineFilter::filter(Data *data){
// check for whether the input is of the correct type. From Albert
ImgData* imgData = dynamic_cast<ImgData*>(data);
if (imgData == 0) {
// track the error and return error
this->track(data,this->filterID,1,1);
return false;
}
cv::imshow("asdf", filter(imgData->getImg().clone(),0));
//begin filter sequence
int linesFound = 0;
cv::Mat src = imgData->getImg();
cv::Mat dst;
cv::Mat cdst = src.clone();
Canny(src, dst, 50, 200, 3);
cvtColor(dst, cdst, CV_GRAY2BGR);
std::vector<cv::Vec2f> lines;
//detects lines
HoughLines(dst, lines, 1, CV_PI/180, 100, 0, 0 );
//ending filter sequence
//calculating the line equation
linesEq.clear();
float x1 = 0, x2 = 0, y1 = 0, y2 = 0;
for( size_t i = 0; i < lines.size(); i++ ){
float rho = lines[i][0], theta = lines[i][1];
cv::Point pt1, pt2;
double a = cos(theta), b = sin(theta);
double x0 = a*rho, y0 = b*rho;
pt1.x = cvRound(x0 + 1000*(-b));
pt1.y = cvRound(y0 + 1000*(a));
pt2.x = cvRound(x0 - 1000*(-b));
pt2.y = cvRound(y0 - 1000*(a));
x1 = pt1.x;
y1 = pt1.y;
x2 = pt2.x;
y2 = pt2.y;
//equation of line
std::vector<float> eq;
//y = mx+b
//B MIGHT BE USELSES, NEED FURTHER TESTING
bool safeMath = true;
float M = 0, B = 0;
if (x2-x1 < 5){ //straight line
safeMath = false;
M = INFINITY;
B = INFINITY;
}
if (safeMath){ //avoid div by 0 error
//M = (y2-y1) / (x2-x1);
double realtheta = (rho < 0)? theta - M_PI:theta;
realtheta = -realtheta + M_PI/2;
M = tan(realtheta);
B = y2 - M*x2;
}
bool repeat = false;
//check if there is a similar line already
for (std::vector<float> lines: linesEq){
//vert line situations
if (M == INFINITY && lines[0] == INFINITY){
//check their x values
if (std::abs(lines[2] - ((x1+x2)/2)) < maxDiff){
repeat = true;
break;
}
}
//check if m is almost vertical
else if (std::abs(M) > maxSlope && lines[0] == INFINITY){
//std::cout<<"almost vert ";
//std::cout<<std::abs(lines[2] - ((x1+x2)/2))<<std::endl;
if (std::abs(lines[2] - ((x1+x2)/2) ) < maxDiff){
repeat = true;
break;
}
}
else if (M == INFINITY && std::abs(lines[0])> maxSlope){
//std::cout<<"almost vert II ";
//std::cout<<std::abs(lines[2] - ((x1+x2)/2))<<std::endl;
if (std::abs(lines[2] - ((x1+x2)/2) ) < maxDiff){
repeat = true;
break;
}
}
//check if m is too similar or not, b is too different to check
else if (std::abs(lines[0] - M) < maxDiff){
if (M > 15){ //vertical lines
//check if the intersection point is near the average x
if (std::abs((B-lines[1])/(lines[0]-M))-(x1+x2)/2 < maxDiff){
repeat = true;
break;
}
}else{ //horziontal lines
if (std::abs((B-lines[1])/(lines[0]-M))*M - (y1+y2)/2 < maxDiff){
repeat = true;
break;
}
}
}
}
if (!repeat){
eq.push_back(M);
eq.push_back(B);
//std::cout<<M<<" "<<B<<" " << ((x1+x2)/2) << " ";
if (std::abs(M) < 5){ //aprox horizontal line
eq.push_back(y2); //give it the y value
//std::cout<<y2;
//printf(" horz line");
}
if (std::abs(M) > maxSlope){ //vertical line
eq.push_back(x2); //x value
//std::cout<<x2;
//printf(" vertal line");
}
//std::cout<<std::endl;
linesEq.push_back(eq);
linesFound++;
//line(*cdst, pt1, pt2, cv::Scalar(0,0,255), 3, CV_AA); //drawing the line
}
}
//should i set imgData to something?? -Carl
//track and return
this->track(imgData, this->filterID, 0,0);
//println(linesFound != 0);
return linesFound != 0;
}
cv::Mat LineFilter::filter(cv::Mat src, int mode){
cv::Mat dst;
//cv::Mat cdst = cv::Mat(src.clone());
cv::Mat cdst = src.clone();
Canny(src, dst, 50, 200, 3);
cvtColor(dst, cdst, CV_GRAY2BGR);
if (mode == 0){
std::vector<cv::Vec2f> lines;
//detects lines
HoughLines(dst, lines, 1, CV_PI/180, 100, 0, 0 );
linesEq.clear();
float x1 = 0, x2 = 0, y1 = 0, y2 = 0;
//draws the lines detected
//println("-----------\n");
for( size_t i = 0; i < lines.size(); i++ ){
float rho = lines[i][0], theta = lines[i][1];
cv::Point pt1, pt2;
double a = cos(theta), b = sin(theta);
double x0 = a*rho, y0 = b*rho;
pt1.x = cvRound(x0 + 1000*(-b));
pt1.y = cvRound(y0 + 1000*(a));
pt2.x = cvRound(x0 - 1000*(-b));
pt2.y = cvRound(y0 - 1000*(a));
x1 = pt1.x;
y1 = pt1.y;
x2 = pt2.x;
y2 = pt2.y;
//equation of line
std::vector<float> eq;
//y = mx+b
//B MIGHT BE USELESS, NEED FURTHER TESTING
bool safeMath = true;
float M = 0, B = 0;
if (x2-x1 < 5){ //straight line
safeMath = false;
M = INFINITY;
B = INFINITY;
}
if (safeMath){ //avoid div by 0 error
M = (y2-y1) / (x2-x1);
B = y2 - M*x2;
}
bool repeat = false;
//check if there is a similar line already
for (std::vector<float> lines: linesEq){
//vert line situations
if (M == INFINITY && lines[0] == INFINITY){
//check their x values
if (std::abs(lines[2] - ((x1+x2)/2)) < maxDiff){
repeat = true;
break;
}
}
//check if m is almost vertical
else if (std::abs(M) > maxSlope && lines[0] == INFINITY){
//std::cout<<"almost vert ";
//std::cout<<std::abs(lines[2] - ((x1+x2)/2))<<std::endl;
if (std::abs(lines[2] - ((x1+x2)/2) ) < maxDiff){
repeat = true;
break;
}
}
else if (M == INFINITY && std::abs(lines[0])> maxSlope){
//std::cout<<"almost vert II ";
//std::cout<<std::abs(lines[2] - ((x1+x2)/2))<<std::endl;
if (std::abs(lines[2] - ((x1+x2)/2) ) < maxDiff){
repeat = true;
break;
}
}
//check if m is too similar or not, b is too different to check
else if (std::abs(lines[0] - M) < maxDiff){
if (M > 15){ //vertical lines
//check if the intersection point is near the average x
if (std::abs((B-lines[1])/(lines[0]-M))-(x1+x2)/2 < maxDiff){
repeat = true;
break;
}
}else{ //horziontal lines
//print("horz ");
//println((y1+y2)/2);
float x = (B-lines[1])/(lines[0]-M);
float y = x * M + B;
if (x < cdst.size().width && y < cdst.size().height){
repeat = true;
break;
}
}
}
}
if (!repeat){
eq.push_back(M);
eq.push_back(B);
//print(M);
//print(" ");
//print(B);
//print(" ");
//print((x1+x2)/2);
if (std::abs(M) < 0.5){ //aprox horizontal line
eq.push_back(y2); //give it the y value
print(y2);
print(" horz line");
}
if (std::abs(M) > maxSlope){ //vertical line
eq.push_back(x2); //x value
print(x2);
print(" vertal line");
}
//println("");
linesEq.push_back(eq);
line(cdst, pt1, pt2, cv::Scalar(0,0,255), 3, CV_AA); //drawing the line
}
}
}
else{
std::vector<cv::Vec4i> lines;
HoughLinesP(dst, lines, 1, CV_PI/180, 50, 50, 10 );
for( size_t i = 0; i < lines.size(); i++ ){
cv::Vec4i l = lines[i];
line(cdst, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]),
cv::Scalar(0,0,255), 3, CV_AA);
}
}
return cdst;
}
Point2f VanishingPointDetector::CalculateVanishingPoint(const Mat &image)
{
if (!initialized)
{
return Point2f(-1, -1);
}
if (stabilized)
{
return currentVanishingPoint;
}
float A = 0.0;
float B = 0.0;
float C = 0.0;
float D = 0.0;
float E = 0.0;
// Pre-Filtering
Mat workingCopy = image.clone();
// cvtColor(workingCopy, workingCopy, CV_RGB2GRAY);
Sobel(workingCopy, workingCopy, -1, 1, 0);
threshold(workingCopy, workingCopy, threshHold, 255, THRESH_BINARY);
vector<Vec2f> lines;
vector<Vec2f> filteredLines;
HoughLines(workingCopy, lines, 2, CV_PI / 60, cvRound(125), 0, 0, CV_PI / 18, CV_PI * 17 / 18);
remove_copy_if(lines.begin(), lines.end(), std::back_inserter(filteredLines), VisionUtils::IsHorizontal);
map<float, vector<float> > groupedLines;
for (size_t i = 0; i < min(filteredLines.size(), maxLines); i++)
{
Vec2f line = filteredLines[i];
float roundedTheta = cvRound(line[1] * 100) / 100.0f;
groupedLines[roundedTheta].push_back(line[0]);
}
int i = 0;
for (map<float, vector<float> >::iterator it = groupedLines.begin(); it != groupedLines.end(); ++it)
{
i++;
float rho = it->second.front(), theta = it->first;
float p = exp2f(
-powf(rho - currentVanishingPoint.x * cos(theta) - currentVanishingPoint.y * sin(theta), 2) / 20000);
float a = cos(theta);
float b = sin(theta);
float H = 10.0f / (i + 1);
A += p * H * a * a;
B += p * H * b * b;
C += p * H * a * b;
D += p * H * a * rho;
E += p * H * b * rho;
}
Mat L = Mat_<float>(2, 2);
L.at<float>(0) = A;
L.at<float>(1) = C;
L.at<float>(2) = C;
L.at<float>(3) = B;
Mat R = Mat_<float>(2, 1);
R.at<float>(0) = D;
R.at<float>(1) = E;
Mat X;
if (solve(L, R, X, DECOMP_CHOLESKY))
{
Point2f newVanishingPoint(X.at<float>(0), X.at<float>(1));
if (GeneralUtils::Equals(newVanishingPoint, currentVanishingPoint, 5))
{
stabilizationCounter++;
stabilized = stabilizationCounter >= 10;
}
else
{
stabilizationCounter = 0;
}
currentVanishingPoint = newVanishingPoint;
}
return currentVanishingPoint;
}
///////////////////////////////////////////////////////////////////
// Panel::CannyDetection()
// Description: This function is called by DetectEdges() and it
// is the Canny edge detection function which does not contain
// debugging statements. We run the image through several different
// image processing functions to prepare the image before edge
// detection. After detection the edges we run Hough lines which
// approximates lines of minimum length as specified in
// Settings.xml. We find all intersections of the Hough lines
// then make a minimum area rectangle around the intersections to
// approximate the edges of the panel which we are trying to
// measure. From there we use the unit conversion calculated
// in DetectFeatures() to find a length and width of the current
// panel. We report the length and width with a message box.
///////////////////////////////////////////////////////////////////
Mat Panel::CannyDetection(Mat image, bool showImg)
{
Mat greyImage;
cvtColor(image, greyImage, CV_BGR2GRAY);
Mat eroded, dilated, thresh, blurredThresh, edges, edgesGray;
vector<Vec2f> lines;
threshold(greyImage, thresh, m_lowCannyThreshold, 255, THRESH_BINARY);
erode(thresh, eroded, Mat());
dilate(eroded, dilated, Mat());
GaussianBlur(thresh, blurredThresh, Size(7, 7), m_sigmaX, m_sigmaY);
Canny(blurredThresh, edges, m_cannyLow, m_cannyLow*m_ratio, 3);
HoughLines(edges, lines, 1, CV_PI / 180, m_houghLength, 0, 0);
cvtColor(edges, edgesGray, CV_GRAY2BGR);
for (size_t i = 0; i < lines.size(); i++)
{
float rho = lines[i][0], theta = lines[i][1];
Point pt1, pt2;
double a = cos(theta), b = sin(theta);
double x0 = a*rho, y0 = b*rho;
pt1.x = cvRound(x0 + 1000 * (-b));
pt1.y = cvRound(y0 + 1000 * (a));
pt2.x = cvRound(x0 - 1000 * (-b));
pt2.y = cvRound(y0 - 1000 * (a));
line(edgesGray, pt1, pt2, Scalar(0, 0, 255), 3, CV_AA);
}
////////////////////////////////////////////////////////
// Compute the intersection from the lines detected
////////////////////////////////////////////////////////
vector<Point2f> intersections;
for (size_t i = 0; i < lines.size(); i++)
{
for (size_t j = 0; j < lines.size(); j++)
{
Vec2f line1 = lines[i];
Vec2f line2 = lines[j];
if (acceptLinePair(line1, line2, (float)CV_PI / 32))
{
Point2f intersection = computeIntersect(line1, line2);
if (intersection.x >= 0 && intersection.y >= 0)
intersections.push_back(intersection);
}
}
}
if (intersections.size() > 0)
{
vector<Point2f>::iterator i;
for (i = intersections.begin(); i != intersections.end(); ++i)
{
cout << "Intersection is " << i->x << ", " << i->y << endl;
circle(image, *i, 2, Scalar(0, 255, 0), 3);
}
// Find the minimum bounding rectangle
RotatedRect rect;
Point2f rectPoints[4];
Scalar color = Scalar(255, 0, 0);
if (intersections.size() == 4)
{
// TODO
}
rect = minAreaRect(intersections);
rect.points(rectPoints);
int j = 0;
for (j; j < 4; j++)
line(image, rectPoints[j], rectPoints[(j + 1) % 4], color, 5, 8);
float topLength = (float)norm(rectPoints[1] - rectPoints[0]);
float botLength = (float)norm(rectPoints[3] - rectPoints[2]);
float panelWidthPixels = topLength < botLength ? topLength : botLength;
float leftHeight = (float)norm(rectPoints[3] - rectPoints[0]);
float rightHeight = (float)norm(rectPoints[2] - rectPoints[1]);
float panelHeightPixels = leftHeight < rightHeight ? leftHeight : rightHeight;
string dimensionDisplayPixels = "Pixels:\nWidth: " + to_string(panelWidthPixels) + " pixels\nHeight: " + to_string(panelHeightPixels) + " pixels";
// ShowMessage(dimensionDisplayPixels);
if (m_conversionRate)
{
float panelWidthReal = panelWidthPixels / m_conversionRate;
float panelHeightReal = panelHeightPixels / m_conversionRate;
string dimensionDisplayActual = "Actual:\nWidth: " + to_string(panelWidthReal) + " cm\nHeight: " + to_string(panelHeightReal) + " cm";
ShowMessage(dimensionDisplayActual);
}
}
if (showImg){
namedWindow("Intersections", CV_WINDOW_KEEPRATIO);
imshow("Intersections", image);
}
/////////////////////////////////////////////////////////////
// End of Computing the intersection from the lines detected
/////////////////////////////////////////////////////////////
return edges;
}
