void optim(const float* costs, float* soCosts,
int x1, int x2, int y1, int y2, const PARAMETERS& params) {
const int w=params.w, h=params.h;
const int dMin = params.dMin, dMax = params.dMax;
float minSoCost = soCosts[0*h*w+x2*h+y2];
// find minimal previous so cost for a given column index
for(int d=1; d<=dMax-dMin; ++d){
if(minSoCost > soCosts[d*h*w+x2*h+y2]){
minSoCost = soCosts[d*h*w+x2*h+y2];
}
}
float minkCr = minSoCost;
for(int d=0; d<=dMax-dMin; ++d) {
// C1(p,d) - min_k(Cr(p-r,k))
float soCost = costs[d*h*w+x1*h+y1]-minkCr;
// compute P1 and P2 parameters for better scanline optimization
float P1, P2;
scanlineOptimizationParameters(x1, x2, y1, y2, d+dMin, P1, P2, params);
// compute min(Cr(p-r,d), Cr(p-r, d+-1) + P1, min_k(Cr(p-r,k)+P2))
minSoCost = minkCr + P2;
if(minSoCost > soCosts[d*h*w+x2*h+y2])
minSoCost = soCosts[d*h*w+x2*h+y2];
if(d != 0 && minSoCost > soCosts[(d-1)*h*w+x2*h+y2] + P1)
minSoCost = soCosts[(d-1)*h*w+x2*h+y2] + P1;
if(d != dMax - dMin && minSoCost > soCosts[(d+1)*h*w+x2*h+y2] + P1)
minSoCost = soCosts[(d+1)*h*w+x2*h+y2] + P1;
soCosts[d*h*w+x1*h+y1] = soCost + minSoCost;
}
}
//previoud pixel (x2,y2)
//current pixel (x1,y1)
void StereoFlow::optim(const float* costs, float* soCosts, int x1, int x2, int y1, int y2)
{
int r = disRanges + 1;
int wr = width * r;
int index2 = y2*wr + x2*r;
int index1 = y1*wr + x1*r;
//find maximal previous so cost for a given column index x2
//max_i(Lr(pr,i))
float maxSoCost = soCosts[index2+0];
for(int k = 0; k <= disRanges; ++k)
{
if(maxSoCost < soCosts[index2+k])
{
maxSoCost = soCosts[index2+k];
}
}
float maxkCr = maxSoCost;
for(int k = 0; k <= disRanges; ++k)
{
//C1(p,d) - max_k(Lr(p-r, k))
float soCost = costs[index1+k] - maxkCr;
//compute P1 and P2 parameters for better scanline optimization
float P1, P2;
int d = DISP(maxDisp, minDisp, k);
scanlineOptimizationParameters(x1,x2,y1,y2,d,P1,P2);
P1 = -P1;
P2 = -P2;
//compute max(Cr(p-r,d), max_k(Cr(p-r,k)+P2))
maxSoCost = maxkCr + P2;
if(maxSoCost < soCosts[index2 + k])
maxSoCost = soCosts[index2 + k];
//compute max( maxSoCost, Cr(p-r,d-1)+P1 )
if(k != 0 && (maxSoCost < soCosts[index2 + (k-1)] + P1) )
{
maxSoCost = soCosts[index2 + (k-1)] + P1;
}
//compute max( maxSoCost, Cr(p-r, d+1)+P1 )
if(k != disRanges && (maxSoCost < soCosts[index2 + (k+1)] + P1) )
{
maxSoCost = soCosts[index2 + (k+1)] + P1;
}
soCosts[index1 + k] = soCost + maxSoCost;
}
}
