diff --git a/TestChangeCoord.h b/TestChangeCoord.h index 951bd8d..af85501 100644 --- a/TestChangeCoord.h +++ b/TestChangeCoord.h @@ -38,11 +38,96 @@ class TestChangeCoord : public CxxTest::TestSuite TS_ASSERT_EQUALS(p.y, 0); } - void testOnlyRotation(void) + void testOnlyRotation180(void) { Point origin = Point(0,0); Point p = map_point(M_PI, origin, Point(1,2)); TS_ASSERT_EQUALS(p.x, -1); TS_ASSERT_EQUALS(p.y, -2); } + + void testOnlyRotation90(void) + { + Point origin = Point(0,0); + Point p = map_point(M_PI/2, origin, Point(1,2)); + TS_ASSERT_EQUALS(p.x, 2); + TS_ASSERT_EQUALS(p.y, -1); + } + + void testOnlyRotation30(void) + { + Point origin = Point(0,0); + Point p = map_point(M_PI/6, origin, Point(200,0)); + TS_ASSERT_EQUALS(p.y, -100); + TS_ASSERT_LESS_THAN(p.x, 180); //0.866*2 ~= 1.73 + TS_ASSERT_LESS_THAN(170, p.x); //0.866*2 ~= 1.73 + } +}; + +class TestChangeBack : public CxxTest::TestSuite +{ + public: + void testOrigin(void) + { + Point origin = Point(1,2); + Point p = unmap_point(M_PI/2, origin, Point(0,0)); + TS_ASSERT_EQUALS(p.x, 1); + TS_ASSERT_EQUALS(p.y, 2); + } + + void testOnlyTranslation(void) + { + Point origin = Point(1,2); + Point p = unmap_point(0, origin, Point(-1,-2)); + TS_ASSERT_EQUALS(p.x, 0); + TS_ASSERT_EQUALS(p.y, 0); + } + + void testOnlyRotationOrigin(void) + { + Point origin = Point(0,0); + Point p = unmap_point(M_PI, origin, origin); + TS_ASSERT_EQUALS(p.x, 0); + TS_ASSERT_EQUALS(p.y, 0); + } + + void testOnlyRotation180(void) + { + Point origin = Point(0,0); + Point p = unmap_point(M_PI, origin, Point(1,2)); + TS_ASSERT_EQUALS(p.x, -1); + TS_ASSERT_EQUALS(p.y, -2); + } + + void testOnlyRotation90(void) + { + Point origin = Point(0,0); + Point p = unmap_point(M_PI/2, origin, Point(-2,1)); + TS_ASSERT_EQUALS(p.x, -1); + TS_ASSERT_EQUALS(p.y, -2); + } + + void testOnlyRotation30(void) + { + Point origin = Point(0,0); + Point p = unmap_point(M_PI/6, origin, Point(200,0)); + TS_ASSERT_LESS_THAN(p.x, 175); //0.866*2 ~= 1.73 + TS_ASSERT_LESS_THAN(170, p.x); //0.866*2 ~= 1.73 + TS_ASSERT_EQUALS(100, p.y); + } +}; + +class TestCoordSystem : public CxxTest::TestSuite +{ + public: + void testCreate(void) + { + auto cs = CoordinateSystem(Point(100,200), Point(200,300)); + TS_ASSERT_EQUALS(cs.map(Point(300,400)).y, 0); + TS_ASSERT_EQUALS(cs.map(Point(200,100)).x, 0); //moving orthogonally + } + void testInverse(void) + { + CoordinateSystem(Point(200,300), Point(100,200)); + } }; diff --git a/cvutils.cpp b/cvutils.cpp index 3c26fec..cb75f79 100644 --- a/cvutils.cpp +++ b/cvutils.cpp @@ -124,11 +124,9 @@ find_longest_line(std::vector hull, unsigned begin, unsigned end) //t for(unsigned i=(begin+2)%hull.size(); i!=end; i++) { assert(2<=thisline.size()); - std::cerr << "prima" << i << " hullsize " << hull.size() << std::endl; if(i==hull.size()) { i=0; } - std::cerr << "dopo " << i << " hullsize " << hull.size() << std::endl; assert(i < hull.size()); if(similar_fit(thisline, hull[i])) { thisline.push_back(hull[i]); diff --git a/geometry.cpp b/geometry.cpp index efd9a79..93df776 100644 --- a/geometry.cpp +++ b/geometry.cpp @@ -9,44 +9,87 @@ using namespace cv; #include "geometry.h" -double dist(cv::Point a, cv::Point b) +double dist(Point a, 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) +double inner_angle(Point a, Point middle, 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); + Point v1 = a - middle; + Point v2 = b - middle; + double v1_length = dist(Point(0, 0), v1); + double v2_length = dist(Point(0, 0), v2); return acos( (v1.x*v2.x + v1.y*v2.y) / (v1_length*v2_length) ); } -cv::Point -map_point(double theta, cv::Point origin, cv::Point tomap) +CoordinateSystem::CoordinateSystem(double angle, Point a) { + theta = angle; + origin = a; + precompute(); + assert(map(a) == Point(0,0)); + assert(unmap(Point(0,0)) == a); +} +CoordinateSystem::CoordinateSystem(Point a, Point b) +{ + theta = atan2(b.y-a.y, b.x-a.x); + origin = a; + precompute(); + assert(map(a) == Point(0,0)); + assert(unmap(Point(0,0)) == a); + assert(map(b).y == 0); + assert(map(b).x > 0); +} + +void CoordinateSystem::precompute() { + double costheta = cos(theta); + double sintheta = sin(theta); + R(0,0) = costheta; + R(0,1) = -sintheta; + R(1,0) = sintheta; + R(1,1) = costheta; + Rt(0,0) = costheta; + Rt(0,1) = sintheta; + Rt(1,0) = -sintheta; + Rt(1,1) = costheta; +} +Point CoordinateSystem::map(Point tomap) { //that's the algorithm: res = Rt*(tomap - origin) cv::Vec p_as_vector; - cv::Point p; - cv::Matx Rt; - + Point p; p = tomap - origin; p_as_vector[0] = p.x; p_as_vector[1] = p.y; - Rt(0,0) = cos(theta); - Rt(0,1) = sin(theta); - Rt(1,1) = Rt(0,0); - Rt(1,0) = -Rt(0,1); cv::Vec res_as_vector = Rt*p_as_vector; - return cv::Point(res_as_vector); + return Point(res_as_vector); +} +Point CoordinateSystem::unmap(Point tounmap) { + cv::Vec q_as_vector; + q_as_vector[0] = tounmap.x; + q_as_vector[1] = tounmap.y; + Point RQ = Point(R*q_as_vector); + return Point(RQ + origin); } +Point +map_point(double theta, Point origin, Point tomap) +{ + auto cs = CoordinateSystem(theta, origin); + return cs.map(tomap); +} + +Point +unmap_point(double theta, Point origin, Point tounmap) +{ + return CoordinateSystem(theta, origin).unmap(tounmap); +} + //that's the algorithm: res = Rt*(tomap - origin) cv::Vec get_line(Point p, Point q) { - Vec line; //the three positions are a,b,c as in: ax + by + c = 0 + cv::Vec line; //the three positions are a,b,c as in: ax + by + c = 0 Point vector = q-p; line(0) = - vector.y; line(1) = vector.x; @@ -61,3 +104,9 @@ is_in_line(cv::Vec line, Point p) { return line(0)*p.x + line(1)*p.y + line(2) == 0; } + +bool +is_above_line(cv::Vec line, Point p) +{ + return line(0)*p.x + line(1)*p.y + line(2) > 0; +} diff --git a/geometry.h b/geometry.h index 2d9bfc5..9eb6143 100644 --- a/geometry.h +++ b/geometry.h @@ -7,5 +7,22 @@ double dist(cv::Point, cv::Point); double inner_angle(cv::Point, cv::Point, cv::Point); cv::Point map_point(double, cv::Point, cv::Point); +cv::Point unmap_point(double, cv::Point, cv::Point); cv::Vec get_line(cv::Point, cv::Point); bool is_in_line(cv::Vec line, cv::Point p); +bool is_above_line(cv::Vec line, cv::Point p); + +class CoordinateSystem +{ + private: + double theta; + cv::Matx R, Rt; + cv::Point origin; + void precompute(); + public: + CoordinateSystem(cv::Point, cv::Point); + CoordinateSystem(double angle, cv::Point origin_); + cv::Point map(cv::Point p); + cv::Point unmap(cv::Point p); +}; + diff --git a/lines.cpp b/lines.cpp index d2954e7..7d00930 100644 --- a/lines.cpp +++ b/lines.cpp @@ -19,6 +19,8 @@ auto WHITE = cv::Scalar(255,255,255); auto BLUE = cv::Scalar(200,50,50); auto MAGENTA = cv::Scalar(110,10,200); auto BROWN = cv::Scalar(90,100,60); +auto BLACK = cv::Scalar(0,0,0); +auto YELLOW = cv::Scalar(20,200,200); int main(int argc, char *argv[]) @@ -107,6 +109,33 @@ int main(int argc, char *argv[]) cv::circle(img,p,dotwidth>>3,color,-1); } } +#endif + + // theta is the angle of the line connecting point 1 and 2; it will be the + // rotation of our new coordinate system + double theta = atan(((double)corn_2.y-corn_1.y) / ((double)corn_2.x-corn_1.x)); + auto cs = CoordinateSystem(corn_1, corn_2); + assert(cs.map(corn_1).x == 0); + assert(cs.map(corn_1).y == 0); + assert(cs.map(corn_2).x > 0); + assert(cs.map(corn_2).y == 0); + cv::Vec diag1, diag2; + diag1 = get_line(cs.map(corn_1), cs.map(corn_3)); + diag2 = get_line(cs.map(corn_4), cs.map(corn_2)); + cv::line(img, corn_1, corn_3, BLUE, dotwidth>>4); + cv::line(img, corn_2, corn_4, MAGENTA, dotwidth>>4); + std::cout << "mapped diag1: " << diag1 << std::endl; + std::cout << "mapped diag2: " << diag2 << std::endl; + + for(cv::Point p: contour) + { + cv::Point mapped = cs.map(p); + if(is_above_line(diag1, mapped) == is_above_line(diag2, mapped)) { + cv::circle(img, p, dotwidth>>3, YELLOW,-1); + } + } + +#ifdef _DEBUG cv::imshow("win",img); while(1) { @@ -116,14 +145,6 @@ int main(int argc, char *argv[]) img.release(); #endif - // theta is the angle of the line connecting point 1 and 2; it will be the - // rotation of our new coordinate system - double theta = atan(((double)corn_1.y - corn_2.y)/(corn_1.x - corn_2.x)); - std::cout << "Theta = " << theta << std::endl; - assert(map_point(theta, corn_1, corn_1).x == 0); - assert(map_point(theta, corn_1, corn_1).y == 0); - assert(map_point(theta, corn_1, corn_2).y == 0); - return EXIT_SUCCESS; }