void show (const int iter, const float err, const float* val) { bar.clear(); bar.addSegment(GnuplotPoint2(*val, -5), GnuplotPoint2(*val, +5)); gp.draw(plot); gp.flush(); usleep(1000*250); }
void Widget::showGnuplot(){ Gnuplot gp; std::cout << "Press Ctrl-C to quit (closing gnuplot window doesn't quit)." << std::endl; gp << "set yrange [-1:1]\n"; const int N = 1000; std::vector<double> pts(N); double theta = 0; while(1) { for(int i=0; i<N; i++) { double alpha = (double(i)/N-0.5) * 10; pts[i] = sin(alpha*8.0 + theta) * exp(-alpha*alpha/2.0); } gp << "plot '-' binary" << gp.binFmt1d(pts, "array") << "with lines notitle\n"; gp.sendBinary1d(pts); gp.flush(); theta += 0.2; myMath::mysleep(100); } }
void show (const int iter, const float err, const float* val) { // bar.clear(); // bar.addSegment(GnuplotPoint2(*val, -5), GnuplotPoint2(*val, +5)); gp.draw(splot); const float x = val[0]; const float y = val[1]; gp << "set arrow 1 from " << x << "," << y << "," << minZ << " to " << x << "," << y << "," << maxZ << "\n"; gp.flush(); usleep(1000*250); }
void draw() { gp.draw(multiplot); gp.flush(); }
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); }