Esempio n. 1
0
		template <typename Func> Plot2(Func func, float min = -10, float max = +10, const int cnt = 100) {
			splot.add(&mesh);
			//plot.add(&bar);
			float stepSize = (max-min) / cnt;

			struct XY {
				float x;
				float y;
			} xy;

			for (xy.y = min; xy.y <= max; xy.y+=stepSize) {
				for (xy.x = min; xy.x <= max; xy.x+=stepSize) {
					float z = func((float*)&xy);
					if (z < minZ) {minZ = z;}
					if (z > maxZ) {maxZ = z;}
					mesh.add(GnuplotPoint3(xy.x, xy.y, z));
				}
			}
		}
Esempio n. 2
0
		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);


		}