123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156 |
- #include <iostream>
- #ifdef HAS_OPENCV3
- #include <opencv2/core.hpp> //Any OPENCV3 code
- #else
- #include <opencv2/core/core.hpp> //Any Opencv2 code
- #endif
- #include "geometry.h"
- #include "cvutils.h"
- #define likely(x) __builtin_expect(!!(x), 1)
- #define unlikely(x) __builtin_expect(!!(x), 0)
- // 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
- unsigned* max2_distance(std::vector<cv::Point> hull)
- {
- #ifdef _DEBUG
- std::cout << "Hull = ";
- for(auto p: hull) {
- std::cout << p << " ";
- }
- std::cout << std::endl;
- #endif
- unsigned *idx = (unsigned*) calloc(2, sizeof(int));
- double distance[2] = {0};
- for( unsigned i=0; hull.size()>i; i++ )
- {
- cv::Point thispoint=hull[i];
- cv::Point nextpoint=hull[(i+1)%hull.size()];
- double d=dist(thispoint,nextpoint);
- if( d>distance[0] ) //the biggest till now
- {
- idx[1]=idx[0];
- idx[0]=i;
- distance[1]=distance[0];
- distance[0]=d;
- }
- else if( d>distance[1] ) // it is the second biggest till now
- {
- idx[1]=i;
- distance[1]=d;
- }
- }
- return idx;
- }
- /* 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) {
- return similar_fit(a, b, newpoint, 5);
- }
- 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( angle > (180-tolerance_degrees) && angle < (180+tolerance_degrees) ) {
- return true;
- }
- return false;
- }
- bool similar_fit(std::vector<cv::Point> group, cv::Point newpoint) {
- // TODO: it should perform a fit instead of just taking first and last
- assert(group.size()>=2);
- return similar_fit(group[0], group[group.size()-1], newpoint);
- }
- /* simplify_hull will group all the points of the hull in groups that fit
- * each group is a "line"
- */
- std::vector<std::vector<cv::Point>>
- simplify_hull(std::vector<cv::Point> hull) {
- return simplify_hull( hull, 0 );
- }
- std::vector<std::vector<cv::Point>>
- simplify_hull(std::vector<cv::Point> hull, double mindistance) {
- //each element is a group of point; a group of point is a vector of points that were
- //contigous and that "fit together"
- std::vector<std::vector<cv::Point>> pointgroups;
- std::vector<cv::Point> *group = new std::vector<cv::Point>();
- cv::Point last, current;
- group->push_back(hull[0]);
- group->push_back(hull[1]);
- for( unsigned i=2; hull.size()>i; i++ )
- {
- current = hull[i];
- last = (*group)[group->size()-1];
- if( similar_fit((*group)[0], last, hull[i]) )
- {
- group->push_back( hull[i] );
- }
- else
- {
- if( dist((*group)[0],last)>=mindistance )
- pointgroups.push_back(*group);
- group = new std::vector<cv::Point>();
- group->push_back(last);
- group->push_back(hull[i]);
- }
- }
- pointgroups.push_back(*group);
- //TODO: first and last might just be the same
- return pointgroups;
- }
- /* find_longest_line: walks along the hull starting at "begin" and ending at "end", and find the
- * longest (well, not strictl: it's a very simple eurhistics) group of points
- * that are aligned we don't care about the number of points, but about the
- * euclidean distance between the first and the last
- * arg hull: the whole convex hull
- * arg begin: index of the array to start from
- * arg end: index of the array to end. It can be lower than start, in which case the hull is used
- * "circularly"
- * returns: a vector of useful points. Just use the first and the last, do not expect that the points in
- * between are added to the vector
- */
- std::vector<cv::Point> //two points, actually
- find_longest_line(std::vector<cv::Point> hull, unsigned begin, unsigned end) //the hull can be open
- {
- std::vector<cv::Point> bestline, thisline;
- int bestdistance = 0;
- thisline.push_back(hull[(begin)%hull.size()]);
- thisline.push_back(hull[(begin+1)%hull.size()]);
- for(unsigned i=(begin+2)%hull.size(); i!=end; i++)
- {
- assert(2<=thisline.size());
- if(i==hull.size()) {
- i=0;
- }
- assert(i < hull.size());
- if(similar_fit(thisline, hull[i])) {
- thisline.push_back(hull[i]);
- } else { // considering if the just-finished line is the best
- double thisdistance = dist(thisline[0],thisline[thisline.size()-1]);
- if(thisdistance>bestdistance) {
- bestline = thisline;
- bestdistance = thisdistance;
- }
- thisline.clear();
- assert(bestline.size()>=2);
- thisline.push_back(hull[(i-1)%hull.size()]);
- thisline.push_back(hull[i]);
- };
- }
- double thisdistance = dist(thisline[0],thisline[thisline.size()-1]);
- if(thisdistance>bestdistance) { // this is relevant if the best line ends at the last point
- bestline = thisline;
- bestdistance = thisdistance;
- }
- assert(bestline.size()>=2);
- return bestline;
- }
|