Hi everybody, I'm writing a program (a drone simulator) that does the following:
1) it creates a blank window with two orthogonal lines, to simulate a wall intersection; 2) it creates a RotatedRect, randomly placed and randomly tilted; 3) it translates this RotatedRect until it is close enough to the wall; 4) then it computes a 2D rotational matrix and applies it to each of the RotatedRect's corners, computed as 2D vectors with respect to the RotatedRect's center; the four corners are rotated and, if I draw a line between each corner and its following one, I get a rectangle with sides parallel to the walls; 5) a new RotatedRect is created from these new corners; 6) this new RotatedRect moves randomly and new data is extracted from it.
This code sometimes work and sometimes doesn't, and when it doesn't, it fails at step 5, with this error:
OpenCV Error: Assertion failed (abs(vecs[0].dot(vecs[1])) / (norm(vecs[0]) * norm(vecs[1])) <= FLT_EPSILON) in RotatedRect, file /home/wauro/opencv-3.1.0/modules/core/src/matrix.cpp, line 5462
terminate called after throwing an instance of 'cv::Exception' what(): /home/wauro/opencv-3.1.0/modules/core/src/matrix.cpp:5462: error: (-215) abs(vecs[0].dot(vecs[1])) / (norm(vecs[0]) * norm(vecs[1])) <= FLT_EPSILON in function RotatedRect
I know this kind of error is due to the failure of the RotatedRect's constructor:
RotatedRect (const Point2f &point1, const Point2f &point2, const Point2f &point3)
I've been searching on Google for three days, and I've attempted several ways to get over this error, but I can't figure it out. This is my code:
int height = 480;
int width = 640;
cv::Mat frame( height, width, CV_8UC1, cv::Scalar(0) );
int bound = 50;
cv::Point orig( 50, 50 );
cv::line( frame, orig, cv::Point( width, bound ), cv::Scalar( 255,255,255 ), 1, 8, 0 );
cv::line( frame, orig, cv::Point( bound, height ), cv::Scalar( 255,255,255 ), 1, 8, 0 );
std::srand(time(0));
cv::Point2f rect_center( (std::rand() % 490) + 2*bound, (std::rand() % 280) + 2*bound );
cv::RotatedRect r( rect_center, cv::Size2f(50, 100), std::rand()%360 );
cv::Rect brect = r.boundingRect();
cv::Mat framerect = frame.clone();
cv::Point2f vertices[4];
r.points(vertices);
for (int i = 0; i < 4; i++)
line(framerect, vertices[i], vertices[(i+1)%4], cv::Scalar(255,255,255));
cv::imshow( "frame", framerect );
cv::waitKey(0);
int distance = (int)cv::norm( r.boundingRect().tl() - orig );
int epsilon = 130;
int delta = 5;
//translation
while ( rect_center.x > epsilon && rect_center.y > epsilon )
{
cv::Point2f step;
std::srand( time(0) );
step.x = (std::rand())%10;
std::srand( time(0) );
step.y = (std::rand())%10;
rect_center -= step;
r.center = rect_center;
distance = (int)cv::norm( r.boundingRect().tl() - orig );
for (int i = 0; i < 4; i++)
line(framerect, vertices[i], vertices[(i+1)%4], cv::Scalar(255,255,255));
framerect = frame.clone();
cv::imshow( "frame", framerect );
cv::waitKey(100);
}
//coordinate system change
rect_center = r.center;
cv::Mat new_vertices[4];
for(int i = 0; i < 4; i++)
{
cv::Mat P( 2, 1, CV_32F );
P.at<float>(0, 0) = rect_center.x - vertices[i].x;
P.at<float>(1, 0) = rect_center.y - vertices[i].y;
new_vertices[i] = P;
P.release();
}
//I make the rotational matrix
float theta = r.angle;
float phi = -(theta)*CV_PI/180;
cv::Mat R(2, 2, CV_32F); // Matrice di rotazione 2x2
R.at<float>(0,0) = cos(phi);
R.at<float>(0,1) = -sin(phi);
R.at<float>(1,0) = sin(phi);
R.at<float>(1,1) = cos(phi);
//new vertices vectorial computation
for(int i = 0; i < 4; i++)
{
new_vertices[i] = R*new_vertices[i];
vertices[i].x = rect_center.x + new_vertices[i].at<float>(0, 0);
vertices[i].y = rect_center.y + new_vertices[i].at<float>(1, 0);
framerect = frame.clone();
}
for(int i = 0; i < 4; i++)
line(framerect, vertices[i], vertices[(i+1)%4], cv::Scalar(255,255,255));
cv::imshow( "frame", framerect );
cv::waitKey(0);
//Here comes the problem:
cv::RotatedRect* new_r = new cv::RotatedRect( vertices[0], vertices[1], vertices[2] );
//you can overlook this following part of code
new_r->points(vertices);
distance = (int)cv::norm( new_r->boundingRect().tl() - orig );
framerect = frame.clone();
while( distance > delta )
{
cv::Point2f step;
brect = new_r->boundingRect();
if ( brect.tl().x > bound && brect.tl().y > bound )
{
std::srand( time(0) );
step.x = (std::rand())%delta;
std::srand( time(0) );
step.y = (std::rand())%delta;
new_r->center -= step;
distance = (int)cv::norm( brect.tl() - orig );
framerect = frame.clone();
cv::rectangle( framerect, new_r->boundingRect(), cv::Scalar(255,255,255), 1, 8, 0 );
}
else if ( brect.tl().x > bound && brect.tl().y <= bound + delta )
{
std::srand( time(0) );
step.x = (std::rand())%delta;
step.y = 0;
new_r->center -= step;
distance = (int)cv::norm( brect.tl() - orig );
framerect = frame.clone();
cv::rectangle( framerect, new_r->boundingRect(), cv::Scalar(255,255,255), 1, 8, 0 );
}
else if ( brect.tl().x <= bound + delta && brect.tl().y > bound )
{
std::srand( time(0) );
step.x = 0;
step.y = (std::rand())%delta;
new_r->center -= step;
distance = (int)cv::norm( brect.tl() - orig );
framerect = frame.clone();
cv::rectangle( framerect, new_r->boundingRect(), cv::Scalar(255,255,255), 1, 8, 0 );
}
else
break;
cv::imshow( "frame", framerect );
cv::waitKey(200);
}
//cv::waitKey(0);
}
I've tried everything. I've tried to number the vertices, I've tried nested if-else statements. I've debugged it, and according to what I've found, the order 0-1-2 of the vertices in the constructor should work. Points 0 and 1 have the same x, and points 1 and 2 have the same y. This never changes. So why doesn't it work?
Can anybody please help me solve this? Thank you very much.