// Optimal K is computed as avg_x(k-th-smallest_d(||I(x)-J(x+d)||)),
// where k = number_of_disparities*0.25.
static float
icvComputeK( CvStereoGCState* state )
{
int x, y, x1, d, i, j, rows = state->left->rows, cols = state->left->cols, n = 0;
int mind = state->minDisparity, nd = state->numberOfDisparities, maxd = mind + nd;
int k = MIN(MAX((nd + 2)/4, 3), nd);
int *arr = (int*)cvStackAlloc(k*sizeof(arr[0])), delta, t, sum = 0;
for( y = 0; y < rows; y++ )
{
const uchar* lptr = state->left->data.ptr + state->left->step*y;
const uchar* rptr = state->right->data.ptr + state->right->step*y;
for( x = 0; x < cols; x++ )
{
for( d = maxd-1, i = 0; d >= mind; d-- )
{
x1 = x - d;
if( (unsigned)x1 >= (unsigned)cols )
continue;
delta = icvDataCostFuncGraySubpix( lptr + x*3, rptr + x1*3 );
if( i < k )
arr[i++] = delta;
else
for( i = 0; i < k; i++ )
if( delta < arr[i] )
CV_SWAP( arr[i], delta, t );
}
delta = arr[0];
for( j = 1; j < i; j++ )
delta = MAX(delta, arr[j]);
sum += delta;
n++;
}
}
return (float)sum/n;
}
static int64 icvAlphaExpand(int64 Eprev, int alpha, CvStereoGCState* state, CvStereoGCState2* state2) {
GCVtx* var, *var1;
int64 E = 0;
int delta, E00 = 0, E0a = 0, Ea0 = 0, Eaa = 0;
int k, a, d, d1, x, y, x1, y1, rows = state->left->rows, cols = state->left->cols;
int nvtx = 0, nedges = 2;
GCVtx* vbuf = (GCVtx*)state->vtxBuf->data.ptr;
GCEdge* ebuf = (GCEdge*)state->edgeBuf->data.ptr;
int maxR = state2->interactionRadius;
const int* dtab = state2->dataCostFuncTab;
const int* stabR = state2->smoothnessR + CUTOFF;
const int* stabI = state2->smoothnessGrayDiff + 255;
const uchar* left0 = state->left->data.ptr;
const uchar* right0 = state->right->data.ptr;
short* dleft0 = state->dispLeft->data.s;
short* dright0 = state->dispRight->data.s;
GCVtx** pleft0 = (GCVtx**)state->ptrLeft->data.ptr;
GCVtx** pright0 = (GCVtx**)state->ptrRight->data.ptr;
int step = state->left->step;
int dstep = (int)(state->dispLeft->step / sizeof(short));
int pstep = (int)(state->ptrLeft->step / sizeof(GCVtx*));
int aa[] = { alpha, -alpha };
//double t = (double)cvGetTickCount();
assert(state->left->step == state->right->step &&
state->dispLeft->step == state->dispRight->step &&
state->ptrLeft->step == state->ptrRight->step);
for (k = 0; k < 2; k++) {
ebuf[k].dst = 0;
ebuf[k].next = 0;
ebuf[k].weight = 0;
}
for (y = 0; y < rows; y++) {
const uchar* left = left0 + step * y;
const uchar* right = right0 + step * y;
const short* dleft = dleft0 + dstep * y;
const short* dright = dright0 + dstep * y;
GCVtx** pleft = pleft0 + pstep * y;
GCVtx** pright = pright0 + pstep * y;
const uchar* lr[] = { left, right };
const short* dlr[] = { dleft, dright };
GCVtx** plr[] = { pleft, pright };
for (k = 0; k < 2; k++) {
a = aa[k];
for (y1 = y + (y > 0); y1 <= y + (y < rows - 1); y1++) {
const short* disp = (k == 0 ? dleft0 : dright0) + y1 * dstep;
GCVtx** ptr = (k == 0 ? pleft0 : pright0) + y1 * pstep;
for (x = 0; x < cols; x++) {
GCVtx* v = ptr[x] = &vbuf[nvtx++];
v->first = 0;
v->weight = disp[x] == (short)(OCCLUDED ? -OCCLUSION_PENALTY2 : 0);
}
}
}
for (x = 0; x < cols; x++) {
d = dleft[x];
x1 = x + d;
var = pleft[x];
// (left + x, right + x + d)
if (d != alpha && d != OCCLUDED && (unsigned)x1 < (unsigned)cols) {
var1 = pright[x1];
d1 = dright[x1];
if (d == -d1) {
assert(var1 != 0);
delta = IS_BLOCKED(alpha, d) ? INFINITY : 0;
//add inter edge
E += icvAddTerm(var, var1,
dtab[icvDataCostFuncGraySubpix(left + x*3, right + x1*3)],
delta, delta, 0, ebuf, nedges);
} else if (IS_BLOCKED(alpha, d)) {
E += icvAddTerm(var, var1, 0, INFINITY, 0, 0, ebuf, nedges);
}
}
// (left + x, right + x + alpha)
x1 = x + alpha;
if ((unsigned)x1 < (unsigned)cols) {
var1 = pright[x1];
d1 = dright[x1];
E0a = IS_BLOCKED(d, alpha) ? INFINITY : 0;
Ea0 = IS_BLOCKED(-d1, alpha) ? INFINITY : 0;
Eaa = dtab[icvDataCostFuncGraySubpix(left + x*3, right + x1*3)];
E += icvAddTerm(var, var1, 0, E0a, Ea0, Eaa, ebuf, nedges);
}
// smoothness
for (k = 0; k < 2; k++) {
GCVtx** p = plr[k];
const short* disp = dlr[k];
const uchar* img = lr[k] + x * 3;
int scale;
var = p[x];
d = disp[x];
a = aa[k];
if (x < cols - 1) {
var1 = p[x+1];
d1 = disp[x+1];
scale = stabI[img[0] - img[3]];
E0a = icvSmoothnessCostFunc(d, a, maxR, stabR, scale);
Ea0 = icvSmoothnessCostFunc(a, d1, maxR, stabR, scale);
E00 = icvSmoothnessCostFunc(d, d1, maxR, stabR, scale);
E += icvAddTerm(var, var1, E00, E0a, Ea0, 0, ebuf, nedges);
}
if (y < rows - 1) {
var1 = p[x+pstep];
d1 = disp[x+dstep];
scale = stabI[img[0] - img[step]];
E0a = icvSmoothnessCostFunc(d, a, maxR, stabR, scale);
Ea0 = icvSmoothnessCostFunc(a, d1, maxR, stabR, scale);
E00 = icvSmoothnessCostFunc(d, d1, maxR, stabR, scale);
E += icvAddTerm(var, var1, E00, E0a, Ea0, 0, ebuf, nedges);
}
}
// visibility term
if (d != OCCLUDED && IS_BLOCKED(alpha, -d)) {
x1 = x + d;
if ((unsigned)x1 < (unsigned)cols) {
if (d != -dleft[x1]) {
var1 = pleft[x1];
E += icvAddTerm(var, var1, 0, INFINITY, 0, 0, ebuf, nedges);
}
}
}
}
}
//t = (double)cvGetTickCount() - t;
ebuf[0].weight = ebuf[1].weight = 0;
E += icvGCMaxFlow(vbuf, nvtx, ebuf, state2->orphans, state2->maxOrphans);
if (E < Eprev) {
for (y = 0; y < rows; y++) {
short* dleft = dleft0 + dstep * y;
short* dright = dright0 + dstep * y;
GCVtx** pleft = pleft0 + pstep * y;
GCVtx** pright = pright0 + pstep * y;
for (x = 0; x < cols; x++) {
GCVtx* var = pleft[x];
if (var && var->parent && var->t) {
dleft[x] = (short)alpha;
}
var = pright[x];
if (var && var->parent && var->t) {
dright[x] = (short) - alpha;
}
}
}
}
return MIN(E, Eprev);
}
static int64 icvComputeEnergy(const CvStereoGCState* state, const CvStereoGCState2* state2,
bool allOccluded) {
int x, y, rows = state->left->rows, cols = state->left->cols;
int64 E = 0;
const int* dtab = state2->dataCostFuncTab;
int maxR = state2->interactionRadius;
const int* stabR = state2->smoothnessR + CUTOFF;
const int* stabI = state2->smoothnessGrayDiff + 255;
const uchar* left = state->left->data.ptr;
const uchar* right = state->right->data.ptr;
short* dleft = state->dispLeft->data.s;
short* dright = state->dispRight->data.s;
int step = state->left->step;
int dstep = (int)(state->dispLeft->step / sizeof(short));
assert(state->left->step == state->right->step &&
state->dispLeft->step == state->dispRight->step);
if (allOccluded) {
return (int64)OCCLUSION_PENALTY * rows * cols * 2;
}
for (y = 0; y < rows; y++, left += step, right += step, dleft += dstep, dright += dstep) {
for (x = 0; x < cols; x++) {
int d = dleft[x], x1, d1;
if (d == OCCLUDED) {
E += OCCLUSION_PENALTY;
} else {
x1 = x + d;
if ((unsigned)x1 >= (unsigned)cols) {
continue;
}
d1 = dright[x1];
if (d == -d1) {
E += dtab[icvDataCostFuncGraySubpix(left + x*3, right + x1*3)];
}
}
if (x < cols - 1) {
d1 = dleft[x+1];
E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+3]]);
}
if (y < rows - 1) {
d1 = dleft[x+dstep];
E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+step]]);
}
d = dright[x];
if (d == OCCLUDED) {
E += OCCLUSION_PENALTY;
}
if (x < cols - 1) {
d1 = dright[x+1];
E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+3]]);
}
if (y < rows - 1) {
d1 = dright[x+dstep];
E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+step]]);
}
assert(E >= 0);
}
}
return E;
}
