panorama!

2 files changed, 156 insertions, 0 deletions

diff --git a/client2.cpp b/client2.cpp
index 8f68fbc..e09dbfc 100644
--- a/client2.cpp
+++ b/client2.cpp
@@ -8,6 +8,7 @@
 
 #include <opencv2/opencv.hpp>
 #include "lib.h"
+#include "ringbuf.h"
 
 #define PI 3.141593654
 
@@ -54,6 +55,15 @@ int main(int argc, const char** argv)
 	Mat frame, gray, oldgray;
 	Mat screencontent(real_canvas_height,real_canvas_width, CV_32FC3);
 
+	#define RINGBUF_SIZE 10
+	ModuloRingbuffer ringbuf_x(RINGBUF_SIZE, -virtual_canvas_width/2, virtual_canvas_width/2);
+	Ringbuffer ringbuf_y(RINGBUF_SIZE);
+	ModuloRingbuffer ringbuf_a(RINGBUF_SIZE, -180,180);
+	ModuloRingbuffer ringbuf_phi(RINGBUF_SIZE, -180,180);
+	ModuloRingbuffer ringbuf_psi(RINGBUF_SIZE, -180,180);
+	ModuloRingbuffer ringbuf_theta(RINGBUF_SIZE, -180,180);
+
+
 	for (int i=0; i<160;i++)
 	{
 		drone.get(frame, &navdata);
@@ -109,6 +119,26 @@ int main(int argc, const char** argv)
 		imshow("screencontent", screencontent);
 
 		oldgray = gray.clone();
+
+
+		ringbuf_x.put(total_x);
+		ringbuf_y.put(total_y);
+		ringbuf_a.put(total_rot);
+		ringbuf_phi.put(navdata.phi);
+		ringbuf_psi.put(navdata.psi);
+		ringbuf_theta.put(navdata.theta);
+
+		double xdiff = fixup_range( ringbuf_x.get() - px_per_deg*ringbuf_psi.get(), -virtual_canvas_width/2, virtual_canvas_width/2);
+		double ydiff = ringbuf_y.get() + px_per_deg*ringbuf_theta.get();
+		double adiff = fixup_angle(ringbuf_a.get() - (-ringbuf_phi.get()));
+
+		total_x = fixup_range(total_x - xdiff/20.0, -virtual_canvas_width/2, virtual_canvas_width/2);
+		total_y = total_y - ydiff/20.0;
+		total_rot = fixup_angle(total_rot - adiff/20.0);
+
+
+
+
 	}
 	
 	return 0;
diff --git a/ringbuf.h b/ringbuf.h
new file mode 100644
index 0000000..5756153
--- /dev/null
+++ b/ringbuf.h
@@ -0,0 +1,126 @@
+#include <math.h>
+
+class Ringbuffer
+{
+	public:
+
+	Ringbuffer(int size)
+	{
+		this->size = size;
+		idx = 0;
+		buf = new double[size];
+		for (int i=0; i<size; i++)
+			buf[i] = 0;
+		avg = 0.0;
+		avg_valid = false;
+	}
+
+	~Ringbuffer()
+	{
+		delete [] buf;
+	}
+
+	double get()
+	{
+		if (!avg_valid)
+		{
+			avg=0.0;
+			for (int i=0; i<size; i++)
+				avg += buf[i];
+			avg/=size;
+			avg_valid=true;
+		}
+
+		return avg;
+	}
+
+	void put(double val)
+	{
+		buf[idx] = val;
+		idx = (idx+1) % size;
+		avg_valid = false;
+	}
+
+	void set(double val)
+	{
+		for (int i=0; i<size; i++)
+			buf[i]=val;
+		avg = val;
+		avg_valid=true;
+	}
+
+	void add(double val)
+	{
+		for (int i=0; i<size; i++)
+			buf[i]+=val;
+		avg += val;
+	}
+
+	private:
+		double* buf;
+		int idx;
+		int size;
+		double avg;
+		bool avg_valid;
+};
+
+class ModuloRingbuffer
+{
+	private:
+		Ringbuffer* rb;
+		double low,upp,span;
+
+	public:
+	
+	ModuloRingbuffer(int size, double low, double upp)
+	{
+		rb = new Ringbuffer(size);
+		this->low=low;
+		this->upp=upp;
+		this->span=upp-low;
+		rb->set(_fixup_range(0.0));
+	}
+
+	~ModuloRingbuffer()
+	{
+		delete rb;
+	}
+
+	double _fixup_range(double val)
+	{
+		while (val < low) val+=span;
+		while (val>= upp) val-=span;
+		return val;
+	}
+
+	double get()
+	{
+		return rb->get();
+	}
+
+	void put(double val)
+	{
+		val=_fixup_range(val);
+
+		// direct way
+		double dist1 = val - get();
+		// over borders
+		double dist2 = dist1 + span;
+		double dist3 = dist1 - span;
+
+		if (fabs(dist1) <= fabs(dist2) && fabs(dist1) <= fabs(dist3))
+			rb->put(val);
+		else if (fabs(dist2) <= fabs(dist3))
+			rb->put(val+span);
+		else
+			rb->put(val-span);
+
+		while (get() < low) rb->add(span);
+		while (get()>= upp) rb->add(-span);
+	}
+
+	void set(double val)
+	{
+		rb->set(_fixup_range(val));
+	}
+};