void perform() { ImageChannel imgDepth(desc.imgLeft.getWidth(), desc.imgLeft.getHeight()); imgDepth.ones(); CV::Gauss g(1.0); desc.imgLeft = g.filter(desc.imgLeft); desc.imgRight = g.filter(desc.imgRight); MatchingSAD sad(desc.imgLeft, desc.imgRight, 13); MatchingConvolution conv(desc.imgLeft, desc.imgRight, 9); Matching matcher; DataMatrix<SimpleFeatures::FeatureVec> fLeft = SimpleFeatures::getFeatures(desc.imgLeft, 25); DataMatrix<SimpleFeatures::FeatureVec> fRight = SimpleFeatures::getFeatures(desc.imgRight, 25); Gnuplot gp; GnuplotPlot p; GnuplotPlotElementLines errLine; p.add(&errLine); GnuplotSplot splot; GnuplotSplotElementPoints depthPlot; splot.add(&depthPlot); DataMatrix<std::vector<float>> hogLeft(desc.imgLeft.getWidth(), desc.imgRight.getHeight()); DataMatrix<std::vector<float>> hogRight(desc.imgLeft.getWidth(), desc.imgRight.getHeight()); HOG hog1(desc.imgLeft, 10); HOG hog2(desc.imgRight, 10); // for (int y = 0; y < desc.imgLeft.getHeight(); ++y) { // for (int x = 0; x < desc.imgLeft.getWidth(); ++x) { // std::vector<HOGGradient> gLeft = hog1.getGradients(desc.imgLeft, x, y); // std::vector<HOGGradient> gRight = hog2.getGradients(desc.imgRight, x, y); // hog.relativeToBestOne(gLeft); // hog.relativeToBestOne(gRight); // hog.normalize(gLeft); // hog.normalize(gRight); // hogLeft.set(x,y,hog.binify(gLeft, 8)); // hogRight.set(x,y,hog.binify(gRight, 8)); // } // } struct QueueElem { float err; Point2i p; QueueElem(const float err, const Point2i p) : err(err), p(p) {;} QueueElem() : err(99999999), p(0,0) {;} bool operator < (const QueueElem& o) const {return err > o.err;} }; // auto comp = [] (const QueueElem& a, const QueueElem& b) { // return a.err < b.err; // }; //#pragma omp parallel 0 for (int y = 0; y < desc.imgLeft.getHeight()-1; y+=2) { std::cout << y << std::endl; for (int x = 0; x < desc.imgLeft.getWidth()-1; x+=2) { const Point2i pL(x,y); // std::cout << "sigma: " << fLeft.get(pL.x, pL.y).sigma << std::endl; // std::cout << "asigma: " << fLeft.get(pL.x, pL.y).avgSigma << std::endl; // std::cout << "agradx: " << fLeft.get(pL.x, pL.y).avgGradX << std::endl; // std::cout << "agrady: " << fLeft.get(pL.x, pL.y).avgGradY << std::endl; const Eigen::Vector3d l = desc.fm.getEpilineRight(pL); const int x1 = 0; const int x2 = desc.imgLeft.getWidth(); const int y1 = -(l(0)*x1 + l(2)) / l(1); const int y2 = -(l(0)*x2 + l(2)) / l(1); BresenhamIter iter(x1,y1,x2,y2); std::priority_queue<QueueElem, std::vector<QueueElem>> queue; // follow the epipolar line while (iter.hasNext()) { const Point2i _pR = iter.next(); const int maxD = desc.imgLeft.getWidth() / 4; if(pL.getDistance(_pR) > maxD) {continue;} //if(pL.getDistance(pR) < 10) {continue;} //const int w = 0; for (int oy = -1; oy <= +1; ++oy) { // for (int ox = -w; ox <= +w; ++ox) { const Point2i pR = _pR + (Point2i(0, oy)); if (pR.x < 2 || pR.y < 2 || pR.x >= desc.imgRight.getWidth() - 2 || pR.y >= desc.imgRight.getHeight() - 2) {continue;} //float err = - fLeft.get(pL.x, pL.y).diff(fRight.get(pR.x, pR.y)); float err = sad.getError(pL, pR); //float err = -conv.getScore(pL, pR); //float err = HOG::getDistance( hogLeft.get(pL.x, pL.y), hogRight.get(pR.x, pR.y) ); //if (err < vMin) {vMin = err; pMin = pR;} queue.push(QueueElem(err, pR)); //errLine.add( GnuplotPoint2(pR.x, err) ); //std::cout << pR.x << "," << pR.y << std::endl; // } } } // gp.draw(p); // gp.flush(); // usleep(1000*1000); if (queue.size() > 10) { Point2i best = queue.top().p; int cnt = 8; Point2i sum(0,0); for (int i = 0; i < cnt; ++i) { sum += queue.top().p; queue.pop(); } sum /= cnt; //if (best.getDistance(sum) < 25) { const float depth = pL.getDistance(best); imgDepth.set(pL.x, pL.y, depth); imgDepth.set(pL.x+1, pL.y, depth); imgDepth.set(pL.x, pL.y+1, depth); imgDepth.set(pL.x+1, pL.y+1, depth); depthPlot.add(GnuplotPoint3(best.x, best.y, depth)); //} } // if (vMin != INFINITY ) { // const float depth = pL.getDistance(bestInRight.p); // imgDepth.set(pL.x, pL.y, depth); // imgDepth.set(pL.x+1, pL.y, depth); // imgDepth.set(pL.x, pL.y+1, depth); // imgDepth.set(pL.x+1, pL.y+1, depth); // } else { // int i = 0; (void) i; // } } } gp.draw(splot); gp.flush(); CV::Normalize::inplace(imgDepth); ImageFactory::writePNG("/tmp/depth.png", imgDepth); }
bool HogWrapper::load(XMLNode &root) { if (root.isEmpty()) { SVL_LOG(SVL_LOG_WARNING, "Haar Feature Extractor XML root is empty"); return false; } if (!root.getName()) { SVL_LOG(SVL_LOG_WARNING, "HogWrapper::load given XML file with root with no name."); return false; } if (strcmp(root.getName(), "svlFeatureExtractor") != 0) { SVL_LOG(SVL_LOG_WARNING, "Attempt to read HogWrapper feature extractor from XML node that is not a feature extractor. Expected \"svlFeatureExtractor\", received \""<<root.getName()); return false; } //todo-- add error checking _windowSize.width = atoi(root.getAttribute("width")); _windowSize.height = atoi(root.getAttribute("height")); SVL_LOG(SVL_LOG_MESSAGE, "HogWrapper loaded with size "<<_windowSize.width << "x" << _windowSize.height); _validChannel = atoi(root.getAttribute("validChannel")); cv::Size blockSize; blockSize.width = atoi(root.getAttribute("blockWidth")); blockSize.height = atoi(root.getAttribute("blockHeight")); cv::Size blockStride; blockStride.width = atoi(root.getAttribute("blockStrideX")); blockStride.height = atoi(root.getAttribute("blockStrideY")); int nbins = atoi(root.getAttribute("nbins")); int derivAperture = atoi(root.getAttribute("derivAperture")); double winSigma = atof(root.getAttribute("winSigma")); int histogramNormType = atoi(root.getAttribute("histogramNormType")); double L2HysThreshold = atoi(root.getAttribute("L2HysThreshold")); bool gammaCorrection = atoi(root.getAttribute("gammaCorrection")); cv::Size windowSize(_windowSize); cv::Size cellSize; cellSize.width = atoi(root.getAttribute("cellWidth")); cellSize.height = atoi(root.getAttribute("cellHeight")); _hog = new cv::HOGDescriptor(windowSize, blockSize, cellSize, blockStride, nbins, derivAperture, winSigma, histogramNormType, L2HysThreshold, gammaCorrection); _hog = new cv::HOGDescriptor(windowSize, blockSize, cellSize, blockStride, nbins); cv::HOGDescriptor hog2(windowSize, blockSize, cellSize, blockStride, nbins); assert(_hog); cv::HOGDescriptor hog(cv::Size(64,128), cv::Size(16,16), cv::Size(8,8), cv::Size(8,8), 9); /* char * hogp = (char *) & hog; char * _hogp = (char *) &(*_hog); cout << "Side by side: " << endl; for (unsigned i = 0; i < sizeof(cv::HOGDescriptor); i++) { cout << (int) hogp[i] << " " << (int) _hogp[i]; if ((int) hogp[i] != (int) _hogp[i]) cout << " !!!!!!!!"; cout << endl; } assert(hog.winSize == _hog->winSize); assert(hog.blockSize == _hog->blockSize); assert(hog.blockStride == _hog->blockStride); assert(hog.cellSize == _hog->cellSize); assert(hog.nbins == _hog->nbins); assert(hog.derivAperture == _hog->derivAperture); assert(hog.winSigma == _hog->winSigma); assert(hog.histogramNormType == _hog->histogramNormType); assert(hog.L2HysThreshold == _hog->L2HysThreshold); assert(hog.gammaCorrection == _hog->gammaCorrection); assert(hog.svmDetector.size() == _hog->svmDetector.size()); for (unsigned i = 0; i < hog.svmDetector.size(); i++) assert(hog.svmDetector[i] == _hog->svmDetector[i]); cout << "(right after construction)" << endl; */ return true; }