|
@@ -13,6 +13,15 @@ double dist(cv::Point a, cv::Point b)
|
|
|
{
|
|
|
return sqrt((double)pow(a.x-b.x,2)+pow(a.y-b.y,2));
|
|
|
}
|
|
|
+
|
|
|
+double inner_angle(cv::Point a, cv::Point middle, cv::Point b)
|
|
|
+{
|
|
|
+ cv::Point v1 = a - middle;
|
|
|
+ cv::Point v2 = b - middle;
|
|
|
+ double v1_length = dist(cv::Point(0, 0), v1);
|
|
|
+ double v2_length = dist(cv::Point(0, 0), v2);
|
|
|
+ return acos( (v1.x*v2.x + v1.y*v2.y) / (v1_length*v2_length) );
|
|
|
+}
|
|
|
// takes the two points that are more distant from the next, ie:
|
|
|
// i st distance(hull[i], hull[i+1]) is maximum and 2nd maximum
|
|
|
int* max2_distance(std::vector<cv::Point> hull)
|
|
@@ -51,16 +60,13 @@ int* max2_distance(std::vector<cv::Point> hull)
|
|
|
|
|
|
/* check if a,b,newpoint belong to the same line (with minor approximation) */
|
|
|
bool similar_fit(cv::Point a, cv::Point b, cv::Point newpoint) {
|
|
|
- if( newpoint.x==b.x && newpoint.y==b.y )
|
|
|
- return true;
|
|
|
- if( a.x==b.x || b.x==newpoint.x )
|
|
|
- return a.x==b.x && b.x==newpoint.x; // TODO: this is just too harsh, there is no tolerance!
|
|
|
+ return similar_fit(a, b, newpoint, 5);
|
|
|
+}
|
|
|
|
|
|
- double coeff1 = ((double)a.y-b.y) / (a.x-b.x);
|
|
|
- double coeff2 = ((double)b.y-newpoint.y) / (b.x-newpoint.x);
|
|
|
+bool similar_fit(cv::Point a, cv::Point b, cv::Point newpoint, float tolerance_degrees) {
|
|
|
+ double angle = inner_angle(a, b, newpoint) * 180 / M_PI;
|
|
|
|
|
|
- if( abs(std::max(coeff1,coeff2)/std::min(coeff1,coeff2)) < 2 ) {
|
|
|
- std::cout << "No fit " << b << newpoint << " " << coeff1 << " " << coeff2 << std::endl;
|
|
|
+ if( angle > (180-tolerance_degrees) && angle < (180+tolerance_degrees) ) {
|
|
|
return true;
|
|
|
}
|
|
|
return false;
|