void BPFlow::FindOptimalSolution()
{
int WinLen = WinSize * 2 + 1;
int nStates = WinLen * WinLen;
for(int p = 0; p < Area; p++)
{
if (!pMask1[p]) continue;
T_message* belief = pBelief[p].data();
int index = nStates;
double Min = belief[index];
int x = p % Width;
int y = p / Width;
for (int i = 0; i < nStates; i++)
{
int fx = i % WinLen - WinSize;
int fy = i / WinLen - WinSize;
int nx = x + fx;
int ny = y + fy;
if (!InsideImage(nx,ny)) continue;
// inside the image but not in the mask
if (!pMask2[nx + ny * Width]) continue;
if (Min > belief[i])
{
Min = belief[i];
index = i;
}
}
pX[p] = index;
}
}
//------------------------------------------------------------------------------------------------
// function to compute data term
//------------------------------------------------------------------------------------------------
void BPFlow::ComputeDataTerm()
{
// allocate the buffer for data term
nTotalMatches=AllocateBuffer<PixelBuffer2D<T_message>,T_message>(pDataTerm,ptrDataTerm,pWinSize[0],pWinSize[1]);
T_message HistMin,HistMax;
double HistInterval;
double* pHistogramBuffer;
int nBins=20000;
int total=0; // total is the total number of plausible matches, used to normalize the histogram
pHistogramBuffer=new double[nBins];
memset(pHistogramBuffer,0,sizeof(double)*nBins);
HistMin= 32767;
HistMax=0;
//--------------------------------------------------------------------------------------------------
// step 1. the first sweep to compute the data term for the visible matches
//--------------------------------------------------------------------------------------------------
for(ptrdiff_t i=0;i<Height;i++) // index over y
for(ptrdiff_t j=0;j<Width;j++) // index over x
{
size_t index=i*Width+j;
int XWinLength=pWinSize[0][index]*2+1;
// loop over a local window
for(ptrdiff_t k=-pWinSize[1][index];k<=pWinSize[1][index];k++) // index over y
for(ptrdiff_t l=-pWinSize[0][index];l<=pWinSize[0][index];l++) // index over x
{
ptrdiff_t x=j+pOffset[0][index]+l;
ptrdiff_t y=i+pOffset[1][index]+k;
// if the point is outside the image boundary then continue
if(!InsideImage(x,y))
continue;
ptrdiff_t index2=y*Width2+x;
T_message foo=0;
for(int n=0;n<nChannels;n++)
foo+=abs(pIm1[index*nChannels+n]-pIm2[index2*nChannels+n]); // L1 norm
//#ifdef INTMESSAGE
// foo+=abs(pIm1[index*nChannels+n]-pIm2[index2*nChannels+n]); // L1 norm
//#else
// foo+=fabs(pIm1[index*nChannels+n]-pIm2[index2*nChannels+n]); // L1 norm
//#endif
pDataTerm[index][(k+pWinSize[1][index])*XWinLength+l+pWinSize[0][index]]=foo;
HistMin=__min(HistMin,foo);
HistMax=__max(HistMax,foo);
total++;
}
}
// compute the histogram info
HistInterval=(double)(HistMax-HistMin)/nBins;
//HistInterval/=21;
//--------------------------------------------------------------------------------------------------
// step 2. get the histogram of the matching
//--------------------------------------------------------------------------------------------------
for(ptrdiff_t i=0;i<Height;i++) // index over y
for(ptrdiff_t j=0;j<Width;j++) // index over x
{
size_t index=i*Width+j;
int XWinLength=pWinSize[0][index]*2+1;
// loop over a local window
for(ptrdiff_t k=-pWinSize[1][index];k<=pWinSize[1][index];k++) // index over y
for(ptrdiff_t l=-pWinSize[0][index];l<=pWinSize[0][index];l++) // index over x
{
ptrdiff_t x=j+pOffset[0][index]+l;
ptrdiff_t y=i+pOffset[1][index]+k;
// if the point is outside the image boundary then continue
if(!InsideImage(x,y))
continue;
int foo=__min(pDataTerm[index][(k+pWinSize[1][index])*XWinLength+l+pWinSize[0][index]]/HistInterval,nBins-1);
pHistogramBuffer[foo]++;
}
}
for(size_t i=0;i<nBins;i++) // normalize the histogram
pHistogramBuffer[i]/=total;
T_message DefaultMatchingScore;
double Prob=0;
for(size_t i=0;i<nBins;i++)
{
Prob+=pHistogramBuffer[i];
if(Prob>=0.5)//(double)Area/nTotalMatches) // find the matching score
{
DefaultMatchingScore=__max(i,1)*HistInterval+HistMin;
break;
}
}
//DefaultMatchingScore=__min(100*DefaultMatchingScore,HistMax/10);
if(IsDisplay)
#ifdef INTMESSAGE
printf("Min: %d, Default: %d, Max: %d\n",HistMin,DefaultMatchingScore,HistMax);
#else
printf("Min: %f, Default: %f, Max: %f\n",HistMin,DefaultMatchingScore,HistMax);
#endif
//DefaultMatchingScore=0.1;
//--------------------------------------------------------------------------------------------------
// step 3. assigning the default matching score to the outside matches
//--------------------------------------------------------------------------------------------------
for(ptrdiff_t i=0;i<Height;i++) // index over y
for(ptrdiff_t j=0;j<Width;j++) // index over x
{
size_t index=i*Width+j;
int XWinLength=pWinSize[0][index]*2+1;
// loop over a local window
for(ptrdiff_t k=-pWinSize[1][index];k<=pWinSize[1][index];k++) // index over y
for(ptrdiff_t l=-pWinSize[0][index];l<=pWinSize[0][index];l++) // index over x
{
ptrdiff_t x=j+pOffset[0][index]+l;
ptrdiff_t y=i+pOffset[1][index]+k;
int _ptr=(k+pWinSize[1][index])*XWinLength+l+pWinSize[0][index];
// if the point is outside the image boundary then continue
if(!InsideImage(x,y))
pDataTerm[index][_ptr]=DefaultMatchingScore;
else if (IsDataTermTruncated) // put truncaitons to the data term
pDataTerm[index][_ptr]=__min(pDataTerm[index][_ptr],DefaultMatchingScore);
}
}
delete pHistogramBuffer;
}
//------------------------------------------------------------------------------------------------
// update the message from (x0,y0) to the neighbors
// the encoding of the direction
// 2 |
// v
// 0 ------> <------- 1
// ^
// 3 |
//------------------------------------------------------------------------------------------------
void BPFlow::UpdateSpatialMessage(int x, int y, int direction)
{
// eliminate impossible messages
if (direction==0 && x==Width-1) return;
if (direction==1 && x==0) return;
if (direction==2 && y==Height-1) return;
if (direction==3 && y==0) return;
int p = y * Width + x;
int WinLen = 2 * WinSize + 1;
int nStates = WinLen * WinLen + 1;
int x1 = x, y1 = y; // get the destination
switch(direction)
{
case 0:
x1++;
break;
case 1:
x1--;
break;
case 2:
y1++;
break;
case 3:
y1--;
break;
}
int q = y1 * Width + x1;
if (!pMask1[q]) return;
// init message
T_message*& message = pSpatialMessage[q * nNeighbors + direction].data();
T_message* message_org;
message_org = new T_message[nStates];
// msg_org = dataTerm
memset(message_org, 0, sizeof(T_message) * nStates);
memcpy(message_org, pDataTerm[p].data(), sizeof(T_message) * (nStates - 1));
// add the range term
Add2Message(message_org, pRangeTerm[p].data(), nStates - 1);
// add spatial messages
// add m s->p where s != q
if(x > 0 && direction != 1 && pMask1[x - 1 + y * Width]) // add left to right
Add2Message(message_org, pSpatialMessage[p * nNeighbors].data(), nStates);
if(x < Width - 1 && direction != 0 && pMask1[x + 1 + y * Width]) // add right to left
Add2Message(message_org, pSpatialMessage[p * nNeighbors + 1].data(), nStates);
if(y > 0 && direction != 3 && pMask1[x + (y - 1) * Width]) // add top to down
Add2Message(message_org, pSpatialMessage[p * nNeighbors + 2].data(), nStates);
if(y < Height - 1 && direction != 2 && pMask1[x + (y + 1) * Width]) // add bottom to up
Add2Message(message_org, pSpatialMessage[p * nNeighbors + 3].data(), nStates);
// msg_org = h(fp)
// calculate min h(fp), fp is not empty
T_message Min = CStochastic::Min(nStates - 1, message_org);
// mahatton distance transformation
// suitable for no predict flow
int i, j, pInWin;
///* // forward pass
for (i = -WinSize; i <= WinSize; i++)
{
for (j = -WinSize; j <= WinSize; j++)
{
pInWin = i + WinSize + WinLen * (j + WinSize);
if (i - 1 >= -WinSize)
{
message_org[pInWin] = __min(message_org[pInWin - 1] + s, message_org[pInWin]);
}
if (j - 1 >= -WinSize)
{
message_org[pInWin] = __min(message_org[pInWin - WinLen] + s, message_org[pInWin]);
}
}
}
// backward pass
for (i = WinSize; i >= -WinSize; i--)
{
for (j = WinSize; j >= -WinSize; j--)
{
pInWin = i + WinSize + WinLen * (j + WinSize);
if (i + 1 <= WinSize)
{
message_org[pInWin] = __min(message_org[pInWin + 1] + s, message_org[pInWin]);
}
if (j + 1 <= WinSize)
{
message_org[pInWin] = __min(message_org[pInWin + WinLen] + s, message_org[pInWin]);
}
}
}
for (i = 0; i < nStates - 1; i++)
{
message_org[i] = __min(message_org[i], Min + d);
}
memcpy(message, message_org, sizeof(T_message) * nStates);
//*/
/*
// brute force method for calculating min(s * |f(p) - f(q)|, d)
T_message tmp;
int k, l, qInWin;
for (i = 0; i < nStates; i++)
message[i] = -1;
for (i = -WinSize; i <= WinSize; i++)
{
for (j = -WinSize; j <= WinSize; j++)
{
pInWin = i + WinSize + WinLen * (j + WinSize);
for (k = -WinSize; k <= WinSize; k++)
{
for (l = -WinSize; l <= WinSize; l++)
{
qInWin = k + WinSize + WinLen * (l + WinSize);
// use manhatton distance to metric distance between fp, fq
// min(s * (|u(p) - u(q)| + |v(p) - v(q)|), d)
tmp = __min((fabs(pOffset[0][p] + i - (k + pOffset[0][q])) + fabs(pOffset[1][p] + j - (l + pOffset[1][q]))) * s, d);
if (message[qInWin] == -1)
message[qInWin] = message_org[pInWin] + tmp;
else
message[qInWin] = __min(message[qInWin], message_org[pInWin] + tmp);
}
}
}
}
*/
T_message Max = 0;
T_message pEmpty = message_org[nStates - 1] + alphaD + alphaV;
message[nStates - 1] = __min(Min + alphaV, pEmpty);
for (i = 0; i < nStates - 1; i++)
if (Max < message[i]) Max = message[i];
// set all pixels out of mask message as maximum value
for (int w = -WinSize; w <= WinSize; w++)
{
for (int h = -WinSize; h <= WinSize; h++)
{
int nx = x1 + w;
int ny = y1 + h;
int l = w + WinSize + (h + WinSize) * WinLen;
message[l] = __min(message[l], pEmpty + pRangeTerm[p].data()[l]);
if (!InsideImage(nx, ny)) continue;
if (!pMask2[nx + ny * Width]) message[l] = Max + s * WinLen;
}
}
// normalize the message by subtracting the minimum value
Min = CStochastic::Min(nStates, message);
for(int l = 0; l < nStates; l++)
message[l] -= Min;
delete message_org;
}
