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