lenk- und strassenerkennungskram in klassen ausgelagert. funzt.

1 files changed, 53 insertions, 0 deletions

diff --git a/naive_steerer.cpp b/naive_steerer.cpp
new file mode 100644
index 0000000..9844664
--- /dev/null
+++ b/naive_steerer.cpp
@@ -0,0 +1,53 @@
+#include "naive_steerer.h"
+#include "util.h"
+
+using namespace std;
+
+NaiveSteerer::NaiveSteerer(int chx, int chy)
+{
+	set_crosshair(chx,chy);
+}
+
+void NaiveSteerer::set_crosshair(int chx, int chy)
+{
+	crosshair_x=chx;
+	crosshair_y=chy;
+}
+
+void NaiveSteerer::process_image(const Mat& img)
+{
+	int left_sum=0, right_sum=0;
+	for (int row = 0; row<img.rows; row++)
+	{
+		const uchar* data=img.ptr<uchar>(row);
+		
+		for (int col=0; col<img.cols;col++)
+		{
+			if (*data)
+			{
+				if (row<=crosshair_y)
+				{
+					if (col < crosshair_x)
+						left_sum++;
+					else
+						right_sum++;
+				}
+			}
+			
+			data+=img.step[1];
+		}
+	}
+	
+	steer = -4* flopow(  (((float)left_sum / (left_sum+right_sum))-0.5  )*2.0 , 1.6);
+}
+
+double NaiveSteerer::get_steer_data()
+{
+	return steer;
+}
+
+double NaiveSteerer::get_confidence()
+{
+	//return confidence;
+	return 1.0; // TODO
+}