diff options
Diffstat (limited to 'Master/Computer Vision/Praktikum1')
| -rw-r--r-- | Master/Computer Vision/Praktikum1/StereoVision.cpp | 423 | ||||
| -rw-r--r-- | Master/Computer Vision/Praktikum1/StereoVision.h | 72 | ||||
| -rw-r--r-- | Master/Computer Vision/Praktikum1/frame.cpp | 409 |
3 files changed, 904 insertions, 0 deletions
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 <cmath>
+
+
+StereoVision::StereoVision(void)
+{
+}
+
+StereoVision::~StereoVision(void)
+{
+}
+std::vector<candidate*> StereoVision::findCorrespPointsPMF(BV::ImageRGB &leftScanline, BV::ImageRGB &rightScanline)
+{
+ std::vector<unsigned> leftEdges;
+ std::vector<unsigned> rightEdges;
+ StereoVision::findEdges(leftScanline,rightScanline,leftEdges,rightEdges);
+
+ std::list<candidatePair> candidatePairs;
+ std::vector<candidate*> candidates;
+ std::list<candidatePair>::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;lc<leftEdges.size();lc++) {
+ for(size_t rc = 0;rc<rightEdges.size();rc++) {
+ BV::RGBPixel leftImgPx = leftScanline.getPixel(leftEdges[lc],0);
+ BV::RGBPixel rightImgPx = rightScanline.getPixel(rightEdges[rc],0);
+ BV::RGBPixel leftImgPxBefore = leftScanline.getPixel(leftEdges[lc]-1,0);
+ BV::RGBPixel rightImgPxBefore = rightScanline.getPixel(rightEdges[rc]-1,0);
+ BV::RGBPixel leftImgPxAfter = leftScanline.getPixel(leftEdges[lc]+1,0);
+ BV::RGBPixel rightImgPxAfter = rightScanline.getPixel(rightEdges[rc]+1,0);
+
+ if (arePixelSimilar(leftImgPx,rightImgPx)
+ && arePixelSimilar(leftImgPxBefore,rightImgPxBefore) && arePixelSimilar(leftImgPxAfter,rightImgPxAfter) ) {
+ candidate* c = new candidate();
+ c->xLeft=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<candidate*>::iterator n=candidates.begin();n!=candidates.end();n++) {
+ for(std::vector<candidate*>::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<candidate*>::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<candidatePair*> 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<candidatePair*>::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<candidatePair*>::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<candidate*> res;
+ /*
+ for(std::vector<candidate*>::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<candidate*>::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<candidate*>::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<candidate*> StereoVision::findCorrespPointsPMF(BV::Image &leftScanline, BV::Image &rightScanline)
+{
+ std::vector<unsigned> leftEdges;
+ std::vector<unsigned> rightEdges;
+ StereoVision::findEdges(leftScanline,rightScanline,leftEdges,rightEdges);
+
+ std::list<candidatePair> candidatePairs;
+ std::vector<candidate*> candidates;
+ std::list<candidatePair>::iterator i;
+ double dgThreshold = 1.0;
+
+ // generate all candidate points (only x coordinates are relevant)
+ for(size_t lc = 0;lc<leftEdges.size();lc++) {
+ for(size_t rc = 0;rc<rightEdges.size();rc++) {
+ if(leftEdges[lc] == 0 || rightEdges[rc] == 0 || leftEdges[lc] == 255 || rightEdges[rc] == 255) {
+ continue;
+ }
+ int leftVal = leftScanline.getPixel(leftEdges[lc],0);
+ int rightVal = rightScanline.getPixel(rightEdges[rc],0);
+ if ( leftVal == rightVal )
+ {
+ candidate* c = new candidate();
+ c->xLeft=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<candidate*>::iterator n=candidates.begin();n!=candidates.end();n++) {
+ for(std::vector<candidate*>::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<candidate*>::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<candidatePair*> 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<candidatePair*>::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<candidatePair*>::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<candidate*> res;
+
+ for(std::vector<candidate*>::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<unsigned> &leftEdges, std::vector<unsigned>&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<width-1;i++) {
+ actPixel = scanlineLeft.getPixel(i,height);
+ if( !arePixelSimilar(prevLeftPixel,actPixel) )
+ {
+ leftEdges.push_back(i);
+ }
+ prevLeftPixel = actPixel;
+
+ actPixel = scanlineRight.getPixel(i,height);
+ if( !arePixelSimilar(prevRightPixel,actPixel) )
+ {
+ rightEdges.push_back(i);
+ }
+ prevRightPixel = actPixel;
+ }
+}
+
+void StereoVision::findEdges(BV::Image &scanlineLeft, BV::Image &scanlineRight, std::vector<unsigned> &leftEdges, std::vector<unsigned>&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<width;i++) {
+ actPixel = scanlineLeft.getPixel(i,height);
+ if( prevLeftPixel != actPixel )
+ {
+ if(prevLeftPixel == sky) {
+ leftEdges.push_back(i);
+ } else {
+ leftEdges.push_back(i-1);
+ }
+ }
+ prevLeftPixel = actPixel;
+
+ actPixel = scanlineRight.getPixel(i,height);
+ if( prevRightPixel != actPixel)
+ {
+ if(prevRightPixel == sky) {
+ rightEdges.push_back(i);
+ } else {
+ rightEdges.push_back(i-1);
+ }
+ }
+ prevRightPixel = actPixel;
+ }
+}
+
+
+double StereoVision::getCyclopDistanceEpipolar(const unsigned int &xAl, const unsigned int &xAr,const unsigned&xBl,const unsigned&xBr)
+{
+ long leftDisparity = xAl - xBl;
+ long rightDisparity = xAr - xBr;
+ return 0.5*(leftDisparity+rightDisparity);
+}
+long StereoVision::getDisparityDifference(const unsigned int &xAl, const unsigned int &xAr,const unsigned&xBl,const unsigned&xBr)
+{
+ long leftDisparity = xAl - xAr;
+ long rightDisparity = xBl - xBr;
+ return std::abs(rightDisparity-leftDisparity);
+}
+double StereoVision::getDisparityGradient(const unsigned int &xAl, const unsigned int &xAr,const unsigned&xBl,const unsigned&xBr)
+{
+ double cyclopDist = getCyclopDistanceEpipolar(xAl,xAr,xBl,xBr);
+ if(cyclopDist != 0.0) {
+ return (getDisparityDifference(xAl,xAr,xBl,xBr)/ std::abs(cyclopDist) );
+ } else {
+ return ERROR_DISP_GRADIENT;
+ }
+
+}
+bool StereoVision::arePixelSimilar(const BV::RGBPixel& left,const BV::RGBPixel& right) {
+ bool res;
+ res = (MAX_COLOR_DIFF >= 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 <vector>
+#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<candidate*> findCorrespPointsPMF(BV::ImageRGB&,BV::ImageRGB&);
+ static std::vector<candidate*> 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<unsigned>&,std::vector<unsigned>&);
+ static void findEdges(BV::Image&,BV::Image&,std::vector<unsigned>&,std::vector<unsigned>&);
+ 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 <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");
+ }
+}
+
+
|