FIXED and tested coordinate system change

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));
+        }
 };

cvutils.cpp

@@ -124,11 +124,9 @@ find_longest_line(std::vector<cv::Point> 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]);

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<double,2> p_as_vector;
-	cv::Point p;
-	cv::Matx<double,2,2> 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<double,2> 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<double,2> 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<double,3>
 get_line(Point p, Point q)
 {
-    Vec<double,3> line; //the three positions are a,b,c as in: ax + by + c = 0
+    cv::Vec<double,3> 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<double,3> line, Point p)
 {
     return line(0)*p.x + line(1)*p.y + line(2) == 0;
 }
+
+bool
+is_above_line(cv::Vec<double,3> line, Point p)
+{
+    return line(0)*p.x + line(1)*p.y + line(2) > 0;
+}

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<double,3> get_line(cv::Point, cv::Point);
 bool is_in_line(cv::Vec<double,3> line, cv::Point p);
+bool is_above_line(cv::Vec<double,3> line, cv::Point p);
+
+class CoordinateSystem
+{
+    private:
+        double theta;
+        cv::Matx<double,2,2> 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);
+};
+

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<double,3> 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;
 }