avana/sagoma
2
1
Fork 1
Code Issues Pull requests Releases Wiki Activity

FIXED and tested coordinate system change

Browse source
This commit is contained in:
boyska 2017-01-12 22:03:11 +01:00
parent aa2d7ba622
commit d044199220
5 changed files with 198 additions and 28 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

87
TestChangeCoord.h
View file

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

2
cvutils.cpp
View file

@ -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]);

83
geometry.cpp
View file

@ -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;
}

17
geometry.h
View file

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

37
lines.cpp
View file

@ -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;
}