Browse Source

New angle calculation concept

master
Tim Zeuner 1 year ago
parent
commit
be7b35f668
2 changed files with 61 additions and 50 deletions
  1. 55
    45
      AutonomousMode/Processing/processing.cpp
  2. 6
    5
      AutonomousMode/lfr.cpp

+ 55
- 45
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)
{
int angle = 0;
int index;
if(turnLeft){
// get the most left/right contour
int leftmostEdge = imageColums;
for (int i = 0; i < data.leftEdges.size(); i++)
{
int edge = data.leftEdges[i].x;
if (edge <= leftmostEdge)
{
leftmostEdge = edge;
index = i;
}
}
}else
int index;
// get the most left/right contour
int leftmostEdge = imageColums;
int rightmostEdge = 0;

for (int i = 0; i < data.leftEdges.size(); i++)
{
int edge = data.leftEdges[i].x;
if (edge <= leftmostEdge && turnLeft)
{
int rightmostEdge = 0;
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;
leftmostEdge = edge;
index = i;
}
else if(edge >= rightmostEdge && !turnLeft)
{
rightmostEdge = edge;
index = i;
}
}
// Find the Top-Left and Bottom-Left Point of the Contour
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;
}
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
int deltaX = abs(xleftButtom - xleftTop);
int deltaX = abs(xleftBottom - xleftTop);
int deltaY = yTop;
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
data.angle = angle;

+ 6
- 5
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
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));
P2.y = (int)round(P1.y + length * sin(frameData.angle * CV_PI / 180.0));
cv::arrowedLine(originalImage, P1, P2, Scalar(0,0,255), 2, 8);
Point contourCenter = frameData.middlePoints[frameData.index];
Point imageCenter = Point(originalImage.size().width/2.0, originalImage.size().height/2.0);
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;
}

Loading…
Cancel
Save