add new repoHEADmaster

1 files changed, 409 insertions, 0 deletions

diff --git a/Master/Computer Vision/Praktikum1/frame.cpp b/Master/Computer Vision/Praktikum1/frame.cpp
new file mode 100644
index 0000000..95f88cb
--- /dev/null
+++ b/Master/Computer Vision/Praktikum1/frame.cpp
@@ -0,0 +1,409 @@
+#include "frame.h"

+#include "parameterset.h"

+#include "utilimagergba.h"

+#include "utiltimeprobe.h"

+#include "vraibolabmodule.h"

+#include "StereoVision.h"

+#include <limits>

+

+#define LINE_LUMINANCE 198.0

+#define EYEDISTANCE 0.5

+#define EPIPOLAR_SCANLINE 128

+#define MIN_OBSTACLE_DIST 3.0

+

+AbImageComponent* leftGrayImageComponent = NULL;

+AbImageComponent* rightGrayImageComponent = NULL;

+AbWindow* leftGrayImgWin = NULL;

+AbWindow* rightGrayImgWin = NULL;

+

+bool doStop = false;

+

+// Register this command in the module factory, don't change this!

+bool Frame::m_registered = VRAIBOLabModule::getFactory()->registerPrimaryCommand(&create);

+

+// Constructor of this command.

+// The first parameter is the used CommandDescriptor, which will identify this Command.

+// The second parameter is the used CommandGUIDescriptor, which will specify the menue and the

+// displayed text. (for more information see WISP-Docu: CommandDescriptor, CommandGUIDescriptor)

+Frame::Frame():Command(new Main::CommandDescriptor("show the parcour-OpenGLscene","vrAiboFrame"),

+                       new Main::CommandGUIDescriptor(Main::CommandGUIDescriptor::ON_MENU,"VRAIBO Frame", "Show Scene Parcour"))

+{

+    // Illustrator is used to show the OPENGL scene

+    mp_Illustrator = NULL;

+   

+    // CloseObserver is used to shut down the Lab frame

+    mp_CloseObserver = NULL;

+

+    //UpdateObserver is used to observe the VRAIBO, to receive synchronized repaints

+    mp_UpdateObserver = NULL;

+

+}

+

+// Destructor of the class

+Frame::~Frame()

+{

+

+}

+

+// This method will be executed if the Command was called from the WISP console or WISP GUI.

+void Frame::execute()

+{

+

+    // if you want to use input parameters, you can do it like the following lines

+    // e.g to get the height and width for the used picture

+    // (for more information see WISP-Docu: Command, AbstractParameter)

+

+    /*Main::Parameter<int> input1;

+    Main::Parameter<int> input2;

+    if(!mp_Selected->getParameterList()->empty())

+    {

+        //solver solves casting problems

+        if(!m_Solver.solve(&input1,mp_Selected->getParameterList()->front()))

+        {

+            Main::Overseer::getInstance()->getOutputConsole()->appendText("Parameter failure");

+            return;

+        }

+

+        if(!m_Solver.solve(&input2,mp_Selected->getParameterList()->at(1)))

+        {

+            Main::Overseer::getInstance()->getOutputConsole()->appendText("Parameter failure");

+            return;

+        }

+        int height = *input1.getValue();

+        int width = *input2.getValue();

+    }*/

+

+    // The VRAibo class is the central interface to the virtual AIBO (neccessary to let the VRAIBO walk for example)

+    // (for more information see WISP-Docu: VRAibo)

+    VRAIBO::VRAibo* vraibo = VRAIBO::VRAibo::getInstance();

+

+    if(vraibo->isRunning())

+    {

+        // Example to use the console for outputs with a linebreak "\n".

+        // alternative, you can use Main::Overseer::getInstance()->getOutputConsole()->appendText("");

+        _Console()->appendText("already running \n");

+        return;

+    }

+    // Illustrator is used to show the OPENGL scene for example the Parcourillustrator or Parcelillustrator

+    //mp_Illustrator = new ParcelIllustrator();

+    mp_Illustrator = new ParcourIllustrator(); //->alternative illustrator

+

+    // activate the viewing of the opengl scenes ( four different camera positions )

+    // params are the used illustrator, height, width, eye distance to center of head

+    // (half distance between both eyes), start PositonX, startPositionY

+    // (for more information see WISP-Docu: VRAibo)

+    //vraibo->showViews(mp_Illustrator,256,256,0.5,-30,-55);

+    vraibo->showViews(mp_Illustrator,256,256,EYEDISTANCE,-30,-55);

+

+

+    mp_CloseObserver = new Frame::MyCloseObserver(this);

+    mp_UpdateObserver = new Frame::MyUpdateObserver(this,256,256);

+

+    //add observers to VRAIBO

+    vraibo->addCloseObserver(mp_CloseObserver);

+    vraibo->addUpdateObserver(mp_UpdateObserver);

+

+    _Console()->appendText("ready\n");

+

+    // Inform module about it's use (ensure correct unloading of the module) 

+    VRAIBOLabModule::setUsed(true);

+}

+

+// Creation method for this Command

+Main::Command* Frame::create()

+{

+    return new Frame();

+}

+

+

+// Constructor of MyCloseObserver

+Frame::MyCloseObserver::MyCloseObserver(Frame* scenes)

+:mp_Frame(scenes)

+{

+

+}

+

+// this method is used to shut down the Lab frame

+void Frame::MyCloseObserver::update()

+{

+   

+    VRAIBO::VRAibo::getInstance()->removeUpdateObserver(mp_Frame->mp_UpdateObserver);

+    VRAIBO::VRAibo::getInstance()->removeCloseObserver(this);

+    VRAIBOLabModule::setUsed(false);

+    if(VRAIBOLabModule::getFactory()->getModule() !=NULL )

+    {

+        delete mp_Frame->mp_Illustrator;

+        delete mp_Frame->mp_UpdateObserver;

+    }

+    delete this;

+}

+

+// Constructor of MyUpdateObserver

+Frame::MyUpdateObserver::MyUpdateObserver(Frame* scenes,int imageWidth,int imageHeight)

+:mp_Frame(scenes)

+{

+    // Get a workspacewindow from the Overseer to put elements on it, like pictures, buttons, labels etc..

+     // (for more information see Ab-Docu: AbWindow, WISP-Docu: Overseer)

+    mp_LeftWindow = Main::Overseer::getInstance()->getWorkspaceWindow("left image");

+    // Create an imageComponent, which can show a picture

+    // (for more information see Ab-Docu: AbImageComponent)

+    mp_LeftImage = new AbImageComponent();

+    // Set the layout for the window (usefull to arrange components)

+    // (for more information see Ab-Docu: AbBorderLayout)

+    mp_LeftWindow->setLayoutManager(new AbBorderLayout());

+    // Add the imageComponent to the window

+    mp_LeftWindow->add(mp_LeftImage);

+    // Set the dimensio of the window

+    AbDimension dim(0,0,imageWidth,imageHeight);

+    mp_LeftWindow->setDimension(dim);

+    // disable resizing of the window

+    mp_LeftWindow->setResizeable(false);

+    // Show the window

+    mp_LeftWindow->show();

+

+

+    mp_RightWindow = Main::Overseer::getInstance()->getWorkspaceWindow("right image");

+    mp_RightImage = new AbImageComponent();

+    mp_RightWindow->setLayoutManager(new AbBorderLayout());

+    mp_RightWindow->add(mp_RightImage);

+    mp_RightWindow->setResizeable(false);

+    mp_RightWindow->setDimension(dim);

+    mp_RightWindow->show();

+

+    leftGrayImgWin = Main::Overseer::getInstance()->getWorkspaceWindow("left gray image");

+    leftGrayImageComponent = new AbImageComponent();

+    leftGrayImgWin->setLayoutManager(new AbBorderLayout());

+    leftGrayImgWin->add(leftGrayImageComponent);

+    leftGrayImgWin->setResizeable(false);

+    leftGrayImgWin->setDimension(dim);

+    leftGrayImgWin->show();

+

+    rightGrayImgWin = Main::Overseer::getInstance()->getWorkspaceWindow("right gray image");

+    rightGrayImageComponent = new AbImageComponent();

+    rightGrayImgWin->setLayoutManager(new AbBorderLayout());

+    rightGrayImgWin->add(rightGrayImageComponent);

+    rightGrayImgWin->setResizeable(false);

+    rightGrayImgWin->setDimension(dim);

+    rightGrayImgWin->show();

+

+    // The VRAibo class is the central interface to the virtual AIBO (neccessary to let the VRAIBO walk for example)

+    // (for more information see WISP-Docu: VRAibo)

+    mp_VRAibo = VRAIBO::VRAibo::getInstance();

+}

+

+// destructor of MyUpdateObserver

+Frame::MyUpdateObserver::~MyUpdateObserver()

+{

+    //close all own windows

+    AbMemoryManaged::destroy(mp_LeftWindow);

+    AbMemoryManaged::destroy(mp_RightWindow);

+	AbMemoryManaged::destroy(leftGrayImgWin);

+    AbMemoryManaged::destroy(rightGrayImgWin);

+}

+

+

+// This method will return true, if a point of interest has been found.

+// image Left/Right is the picture of the actual position of VRAIBO. You can use this to analyse

+// your situation with different image algorithms or something else.

+// linePos is the Pixel, which will be used to calculate the next angle of rotation

+// (e.g. the center of the line, which you want to follow).

+bool Frame::MyUpdateObserver::evaluate(BV::ImageRGB& imageLeft,BV::ImageRGB& imageRight, double& linePos)

+{   

+	unsigned width = imageLeft.getWidth();

+	unsigned height = imageLeft.getHeight();

+	std::vector<candidate*> correspPoints;

+	BV::ImageRGB leftScanLine(width,1);

+	BV::ImageRGB rightScanLine(width,1);

+	for(unsigned x=0;x<256;x++) {

+		leftScanLine.setPixel(x,0,imageLeft.getPixel(x,EPIPOLAR_SCANLINE));

+		rightScanLine.setPixel(x,0,imageRight.getPixel(x,EPIPOLAR_SCANLINE));

+	}

+	std::vector<BV::RGBPixel> palette;

+	BV::RGBPixel red(255,0,0); palette.push_back(red);

+	BV::RGBPixel green(0,255,0); palette.push_back(green);

+	BV::RGBPixel blue(0,0,255); palette.push_back(blue);

+	BV::RGBPixel yellow(255,255,0); palette.push_back(yellow);

+	BV::RGBPixel magenta(255,0,255); palette.push_back(magenta);

+	BV::RGBPixel cyan(0,255,255); palette.push_back(cyan);

+	BV::RGBPixel white(255,255,255); palette.push_back(white);

+	BV::RGBPixel black(0,0,0); palette.push_back(black);

+	BV::RGBPixel gray(128,128,128); palette.push_back(gray);

+	BV::ImageRGB leftImg(imageLeft);

+	BV::ImageRGB rightImg(imageRight);

+	leftImg.drawLine(BV::Point(0,EPIPOLAR_SCANLINE),BV::Point(width-1,EPIPOLAR_SCANLINE),white);

+	rightImg.drawLine(BV::Point(0,EPIPOLAR_SCANLINE),BV::Point(width-1,EPIPOLAR_SCANLINE),white);

+	

+	correspPoints = StereoVision::findCorrespPointsPMF(leftScanLine,rightScanLine);

+	double focalLength = (width/2)/std::tan(((mp_VRAibo->getFovy()*PI)/180)/2);

+	unsigned n = 0;

+	for(std::vector<candidate*>::iterator i = correspPoints.begin();i!=correspPoints.end();i++) {

+		n++;

+		leftImg.drawCircle(BV::Point((*i)->xLeft,EPIPOLAR_SCANLINE),8,palette[n]);

+		rightImg.drawCircle(BV::Point((*i)->xRight,EPIPOLAR_SCANLINE),8,palette[n]);

+

+		double dispL = (*i)->xLeft+EYEDISTANCE;

+		double dispR = (*i)->xRight-EYEDISTANCE;

+

+		Util::Point3d realWorldPoint;

+		// X

+		realWorldPoint.m_Elements[0] = (-EYEDISTANCE*(dispR+dispL))/(dispR-dispL);

+		// Y

+		realWorldPoint.m_Elements[1] = 0;

+		// Z

+		realWorldPoint.m_Elements[2] = std::abs((2*EYEDISTANCE*focalLength)/(dispR-dispL));

+		_Console()->appendText(AbString::format("\nReal world Point P[%f , %f , %f]",realWorldPoint.m_Elements[0],realWorldPoint.m_Elements[1],realWorldPoint.m_Elements[2]));

+		if(realWorldPoint.m_Elements[2] <= MIN_OBSTACLE_DIST) {

+			leftImg.drawText("X",BV::Point((*i)->xLeft-8,EPIPOLAR_SCANLINE-8),palette[n]);

+			rightImg.drawText("X",BV::Point((*i)->xRight-8,EPIPOLAR_SCANLINE-8),palette[n]);

+			doStop = true;

+		}

+	}

+	//_Console()->appendText(AbString::format("\nFound %d corresponding edge point in scanline",correspPoints.size()));

+	leftGrayImageComponent->setNewImage(leftImg.getBufferPointer(),leftImg.getWidth(),leftImg.getHeight());

+	rightGrayImageComponent->setNewImage(rightImg.getBufferPointer(),rightImg.getWidth(),rightImg.getHeight());	

+	

+	/*

+	BV::ImageRGB farbbildHSV; 

+    BV::Filter2D::ColorSpaceTransformationFilters::filterConvertRGB2HSV(imageLeft, farbbildHSV);

+	BV::Image helligkeitbild;

+	BV::Filter2D::ImageFeatureExtractionFilters::filterExtractComponent(farbbildHSV, helligkeitbild, 2);

+	int h =  (int) (height * 0.75);

+	BV::RGBPixel test;

+	bool begin = true;

+	unsigned start = 0;

+	unsigned end = 0;

+	double linePosRight = -1.0;

+	double linePosLeft = -1.0;

+	double lum = -1.0;

+	for(unsigned x = 0; x < width; x++)

+	{

+		//TB: only for debug issues, the function call could be direct in the if below

+		test = helligkeitbild.getPixel(x,h);

+		//TB: get the right side of the red line

+		lum = test.getLuminance();

+		if( (2.0 >= std::abs(lum - LINE_LUMINANCE))&& begin == true)

+		{

+			start = x;

+			begin = false;

+		}

+		//TB: get the left side if the red line

+		else if( ( 2.0 < std::abs(lum - LINE_LUMINANCE) || x == width-1) && begin == false)

+		{

+			//TB: calc the middle of the red line and return this point

+			end = x-1;

+			//linePos = (start + end)*0.5;

+			//linePos = start + ((end - start)*0.5);

+			linePosLeft = start + ((end - start)*0.5);

+		}

+		

+	}

+	BV::Filter2D::ColorSpaceTransformationFilters::filterConvertRGB2HSV(imageRight, farbbildHSV);

+	BV::Filter2D::ImageFeatureExtractionFilters::filterExtractComponent(farbbildHSV, helligkeitbild, 2);

+	begin = true;

+	for(unsigned x = 0; x < width; x++)

+	{

+		//TB: only for debug issues, the function call could be direct in the if below

+		test = helligkeitbild.getPixel(x,h);

+		//TB: get the right side of the red line

+		lum = test.getLuminance();

+		if( (2.0 >= std::abs(lum - LINE_LUMINANCE) )&& begin == true)

+		{

+			start = x;

+			begin = false;

+		}

+		//TB: get the left side if the red line

+		else if( ( 2.0 < std::abs(lum - LINE_LUMINANCE) || x == width-1) && begin == false)

+		{

+			//TB: calc the middle of the red line and return this point

+			end = x-1;

+			//linePos = (start + end)*0.5;

+			//linePos = start + ((end - start)*0.5);

+			linePosRight = start + ((end - start)*0.5);

+		}

+		

+	}

+	if( (linePosLeft == -1.0) || (linePosRight == -1.0) ) {

+		return false;

+	} else {

+		// cyclop view...

+		linePos = (linePosLeft+linePosRight)/2;

+		return true;

+	}

+	*/

+    return false;

+}

+

+

+// This method will be invoked if the VRAIBO has synchronized all views again

+void Frame::MyUpdateObserver::update()

+{

+

+    // Get a rgb-images of VRAIBO's camera

+    unsigned char* tempLeft = mp_VRAibo->getLeftImage();

+    unsigned char* tempRight = mp_VRAibo->getRightImage();

+   

+    if(tempLeft == NULL || tempRight == NULL)

+    {

+        return;

+    }

+

+    // Set image for the imageComponents, which shows the Images

+    if(mp_LeftWindow != NULL)

+    {

+        mp_LeftImage->setNewImage(tempLeft, mp_VRAibo->getPictureWidth(), mp_VRAibo->getPictureHeight(), Ab::RGB24);

+    }

+

+    if(mp_RightWindow != NULL)

+    {

+        mp_RightImage->setNewImage(tempRight, mp_VRAibo->getPictureWidth(), mp_VRAibo->getPictureHeight(), Ab::RGB24);

+    }

+

+   

+    // Create a ImageRGBs for image processing and set buffer pointer for bvImages

+    BV::ImageRGB imageLeft;

+    imageLeft.setBufferPointer(mp_VRAibo->getPictureWidth(),mp_VRAibo->getPictureHeight(),tempLeft);

+   

+    BV::ImageRGB imageRight;

+    imageRight.setBufferPointer(mp_VRAibo->getPictureWidth(),mp_VRAibo->getPictureHeight(),tempRight);

+

+    // Set the evaluation return value "linePos" to a default value

+    double linePos = 0;

+    // Set the angle of rotation for AIBO to a default value

+    double walkAngle = 0;

+

+

+    if (evaluate(imageLeft,imageRight,linePos))//pos found

+    {

+        // Point of interest has been found

+        //TB: calc the angle for the rotation of the robo (see worksheet)

+        walkAngle = (mp_VRAibo->getFovy()/mp_VRAibo->getPictureWidth())*-(linePos-imageLeft.getWidth()*0.5);

+        // This is a example for a console output in which a variable

+        // and a string will be written to the console

+        // AbString::format() is used like printf()

+        // (for more information see Ab-Docu: AbString)

+        _Console()->appendText(AbString::format("aktuelle Linienposition: %f\n", linePos));

+        _Console()->appendText(AbString::format("aktueller Walkangle: %d\n", walkAngle));

+

+        //let the VRAIBO turn "walkAngle" degrees

+        mp_VRAibo->turn(walkAngle);

+        //let the VRAIBO walk 0.5

+        mp_VRAibo->walk(0.5);

+       

+    }

+    else

+    {

+        //TB: if no line was found walk forward...

+		if(!doStop) {

+			mp_VRAibo->walk(0.5);

+		} else {

+			_Console()->appendText("\nObstacle in front detected");

+			doStop = false;

+		}

+		

+        // Point of interest has not been found

+        // Search for point of interest

+        _Console()->appendText("\nNo Lineposition found");

+    }

+}

+

+