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