/*
* Returns the vanishing point for a group of lines based on the intersections for each pair of lines.
* Lines with a higher angle difference in between them have a higher weight in the resulting point.
*/
Point RoadDetection::getVanishingPoint(vector<Line> lines, Size frameSize)
{
vector<Point> interPoints;
vector<double> interAngles;
double interAnglesSum = 0;
Point vanishPoint = Point(0, 0);
if (lines.size() <= 0)
{
return vanishPoint;
}
for (size_t i = 0; i < lines.size() - 1; i++)
{
for (size_t j = i + 1; j < lines.size(); j++)
{
Line l1 = lines[i];
Line l2 = lines[j];
Point l1p1 = l1.pt1;
Point l1p2 = l1.pt2;
Point l2p1 = l2.pt1;
Point l2p2 = l2.pt2;
Point interPoint = getLineIntersection(l1, l2);
if (interPoint.x > frameSize.width || interPoint.x <= 0 || interPoint.y > frameSize.height || interPoint.y <= 0)
{
continue;
}
double interAngle = abs(l1.angle - l2.angle);
interAnglesSum += interAngle;
interPoints.push_back(interPoint);
interAngles.push_back(interAngle);
}
}
if (interPoints.size() <= 0)
{
return vanishPoint;
}
for (size_t i = 0; i < interPoints.size(); i++)
{
Point interPoint = interPoints[i];
double interAngle = interAngles[i];
double weight = interAngle / interAnglesSum;
vanishPoint.x += cvRound(interPoint.x * weight);
vanishPoint.y += cvRound(interPoint.y * weight);
}
return vanishPoint;
}
cv::Point2f getVanishPoint(cv::Point2f v0, cv::Point2f v1, cv::Point2f v2, cv::Point2f v3)
{
cv::Point2f vp;
vp = getLineIntersection(v0, v1, v2, v3);
return vp;
}
/*
* General image processing. Returns the original image with objects drawn on top of it.
*/
Mat RoadDetection::processImage()
{
Mat rawFrame, tmpFrame, grayFrame, blurredFrame, contoursFrame, houghFrame, sectionFrame;
Point vanishingPoint;
vector<Line> houghLines, houghMainLines;
vector<Point> roadShape;
int sectionOffset;
// save a copy of the original frame
original.copyTo(rawFrame);
// smooth and remove noise
GaussianBlur(rawFrame, blurredFrame, Size(BLUR_KERNEL, BLUR_KERNEL), 0, 0);
// edge detection (canny, inverted)
Canny(blurredFrame, contoursFrame, CANNY_MIN_THRESH, CANNY_MAX_THRESH);
threshold(contoursFrame, contoursFrame, 128, 255, THRESH_BINARY);
// hough transform lines
houghLines = getHoughLines(contoursFrame, true);
// vanishing point
vanishingPoint = getVanishingPoint(houghLines, rawFrame.size());
sectionOffset = vanishingPoint.y;
// section frame (below vanishing point)
contoursFrame.copyTo(tmpFrame);
sectionFrame = tmpFrame(CvRect(0, vanishingPoint.y, contoursFrame.cols, contoursFrame.rows - vanishingPoint.y));
// re-apply hough transform to section frame
houghLines = getHoughLines(sectionFrame, true);
// shift lines downwards
houghLines = shiftLines(houghLines, sectionOffset);
houghLines = getFilteredLines(houghLines);
// best line matches
houghMainLines = getMainLines(houghLines);
if (houghMainLines.size() >= 2)
{
Point intersection = getLineIntersection(houghMainLines[0], houghMainLines[1]);
if (intersection.x > 0 && intersection.y >= 0)
{
vanishingPoint = intersection;
sectionOffset = intersection.y;
}
// get road shape
roadShape = getRoadShape(rawFrame, houghMainLines[0], houghMainLines[1], vanishingPoint);
}
// limit lines
houghLines = getLimitedLines(houghLines, sectionOffset);
houghMainLines = getLimitedLines(houghMainLines, sectionOffset);
// drawing process
drawLines(rawFrame, houghLines, Scalar(0, 0, 255), 2, 0);
drawLines(rawFrame, houghMainLines, Scalar(20, 125, 255), 2, 0);
drawRoadShape(rawFrame, roadShape, Scalar(20, 125, 255), 0.4);
drawCircle(rawFrame, vanishingPoint, Scalar(20, 125, 255), 15, -1, 0);
return rawFrame;
}
/*
* General videos processing. @TODO Make use of previous frames?
*/
Mat RoadDetection::processVideo(Mat rawFrame)
{
Mat originalFrame, tmpFrame, grayFrame, blurredFrame, contoursFrame, houghFrame, sectionFrame, drawingFrame;
Point vanishingPoint;
vector<Line> houghLines, houghMainLines;
vector<Point> roadShape;
vector<Rect> vehicles;
int sectionOffset;
// convert to grayscale
cvtColor(rawFrame, grayFrame, CV_BGR2GRAY);
equalizeHist(grayFrame, grayFrame);
// smooth and remove noise
GaussianBlur(grayFrame, blurredFrame, Size(BLUR_KERNEL, BLUR_KERNEL), 0);
// edge detection (canny, inverted)
Canny(blurredFrame, contoursFrame, CANNY_MIN_THRESH, CANNY_MAX_THRESH);
threshold(contoursFrame, contoursFrame, 128, 255, THRESH_BINARY);
// hough transform
houghLines = getHoughLines(contoursFrame, true);
// vanishing point
vanishingPoint = getVanishingPoint(houghLines, rawFrame.size());
sectionOffset = vanishingPoint.y;
// if we can't find a point, use the one from a previous frame
if (vanishingPoint.x == 0 && vanishingPoint.y == 0)
{
vanishingPoint = previousVanishingPoint;
}
// if we can, save it for future use
else
{
previousVanishingPoint = vanishingPoint;
}
// section frame (below vanishing point)
sectionFrame = contoursFrame(CvRect(0, sectionOffset, contoursFrame.cols, contoursFrame.rows - sectionOffset));
// re-apply hough transform to section frame
houghLines = getHoughLines(sectionFrame, true);
// shift lines downwards
houghLines = shiftLines(houghLines, sectionOffset);
// best line matches
houghMainLines = getMainLines(houghLines);
// update vanishing point according to the new section
if (houghMainLines.size() >= 2)
{
previousLines = houghMainLines;
}
else
{
houghMainLines = previousLines;
}
if (houghMainLines.size() >= 2)
{
Point intersection = getLineIntersection(houghMainLines[0], houghMainLines[1]);
if (intersection.x > 0 && intersection.x < rawFrame.cols && intersection.y > 0 && intersection.y < rawFrame.rows)
{
vanishingPoint = intersection;
sectionOffset = intersection.y;
}
// get road shape
roadShape = getRoadShape(rawFrame, houghMainLines[0], houghMainLines[1], vanishingPoint);
}
// limit lines
houghLines = getLimitedLines(houghLines, sectionOffset);
houghMainLines = getLimitedLines(houghMainLines, sectionOffset);
// drawing frame and vehicles
drawingFrame = rawFrame(CvRect(0, sectionOffset, contoursFrame.cols, contoursFrame.rows - sectionOffset));
// vehicles = getVehicles(drawingFrame);
// drawing process
drawLines(rawFrame, houghLines, Scalar(0, 0, 255), 2, 0);
drawLines(rawFrame, houghMainLines, Scalar(20, 125, 255), 2, 0);
drawRoadShape(rawFrame, roadShape, Scalar(20, 125, 255), 0.3);
drawCircle(rawFrame, vanishingPoint, Scalar(20, 125, 255), 15, -1, 0);
// drawRects(rawFrame, vehicles, Scalar(255, 0, 0), 1, sectionOffset);
return rawFrame;
}