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