1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
|
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/socket.h>
#include <string.h>
#include <sys/un.h>
#include <time.h>
#include <opencv2/opencv.hpp>
#include "lib.h"
#define PI 3.141593654
using namespace cv;
#define SOCKETPATH "/home/flo/uds_socket"
const int width = 1280, height = 720;
float fixup_range(float a, float low, float upp)
{
float tot=upp-low;
while (a < low) a+=tot;
while (a>= upp) a-=tot;
return a;
}
float fixup_angle(float a)
{
return fixup_range(a,-180,180);
}
int main(int argc, const char** argv)
{
DroneConnection drone(SOCKETPATH);
navdata_t navdata;
float scale_factor = 0.2;
float diag = sqrt(1280*1280+720*720);
float px_per_deg = diag / 92.;
int virtual_canvas_width = 360. * px_per_deg;
int virtual_canvas_height = 90. * px_per_deg;
int real_canvas_extra_width = sqrt(1280*1280+720*720)*scale_factor/2 + 2;
int real_canvas_width = virtual_canvas_width * scale_factor + 2*real_canvas_extra_width;
int real_canvas_height = virtual_canvas_height * scale_factor;
int total_x = 100, total_y = 00;
float total_rot = 0.0;
Mat frame, gray, oldgray;
Mat screencontent(real_canvas_height,real_canvas_width, CV_32FC3);
for (int i=0; i<160;i++)
{
drone.get(frame, &navdata);
cvtColor(frame, oldgray, COLOR_BGR2GRAY);
}
while (waitKey(1) != 'x')
{
drone.get(frame, &navdata);
imshow("dingens",frame);
cvtColor(frame, gray, COLOR_BGR2GRAY);
Mat mat = estimateRigidTransform(gray, oldgray, false);
printf("_____ %i\n", mat.type());
float angle; int shift_x, shift_y;
if (mat.total() > 0)
{
angle = atan2(mat.at<double>(0,1), mat.at<double>(0,0)) / PI * 180.;
shift_x = mat.at<double>(0,2) - width/2 + (mat.at<double>(0,0)*width/2 + mat.at<double>(0,1)*height/2);
shift_y = mat.at<double>(1,2) - height/2 + (mat.at<double>(1,0)*width/2 + mat.at<double>(1,1)*height/2);
}
else
{
angle = 0;
shift_x = 0;
shift_y = 0;
printf("no mat!\n");
}
total_x += shift_x;
total_y += shift_y;
total_rot = fixup_angle(total_rot+angle);
printf("sh: %i\t%i\t%f\n", shift_x, shift_y, angle);
printf("tot: %i\t%i\t%f\n", total_x, total_y, total_rot);
Mat rotmat = getRotationMatrix2D(Point2f(width/2,height/2), total_rot, scale_factor);
printf("dingskram %i\n", rotmat.type());
rotmat.at<double>(0,2) += total_x*scale_factor - width/2 + real_canvas_width/2;
rotmat.at<double>(1,2) += total_y*scale_factor - height/2 + real_canvas_height/2;
warpAffine(frame, screencontent, rotmat, Size(real_canvas_width, real_canvas_height));
printf("%i/%i\n", screencontent.size().width, screencontent.size().height);
if (total_x > 0)
screencontent.colRange(0, (2*real_canvas_extra_width)) = screencontent.colRange( real_canvas_width - 2*real_canvas_extra_width, real_canvas_width);
else
screencontent.colRange( real_canvas_width - 2*real_canvas_extra_width, real_canvas_width) = screencontent.colRange(0, (2*real_canvas_extra_width));
imshow("screencontent", screencontent);
oldgray = gray.clone();
}
return 0;
}
|
