void BPFlow::FindOptimalSolutionSequential()
{
for(size_t plane=0;plane<2;plane++)
memset(ptrBelief[plane],0,sizeof(T_message)*nTotalBelifElements[plane]);
for(size_t i=0;i<Height;i++)
for(size_t j=0;j<Width;j++)
for(size_t k=0;k<2;k++)
{
size_t plane;
if(j%2==0)
plane=k;
else
plane=1-k;
size_t offset=i*Width+j;
int nStates=pWinSize[plane][offset]*2+1;
T_message* belief=pBelief[plane][offset].data();
// add range term
Add2Message(belief,pRangeTerm[plane][offset].data(),nStates);
if (k==0)
// add message from the dual layer
Add2Message(belief,pDualMessage[plane][offset].data(),nStates);
else
for(int l=0;l<nStates;l++)
{
if(plane==0) // if the current is horizontal plane
belief[l]+=pDataTerm[offset].data()[pX[offset*2+1]*nStates+l];
else // if the current is vertical plane
{
int nStates1=pWinSize[1-plane][offset]*2+1;
belief[l]+=pDataTerm[offset].data()[l*nStates1+pX[offset*2]];
}
}
if(j>0) // horizontal energy
for(int l=0;l<nStates;l++)
belief[l]+=__min((double)abs(l-pWinSize[plane][offset]+pOffset[plane][offset]-pX[(offset-1)*2+plane]+pWinSize[plane][offset-1]-pOffset[plane][offset+1])*s,d);
if(i>0) // vertical energy
for(int l=0;l<nStates;l++)
belief[l]+=__min((double)abs(l-pWinSize[plane][offset]+pOffset[plane][offset]-pX[(offset-Width)*2+plane]+pWinSize[plane][offset-Width]-pOffset[plane][offset-Width])*s,d);
if(j<Width-1)
Add2Message(belief,pSpatialMessage[plane][offset*nNeighbors+1].data(),nStates);
if(i<Height-1)
Add2Message(belief,pSpatialMessage[plane][offset*nNeighbors+3].data(),nStates);
// find the minimum
int index=0;
double Min=belief[0];
for(int l=1;l<nStates;l++)
if(Min>belief[l])
{
Min=belief[l];
index=l;
}
pX[offset*2+plane]=index;
}
}
//------------------------------------------------------------------------------------------------
// compute belief
//------------------------------------------------------------------------------------------------
void BPFlow::ComputeBelief()
{
for(size_t plane=0;plane<2;plane++)
{
memset(ptrBelief[plane],0,sizeof(T_message)*nTotalBelifElements[plane]);
for(size_t i=0;i<Height;i++)
for(size_t j=0;j<Width;j++)
{
size_t offset=i*Width+j;
T_message* belief=pBelief[plane][offset].data();
int nStates=pWinSize[plane][offset]*2+1;
// add range term
Add2Message(belief,pRangeTerm[plane][offset].data(),nStates);
// add message from the dual layer
Add2Message(belief,pDualMessage[plane][offset].data(),nStates);
if(j>0)
Add2Message(belief,pSpatialMessage[plane][offset*nNeighbors].data(),nStates);
if(j<Width-1)
Add2Message(belief,pSpatialMessage[plane][offset*nNeighbors+1].data(),nStates);
if(i>0)
Add2Message(belief,pSpatialMessage[plane][offset*nNeighbors+2].data(),nStates);
if(i<Height-1)
Add2Message(belief,pSpatialMessage[plane][offset*nNeighbors+3].data(),nStates);
}
}
}
//------------------------------------------------------------------------------------------------
// compute belief
//------------------------------------------------------------------------------------------------
void BPFlow::ComputeBelief()
{
memset(ptrBelief, 0, sizeof(T_message) * nTotalBeliefElements);
int nStates = (WinSize * 2 + 1) * (WinSize * 2 + 1) + 1;
for(int i = 0; i < Height; i++)
{
for (int j = 0; j < Width; j++)
{
int p = j + i * Width;
if (!pMask1[p]) continue;
T_message* belief = pBelief[p].data();
// add range term
Add2Message(belief, pRangeTerm[p].data(), nStates - 1);
// add data term
Add2Message(belief, pDataTerm[p].data(), nStates - 1);
belief[nStates - 1] += alphaD;
if(j>0 && pMask1[p - 1])
Add2Message(belief, pSpatialMessage[p * nNeighbors].data(), nStates);
if(j<Width-1 && pMask1[p + 1])
Add2Message(belief, pSpatialMessage[p * nNeighbors + 1].data(), nStates);
if(i>0 && pMask1[p - Width])
Add2Message(belief, pSpatialMessage[p * nNeighbors + 2].data(), nStates);
if(i<Height-1 && pMask1[p + Width])
Add2Message(belief, pSpatialMessage[p * nNeighbors + 3].data(), nStates);
}
}
}
//------------------------------------------------------------------------------------------------
// update dual message passing from one plane to the other
//------------------------------------------------------------------------------------------------
void BPFlow::UpdateDualMessage(int x, int y, int plane)
{
int offset=y*Width+x;
int offset1=offset;
int wsize=pWinSize[plane][offset];
int nStates=wsize*2+1;
int wsize1=pWinSize[1-plane][offset];
int nStates1=wsize1*2+1;
s=Im_s.data()[offset*2+plane];
d=Im_d.data()[offset*2+plane];
//s=m_s;
//d=m_d;
T_message* message_org;
message_org=new T_message[nStates];
//memset(message_org,0,sizeof(T_message)*nStates);
for(int i = 0;i<nStates;i++)
message_org[i] = 0;
// add the range term
if(!IsTRW)
Add2Message(message_org,pRangeTerm[plane][offset].data(),nStates);
else
Add2Message(message_org,pRangeTerm[plane][offset].data(),nStates,CTRW);
// add spatial messages
if(x>0) //add left to right
{
if(!IsTRW)
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors].data(),nStates);
else
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors].data(),nStates,CTRW);
}
if(x<Width-1) // add right to left
{
if(!IsTRW)
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+1].data(),nStates);
else
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+1].data(),nStates,CTRW);
}
if(y>0) // add top down
{
if(!IsTRW)
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+2].data(),nStates);
else
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+2].data(),nStates,CTRW);
}
if(y<Height-1) // add bottom up
{
if(!IsTRW)
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+3].data(),nStates);
else
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+2].data(),nStates,CTRW);
}
if(IsTRW)
Add2Message(message_org,pDualMessage[plane][offset1].data(),nStates,CTRW-1);
T_message*& message=pDualMessage[1-plane][offset1].data();
T_message Min;
// use the data term
if(plane==0) // from vx plane to vy plane
for(size_t l=0;l<nStates1;l++)
message[l]=CStochastic::Min(nStates,pDataTerm[offset].data()+l*nStates,message_org);
else // from vy plane to vx plane
for(size_t l=0;l<nStates1;l++)
{
Min=message_org[0]+pDataTerm[offset].data()[l];
for(size_t h=0;h<nStates;h++)
Min=__min(Min,message_org[h]+pDataTerm[offset].data()[h*nStates1+l]);
message[l]=Min;
}
// normalize the message
Min=CStochastic::Min(nStates1,message);
for(size_t l=0;l<nStates1;l++)
message[l]-=Min;
delete message_org;
}
//------------------------------------------------------------------------------------------------
// update the message from (x0,y0,plane) to the neighbors on the same plane
// the encoding of the direction
// 2 |
// v
// 0 ------> <------- 1
// ^
// 3 |
//------------------------------------------------------------------------------------------------
void BPFlow::UpdateSpatialMessage(int x, int y, int plane, 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 offset=y*Width+x;
int nStates=pWinSize[plane][offset]*2+1;
T_message* message_org;
message_org=new T_message[nStates];
int x1=x,y1=y; // get the destination
switch(direction){
case 0:
x1++;
s=Im_s.data()[offset*2+plane];
d=Im_d.data()[offset*2+plane];
break;
case 1:
x1--;
s=Im_s.data()[(offset-1)*2+plane];
d=Im_d.data()[(offset-1)*2+plane];
break;
case 2:
y1++;
s=Im_s.data()[offset*2+plane];
d=Im_d.data()[offset*2+plane];
break;
case 3:
y1--;
s=Im_s.data()[(offset-Width)*2+plane];
d=Im_d.data()[(offset-Width)*2+plane];
break;
}
//s=m_s;
//d=m_d;
int offset1=y1*Width+x1;
int nStates1=pWinSize[plane][offset1]*2+1; // get the number of states for the destination node
int wsize=pWinSize[plane][offset];
int wsize1=pWinSize[plane][offset1];
T_message*& message=pSpatialMessage[plane][offset1*nNeighbors+direction].data();
// initialize the message from the dual plane
if(!IsTRW)
{
//memcpy(message_org,pDualMessage[plane][offset].data(),sizeof(T_message)*nStates);
for(int i = 0;i<nStates;i++)
message_org[i] = pDualMessage[plane][offset].data()[i];
}
else
{
//memset(message_org,0,sizeof(T_message)*nStates);
for(int i = 0;i<nStates;i++)
message_org[i] = 0;
Add2Message(message_org,pDualMessage[plane][offset].data(),nStates,CTRW);
}
// add the range term
if(!IsTRW)
Add2Message(message_org,pRangeTerm[plane][offset].data(),nStates);
else
Add2Message(message_org,pRangeTerm[plane][offset].data(),nStates,CTRW);
// add spatial messages
if(!IsTRW)
{
if(x>0 && direction!=1) // add left to right
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors].data(),nStates);
if(x<Width-1 && direction!=0) // add right to left
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+1].data(),nStates);
if(y>0 && direction!=3) // add top down
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+2].data(),nStates);
if(y<Height-1 && direction!=2) // add bottom up
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+3].data(),nStates);
}
else
{
if(x>0) // add left to right
if(direction==1)
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors].data(),nStates,CTRW-1);
else
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors].data(),nStates,CTRW);
if(x<Width-1) // add right to left
if(direction==0)
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+1].data(),nStates,CTRW-1);
else
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+1].data(),nStates,CTRW);
if(y>0) // add top down
if(direction==3)
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+2].data(),nStates,CTRW-1);
else
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+2].data(),nStates,CTRW);
if(y<Height-1) // add bottom up
if(direction==2)
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+3].data(),nStates,CTRW-1);
else
Add2Message(message_org,pSpatialMessage[plane][offset*nNeighbors+3].data(),nStates,CTRW);
}
// use distance transform function to impose smoothness compatibility
T_message Min=CStochastic::Min(nStates,message_org)+d;
for(ptrdiff_t l=1;l<nStates;l++)
message_org[l]=__min(message_org[l],message_org[l-1]+s);
for(ptrdiff_t l=nStates-2;l>=0;l--)
message_org[l]=__min(message_org[l],message_org[l+1]+s);
// transform the compatibility
int shift=pOffset[plane][offset1]-pOffset[plane][offset];
if(abs(shift)>wsize+wsize1) // the shift is too big that there is no overlap
{
if(offset>0)
for(ptrdiff_t l=0;l<nStates1;l++)
message[l]=l*s;
else
for(ptrdiff_t l=0;l<nStates1;l++)
message[l]=-l*s;
}
else
{
int start=__max(-wsize,shift-wsize1);
int end=__min(wsize,shift+wsize1);
for(ptrdiff_t i=start;i<=end;i++)
message[i-shift+wsize1]=message_org[i+wsize];
if(start-shift+wsize1>0)
for(ptrdiff_t i=start-shift+wsize1-1;i>=0;i--)
message[i]=message[i+1]+s;
if(end-shift+wsize1<nStates1)
for(ptrdiff_t i=end-shift+wsize1+1;i<nStates1;i++)
message[i]=message[i-1]+s;
}
// put back the threshold
for(ptrdiff_t l=0;l<nStates1;l++)
message[l]=__min(message[l],Min);
// normalize the message by subtracting the minimum value
Min=CStochastic::Min(nStates1,message);
for(ptrdiff_t l=0;l<nStates1;l++)
message[l]-=Min;
delete message_org;
}
//------------------------------------------------------------------------------------------------
// 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;
}
