stabilisierung I

2 files changed, 89 insertions, 6 deletions

diff --git a/Makefile b/Makefile
index a8478d5..6a03720 100644
--- a/Makefile
+++ b/Makefile
@@ -1,4 +1,4 @@
 client2: client2.cpp lib.cpp
-	g++ client2.cpp lib.cpp `pkg-config --libs opencv` -lm  -o client2
+	g++ -g client2.cpp lib.cpp `pkg-config --libs opencv` -lm  -o client2
 client: client.c
 	gcc client.c -lX11 -lXi -lXmu -lglut -lGL -lGLU -lm  -o client
diff --git a/client2.cpp b/client2.cpp
index 1671ae0..8f68fbc 100644
--- a/client2.cpp
+++ b/client2.cpp
@@ -9,23 +9,106 @@
 #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;
 
-	
-	Mat dingens;
+	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')
 	{
-		navdata_t navdata;
-		drone.get(dingens, &navdata);
+		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);
 
-		imshow("dingens",dingens);
+		oldgray = gray.clone();
 	}
 	
 	return 0;