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));
}
}
}
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);
}