/// update cost with scanline optimization method: /// C1(p,d) + min(Cr(p-r,d), Cr(p-r, d+-1) + P1, min_k(Cr(p-r,k)+P2)) - min_k(Cr(p-r,k)) float* scanlineOptimization(const float* costs, const PARAMETERS& params) { const int h = params.h, w = params.w; const int dMin = params.dMin, dMax = params.dMax; int size = w*h*(dMax-dMin+1); float *costSO = new float[size]; std::fill(costSO, costSO+size, 0); // computes the 4 directionnal so costs // vertical downward scanlineOptimizationComputationV(costs, 0, 1, costSO, params); // vertical upward scanlineOptimizationComputationV(costs, h-1, -1, costSO, params); // horizontal leftward scanlineOptimizationComputationH(costs, 0, 1, costSO, params); // horizontal rightward scanlineOptimizationComputationH(costs, w-1, -1, costSO, params); return costSO; }
void StereoFlow::scanlineOptimization(const int direction) { cout << "\n\tscanline optimization. disRanges = " << disRanges ; cout << ", width = " << width << ", height = " << height << endl; activePicture = direction; int size = height * width * (disRanges + 1); float * costSO = new float[size]; if(costSO == nullptr) { cout << "failed to new cost-volume scaline optimization." << endl; exit(1); } std::fill(costSO, costSO+size, 0); float * costs = new float[size]; int r = disRanges + 1; int wr = width * r; for(int y = 0; y < height; ++y) { for(int x = 0; x < width; ++x) { for(int k = 0; k <= disRanges; ++k) { Mat tcost = (direction == 0) ? costVolumeL[k] : costVolumeR[k]; costs[y*wr+x*r+k] = tcost.at<float>(y,x); } } } // cout << "DEBUG: y = " << deY << ", x = " << deX << endl; //computes the 4 directional so costs: aggregate in order //vertical downward scanlineOptimizationComputationV(costs, 0, 1, costSO); //cout << "DEBUG: vertical downward: " << endl; //for(int k = 0; k <= disRanges; ++k) //{ // cout << costSO[deY*wr + deX*r + k] << ' '; //} //cout << endl; //vertical upward scanlineOptimizationComputationV(costs, height-1, -1, costSO); /*cout << "DEBUG: vertical upward: " << endl; for(int k = 0; k <= disRanges; ++k) { cout << costSO[deY*wr + deX*r + k] << ' '; } cout << endl;*/ //horizontal leftward scanlineOptimizationComputationH(costs, 0, 1, costSO); /*cout << "DEBUG: horizontal leftward: " << endl; for(int k = 0; k <= disRanges; ++k) { cout << costSO[deY*wr + deX*r + k] << ' '; } cout << endl;*/ //horizontal rightward scanlineOptimizationComputationH(costs, width-1,-1, costSO); //cout << "DEBUG: horizontal rightward: " << endl; //for(int k = 0; k <= disRanges; ++k) //{ // cout << costSO[deY*wr + deX*r + k] << ' '; //} //cout << endl; //cost2disparity cout << "\n\tcost to disparity." << endl; float * maxcosts = new float[width * height]; int * kdata = new int[width * height]; int * dispdata = new int[width * height]; //std::fill_n(maxcosts, width * height, std::numeric_limits<float>::min()); std::fill_n(maxcosts, width*height, numeric_limits<int>::min()*1.0f); std::fill_n(kdata, width * height, 0); std::fill_n(dispdata, width * height, minDisp); for(int y = 0; y < height; ++y) { for(int x = 0; x < width; ++x) { /* if(y == deY && x== deX) { cout << "DEBUG: find max cost .." << endl; }*/ for(int k = 0; k <= disRanges; ++k) { int d = DISP(maxDisp, minDisp, k); /* if(y == deY && x== deX) { cout << "k= " << k << ", d = " << d << ", costSO = " << costSO[y*wr+x*r+k] << endl; }*/ if(costSO[y*wr+x*r+k] > maxcosts[y*width+x]) { maxcosts[y*width+x] = costSO[y*wr+x*r+k]; dispdata[y*width+x] = d; kdata[y*width+x] = k; } } /* if(y == deY && x== deX) { cout << "maxcost = " << maxcosts[y*width+x] << ", disp = " << dispdata[y*width+x] << ", k = " << kdata[y*width+x] << endl; }*/ } } for(int y = 0; y < height; ++y) { uchar * pmsk = (direction==0) ? (uchar *)mskL.ptr<uchar>(y) : (uchar *)mskR.ptr<uchar>(y); _uint * pk = (direction==0) ? (_uint *)bestKL.ptr<_uint>(y) : (_uint *)bestKR.ptr<_uint>(y); float * pdsp = (direction==0) ? (float *)dispL.ptr<float>(y) : (float *)dispR.ptr<float>(y); float * pcost = (direction == 0) ? (float *)bestCostL.ptr<float>(y) : (float *)bestCostR.ptr<float>(y); float * pprior = (direction == 0) ? (float *)bestPriorL.ptr<float>(y) : (float *)bestPriorR.ptr<float>(y); for(int x = 0; x < width; ++x) { if(pmsk[x] == 0) continue; int idx = y * width + x; pk[x] = kdata[idx]; pdsp[x] = dispdata[idx]; pcost[x] = maxcosts[idx]; pprior[x] = 1; } } }