yasarba71520/Line-Following-Robot
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

New angle calculation concept

Browse Source
This commit is contained in:
Tim Zeuner 2023-01-31 22:01:25 +01:00
parent d7fc54cfd0
commit be7b35f668
2 changed files with 62 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
AutonomousMode/Processing/processing.cpp
View File

@ -82,58 +82,68 @@ void Processing::calcAngles(FrameData &data, int imageColums, int imageRows, boo
if(data.contours.size() > 0) if(data.contours.size() > 0)
{ {
int angle = 0; int index;
int index; // get the most left/right contour
if(turnLeft){
// get the most left/right contour int leftmostEdge = imageColums;
int leftmostEdge = imageColums; int rightmostEdge = 0;
for (int i = 0; i < data.leftEdges.size(); i++) for (int i = 0; i < data.leftEdges.size(); i++)
{ {
int edge = data.leftEdges[i].x; int edge = data.leftEdges[i].x;
if (edge <= leftmostEdge) if (edge <= leftmostEdge && turnLeft)
{
leftmostEdge = edge;
index = i;
}
}
}else
{ {
int rightmostEdge = 0; leftmostEdge = edge;
index = i;
for (int i = 0; i < data.leftEdges.size(); i++)
{
int edge = data.leftEdges[i].x;
if (edge >= rightmostEdge)
{
rightmostEdge = edge;
index = i;
}
}
}
// Find the Top-Left and Buttom-Left Point of the Contour
std::vector<cv::Point> leftMostContour = data.contours[index];
int xleftButtom = imageColums;
int xleftTop = imageColums;
int yButtom = data.boundingBoxes[index].y;
int yTop = data.boundingBoxes[index].height;
for (int i = 0; i < leftMostContour.size(); i++){
if(leftMostContour[i].y == 0 && leftMostContour[i].x < xleftButtom)
{
xleftButtom = leftMostContour[i].x;
}else if(leftMostContour[i].y > yTop -4 && leftMostContour[i].x < xleftTop){
xleftTop = leftMostContour[i].x;
}
} }
else if(edge >= rightmostEdge && !turnLeft)
{
rightmostEdge = edge;
index = i;
}
}
// Find the Top-Left and Bottom-Left Point of the Contour
std::vector<cv::Point> leftMostContour = data.contours[index];
/*
Not going to dive into that part to fix it.
Trying to use centroid instead.
int xleftBottom = imageColums;
int xleftTop = imageColums;
int yBottom = data.boundingBoxes[index].y;
int yTop = data.boundingBoxes[index].height;
for (int i = 0; i < leftMostContour.size(); i++)
{
if(leftMostContour[i].y == 0 && leftMostContour[i].x < xleftBottom)
{
xleftBottom = leftMostContour[i].x;
}
else if(leftMostContour[i].y > yTop -4 && leftMostContour[i].x < xleftTop)
{
xleftTop = leftMostContour[i].x;
}
}
// calculate angle // calculate angle
int deltaX = abs(xleftButtom - xleftTop); int deltaX = abs(xleftBottom - xleftTop);
int deltaY = yTop; int deltaY = yTop;
angle = Calcs::calcAngle(deltaX, deltaY); angle = Calcs::calcAngle(deltaX, deltaY);
*/
//Get the contours center using moments:
cv::Moments moments=cv::moments(leftMostContour);
LFRPoint contourCenter(moments.m10/moments.m00, moments.m01/moments.m00);
LFRPoint imageCenter(double(imageColums)/2.0, double(imageRows)/2.0);
LFRPoint focusPoint(imageCenter.x, imageCenter.y+1000.0);
LFRVector a(imageCenter-focusPoint);
LFRVector b(contourCenter-focusPoint);
double angle = a.angle(b);
//Write to Data //Write to Data
data.angle = angle; data.angle = angle;

11
AutonomousMode/lfr.cpp
View File

@ -139,12 +139,13 @@ cv::Mat LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameDat
{ {
//Draw the Arrow for the check of the angle //Draw the Arrow for the check of the angle
int length = 100; int length = 100;
Point P1 = frameData.middlePoints[frameData.index];
Point P2;
P2.x = (int)round(P1.x + length * cos(frameData.angle * CV_PI / 180.0)); Point contourCenter = frameData.middlePoints[frameData.index];
P2.y = (int)round(P1.y + length * sin(frameData.angle * CV_PI / 180.0)); Point imageCenter = Point(originalImage.size().width/2.0, originalImage.size().height/2.0);
cv::arrowedLine(originalImage, P1, P2, Scalar(0,0,255), 2, 8); Point focalPoint = Point(imageCenter.x, imageCenter.y+1000.0);
cv::arrowedLine(originalImage, focalPoint, contourCenter, Scalar(0,0,255), 2, 8);
cv::arrowedLine(originalImage, focalPoint, imageCenter, Scalar(0,0,255), 2, 8);
} }
return originalImage; return originalImage;
} }