From 33613a85afc4b1481367fbe92a17ee59c240250b Mon Sep 17 00:00:00 2001 From: Sven Eisenhauer Date: Fri, 10 Nov 2023 15:11:48 +0100 Subject: add new repo --- Master/Computer Vision/Praktikum1/StereoVision.cpp | 423 +++++++++++++++++++++ Master/Computer Vision/Praktikum1/StereoVision.h | 72 ++++ Master/Computer Vision/Praktikum1/frame.cpp | 409 ++++++++++++++++++++ 3 files changed, 904 insertions(+) create mode 100644 Master/Computer Vision/Praktikum1/StereoVision.cpp create mode 100644 Master/Computer Vision/Praktikum1/StereoVision.h create mode 100644 Master/Computer Vision/Praktikum1/frame.cpp (limited to 'Master/Computer Vision/Praktikum1') diff --git a/Master/Computer Vision/Praktikum1/StereoVision.cpp b/Master/Computer Vision/Praktikum1/StereoVision.cpp new file mode 100644 index 0000000..4e8c131 --- /dev/null +++ b/Master/Computer Vision/Praktikum1/StereoVision.cpp @@ -0,0 +1,423 @@ +#include "StereoVision.h" +#include + + +StereoVision::StereoVision(void) +{ +} + +StereoVision::~StereoVision(void) +{ +} +std::vector StereoVision::findCorrespPointsPMF(BV::ImageRGB &leftScanline, BV::ImageRGB &rightScanline) +{ + std::vector leftEdges; + std::vector rightEdges; + StereoVision::findEdges(leftScanline,rightScanline,leftEdges,rightEdges); + + std::list candidatePairs; + std::vector candidates; + std::list::iterator i; + double dgThreshold = 1.0; + + // generate all candidates of corresponding points (only x coordinates are relevant) + if(leftEdges.size() == 0 || rightEdges.size() == 0) { + return candidates; + } + + for(size_t lc = 0;lcxLeft=leftEdges[lc]; + c->xRight=rightEdges[rc]; + c->propapilityCounter = 0.0; + c->isAssigned = false; + candidates.push_back(c); + } + } + } + size_t numCandidates = candidates.size(); + if(numCandidates == 1) { + return candidates; + } + //long pairCounter = 0; + // generate candidate Pairs (each with each) + for(std::vector::iterator n=candidates.begin();n!=candidates.end();n++) { + for(std::vector::iterator m=n;m!=candidates.end();m++) { + double dispDist = std::abs(getDisparityDifference((*n)->xLeft,(*n)->xRight,(*m)->xLeft,(*m)->xRight)); + // check that candidates are not to far away from each other + if( dispDist < DISP_DIST_THRESHOLD) { + double dg = getDisparityGradient((*n)->xLeft,(*n)->xRight,(*m)->xLeft,(*m)->xRight); + if( (dg != ERROR_DISP_GRADIENT) && (dg <= MAX_DISP_GRADIENT) ) { + candidatePair cp; + cp.firstCand = *n; + cp.secondCand = *m; + cp.disparityGradient = dg; + cp.isAssigned = false; + cp.isDeleted = false; + cp.candidateDistance = dispDist; + candidatePairs.push_back(cp); + } + } + } + } + size_t numCandidatePairs = candidatePairs.size(); + bool allAssigned = false; + long loopCounter = 0; + do { + // for each candidate set propapilityCounter to 0 + for(std::vector::iterator n=candidates.begin();n!=candidates.end();n++) { + if(!(*n)->isAssigned) { + (*n)->propapilityCounter = 0.0; + } + } + // set propapility counters for all unassigned pairs + for(i = candidatePairs.begin(); i!=candidatePairs.end();i++) { + if(!i->isAssigned && !i->isDeleted) { + if(i->disparityGradient <= dgThreshold) { + // weight with reciprocal of distance + i->firstCand->propapilityCounter+=1/(i->candidateDistance); + i->secondCand->propapilityCounter+=1/(i->candidateDistance); + } + } + } + std::vector winnerPairs; + double maxPropability = 0; + // find first pair, which is not assigned and set this as max propability + for(i = candidatePairs.begin(); i!=candidatePairs.end();i++) { + if(!i->isAssigned && !i->isDeleted) { + winnerPairs.push_back(&(*i)); + maxPropability = i->firstCand->propapilityCounter + i->secondCand->propapilityCounter; + break; + } + } + // find the pair with greatest sum of both propability counters + for(i = candidatePairs.begin(); i!=candidatePairs.end();i++) { + if(!i->isAssigned && !i->isDeleted) { + double propapility = i->firstCand->propapilityCounter + i->secondCand->propapilityCounter; + if (maxPropability < propapility ) + { + winnerPairs.clear(); + winnerPairs.push_back(&(*i)); + maxPropability = propapility; + } else if(maxPropability == propapility) { + winnerPairs.push_back(&(*i)); + } + } + } + // set each winner pair to state assigned + size_t numWinPairs = winnerPairs.size(); + for(std::vector::iterator w = winnerPairs.begin();w!=winnerPairs.end();w++) { + (*w)->isAssigned = true; + (*w)->firstCand->isAssigned = true; + (*w)->secondCand->isAssigned = true; + } + // delete all candidate pairs, which contain one the candidates of the "winner" candidate pairs + for(i=candidatePairs.begin();i!=candidatePairs.end();i++) { + if(!i->isAssigned && !i->isDeleted) { + for(std::vector::iterator w = winnerPairs.begin();w!=winnerPairs.end();w++) { + if(! ( (*i) == *(*w) ) ) { + if( (*w)->hasCandidate(i->firstCand) || (*w)->hasCandidate(i->secondCand) ) { + i->isDeleted = true; + } + } + } + } + } + // check if one more iteration is needed + int numOfUnassignedPairs = 0; + for(i=candidatePairs.begin();i!=candidatePairs.end();i++) { + if(!i->isAssigned && !i->isDeleted) { + numOfUnassignedPairs++; + } + } + if(numOfUnassignedPairs == 0) { + allAssigned = true; + } + } while(!allAssigned); + + /* for debugging only + for(i=candidatePairs.begin();i!=candidatePairs.end();i++) { + if(i->isAssigned && (!i->firstCand->isAssigned || !i->secondCand->isAssigned)) { + int x = 7; + } + } + */ + std::vector res; + /* + for(std::vector::iterator ci = candidates.begin();ci!=candidates.end();ci++) { + if((*ci)->isAssigned) { + res.push_back(*ci); + } + }*/ + for(i=candidatePairs.begin();i!=candidatePairs.end();i++) { + if(i->isAssigned && !i->isDeleted) { + bool alreadyInResult = false; + for(std::vector::iterator ri = res.begin();ri!=res.end();ri++) { + if( (*ri) == i->firstCand ) { + alreadyInResult = true; + break; + } + } + if(!alreadyInResult) { + res.push_back(i->firstCand); + } + alreadyInResult = false; + for(std::vector::iterator ri = res.begin();ri!=res.end();ri++) { + if( (*ri) == i->secondCand ) { + alreadyInResult = true; + break; + } + } + if(!alreadyInResult) { + res.push_back(i->secondCand); + } + } + } + return res; +} + +std::vector StereoVision::findCorrespPointsPMF(BV::Image &leftScanline, BV::Image &rightScanline) +{ + std::vector leftEdges; + std::vector rightEdges; + StereoVision::findEdges(leftScanline,rightScanline,leftEdges,rightEdges); + + std::list candidatePairs; + std::vector candidates; + std::list::iterator i; + double dgThreshold = 1.0; + + // generate all candidate points (only x coordinates are relevant) + for(size_t lc = 0;lcxLeft=leftEdges[lc]; + c->xRight=rightEdges[rc]; + c->propapilityCounter = 0.0; + c->isAssigned = false; + candidates.push_back(c); + } + } + } + size_t numCandidates = candidates.size(); + + //long pairCounter = 0; + // generate candidate Pairs (each with each) + for(std::vector::iterator n=candidates.begin();n!=candidates.end();n++) { + for(std::vector::iterator m=n;m!=candidates.end();m++) { + double dispDist = std::abs(getCyclopDistanceEpipolar((*n)->xLeft,(*n)->xRight,(*m)->xLeft,(*m)->xRight)); + // check that candidates are not to far away from each other + if( dispDist < DISP_DIST_THRESHOLD) { + double dg = getDisparityGradient((*n)->xLeft,(*n)->xRight,(*m)->xLeft,(*m)->xRight); + if( (dg != ERROR_DISP_GRADIENT) && (dg < MAX_DISP_GRADIENT) ) { + candidatePair cp; + cp.firstCand = *n; + cp.secondCand = *m; + cp.disparityGradient = dg; + cp.isAssigned = false; + cp.isDeleted = false; + cp.candidateDistance = dispDist; + candidatePairs.push_back(cp); + } else { + continue; + } + } + } + } + size_t numCandidatePairs = candidatePairs.size(); + bool allAssigned = false; + long loopCounter = 0; + do { + // for each candidate set propapilityCounter to 0 + for(std::vector::iterator n=candidates.begin();n!=candidates.end();n++) { + if(!(*n)->isAssigned) { + (*n)->propapilityCounter = 0.0; + } + } + // set propapility counters for all unassigned pairs + for(i = candidatePairs.begin(); i!=candidatePairs.end();i++) { + if(!i->isAssigned && !i->isDeleted) { + if(i->disparityGradient < dgThreshold) { + // weight with reciprocal of distance + i->firstCand->propapilityCounter+=1.0; + i->secondCand->propapilityCounter+=1.0; + } + break; + } + } + std::vector winnerPairs; + double maxPropability = 0; + // find first pair, which is not assigned and set this as max propability + for(i = candidatePairs.begin(); i!=candidatePairs.end();i++) { + if(!i->isAssigned && !i->isDeleted) { + winnerPairs.push_back(&(*i)); + // weight with reciprocal of distance + maxPropability = (i->firstCand->propapilityCounter + i->secondCand->propapilityCounter) / (100*i->candidateDistance); + break; + } + } + // find the pair with greatest sum of both propability counters + for(i = candidatePairs.begin(); i!=candidatePairs.end();i++) { + if(!i->isAssigned && !i->isDeleted) { + // weight with reciprocal of distance + double propapility = (i->firstCand->propapilityCounter + i->secondCand->propapilityCounter) / (100*i->candidateDistance); + if (maxPropability < propapility ) + { + winnerPairs.clear(); + winnerPairs.push_back(&(*i)); + maxPropability = propapility; + } else if(maxPropability == propapility) { + winnerPairs.push_back(&(*i)); + } + } + } + // set each winner pair to state assigned + size_t numWinPairs = winnerPairs.size(); + for(std::vector::iterator w = winnerPairs.begin();w!=winnerPairs.end();w++) { + (*w)->isAssigned = true; + (*w)->firstCand->isAssigned = true; + (*w)->secondCand->isAssigned = true; + } + // delete all candidate pairs, which contain one the candidates of the "winner" candidate pairs + for(i=candidatePairs.begin();i!=candidatePairs.end();i++) { + if(!i->isAssigned && !i->isDeleted) { + for(std::vector::iterator w = winnerPairs.begin();w!=winnerPairs.end();w++) { + if(! ( (*i) == *(*w) ) ) { + if( (*w)->hasCandidate(i->firstCand) || (*w)->hasCandidate(i->secondCand) ) { + i->isDeleted = true; + } + } + } + } + } + // check if one more iteration is needed + int numOfUnassignedPairs = 0; + for(i=candidatePairs.begin();i!=candidatePairs.end();i++) { + if(!i->isAssigned && !i->isDeleted) { + numOfUnassignedPairs++; + } + } + if(numOfUnassignedPairs == 0) { + allAssigned = true; + } + } while(!allAssigned); + + /* for debugging only + for(i=candidatePairs.begin();i!=candidatePairs.end();i++) { + if(i->isAssigned && (!i->firstCand->isAssigned || !i->secondCand->isAssigned)) { + int x = 7; + } + } + */ + std::vector res; + + for(std::vector::iterator ci = candidates.begin();ci!=candidates.end();ci++) { + if((*ci)->isAssigned) { + res.push_back(*ci); + } + } + return res; +} + +void StereoVision::findEdges(BV::ImageRGB &scanlineLeft, BV::ImageRGB &scanlineRight, std::vector &leftEdges, std::vector&rightEdges) +{ + unsigned width = scanlineLeft.getWidth();; + unsigned height = 0; + BV::RGBPixel prevLeftPixel = scanlineLeft.getPixel(0,height); + BV::RGBPixel prevRightPixel = scanlineRight.getPixel(0,height); + BV::RGBPixel actPixel; + for(unsigned i=1;i &leftEdges, std::vector&rightEdges) +{ + unsigned width = scanlineLeft.getWidth();; + unsigned height = 0; + int prevLeftPixel = scanlineLeft.getPixel(0,height); + int prevRightPixel = scanlineRight.getPixel(0,height); + int actPixel; + int sky = 0; + for(unsigned i=1;i= std::abs(left.getRed() - right.getRed())) && (MAX_COLOR_DIFF >= std::abs(left.getGreen() - right.getGreen())) && (MAX_COLOR_DIFF >= std::abs(left.getBlue() - right.getBlue())); + return res; +} \ No newline at end of file diff --git a/Master/Computer Vision/Praktikum1/StereoVision.h b/Master/Computer Vision/Praktikum1/StereoVision.h new file mode 100644 index 0000000..1ccb96a --- /dev/null +++ b/Master/Computer Vision/Praktikum1/StereoVision.h @@ -0,0 +1,72 @@ +#pragma once + +#include +#include "abstring.h" +#include "bvimagergb.h" +#include "bvrgbpixel.h" +#include "bvfilters.h" +#include "vraibolabmodule.h" + +#define DISP_DIST_THRESHOLD 20 +#define MAX_DISP_GRADIENT 1.0 +#define ERROR_DISP_GRADIENT 1000.0 +#define MAX_COLOR_DIFF 5 +#define MAX_LUM_DIFF 10.0 + +struct candidate { + unsigned xLeft; + unsigned xRight; + double propapilityCounter; + bool isAssigned; + bool operator== (const candidate& other) const { + return (xLeft == other.xLeft && xRight == other.xRight); + } + bool operator== (const candidate* other) const { + return (xLeft == other->xLeft && xRight == other->xRight); + } +}; + +struct candidatePair { + candidate* firstCand; + candidate* secondCand; + double disparityGradient; + bool isAssigned; + bool isDeleted; + //long pairNum; + double candidateDistance; + bool operator== (const candidatePair& other) const { + return ( (firstCand == other.firstCand) && (secondCand == other.secondCand) ); + } + bool hasCandidate(const candidate& other) const { + return (*firstCand == other) || (*secondCand == other); + } + bool hasCandidate(const candidate* other) const { + return (*firstCand == *other) || (*secondCand == *other); + } + candidatePair() {} + candidatePair(const candidatePair& other) { + firstCand = other.firstCand; + secondCand = other.secondCand; + disparityGradient = other.disparityGradient; + isAssigned = other.isAssigned; + isDeleted = other.isDeleted; + //pairNum = other.pairNum; + candidateDistance = other.candidateDistance; + } +}; + +class StereoVision +{ +public: + StereoVision(void); + ~StereoVision(void); + static std::vector findCorrespPointsPMF(BV::ImageRGB&,BV::ImageRGB&); + static std::vector findCorrespPointsPMF(BV::Image&,BV::Image&); + static long getDisparityDifference(const unsigned&,const unsigned&,const unsigned&,const unsigned&); +private: + static void findEdges(BV::ImageRGB&,BV::ImageRGB&,std::vector&,std::vector&); + static void findEdges(BV::Image&,BV::Image&,std::vector&,std::vector&); + static double getCyclopDistanceEpipolar(const unsigned&,const unsigned&,const unsigned&,const unsigned&); + static double getDisparityGradient(const unsigned&,const unsigned&,const unsigned&,const unsigned&); + static bool arePixelSimilar(const BV::RGBPixel&,const BV::RGBPixel&); +}; 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 + +#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 input1; + Main::Parameter 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 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 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::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"); + } +} + + -- cgit v1.2.3