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