void SwapOperation(INT32T count){ INT32T newEnergy, oldEnergy; InitMap(); BuildMap(); InitLabel(); newEnergy = ComputeEnergy(); oldEnergy = newEnergy + 1; while ((oldEnergy != newEnergy) && (count-- > 0)){ oldEnergy = newEnergy; AlphaBetaSwap(); newEnergy = ComputeEnergy(); } }
void CStereoMatcher::OptBP() { CShape sh = m_cost.Shape(); int H = sh.height; int W = sh.width; const int numRows = H, numCols = W, numNodes = W *H; OneNodeCluster *nodeArray; const int numStates = m_disp_n; FLOATTYPE alpha = 0.95; unsigned int totalMem = 0; nodeArray = new OneNodeCluster[numNodes]; FLOATTYPE *oneNodeMsgArray = new FLOATTYPE[numNodes * 8 * numStates]; FLOATTYPE *oneNodeLocalEv = new FLOATTYPE[numStates * numNodes], *currLocalEvPtr = oneNodeLocalEv; totalMem += sizeof(OneNodeCluster) * numNodes; totalMem += sizeof(FLOATTYPE) * numNodes * 8 * numStates; totalMem += sizeof(FLOATTYPE) * numNodes * numStates; OneNodeCluster::numStates = numStates; TwoNodeCluster::numStates = numStates; printf("NumStates: %d\n",nodeArray[0].numStates); for (int i = 0; i < numNodes * 8 * numStates; i++) { oneNodeMsgArray[i] = 1.0f/numStates; } initOneNodeMsgMem(nodeArray,oneNodeMsgArray,numNodes, numStates); TwoNodeCluster dummyNode; FLOATTYPE dummyMessages[numStates * numStates*4]; for(int i = 0; i < numStates * numStates*4; i++) dummyMessages[i] = 1.0f; printf("Msg Space Allocated and Initialized\n"); CShape sh2 = m_cost.Shape(); printf("%d %d\n",m_disp_n, sh2.nBands); for(int m = 0; m < numRows; m++) { float* smoothcost_vert = &m_smooth.Pixel(0, m, 0); // band 1 is horizontal cost float* smoothcost_horz = &m_smooth.Pixel(0, m, 1); // band 1 is horizontal cost float* local_cost = &m_cost.Pixel(0, m, 0); for(int n = 0; n < numCols; n++) { const double div_factor = 50; const double vsmooth_cost = exp(-1*smoothcost_vert[0]/div_factor), hsmooth_cost = exp(-1*smoothcost_horz[0]/div_factor); const int cind = m * numCols + n; nodeArray[cind].localEv = currLocalEvPtr; currLocalEvPtr += numStates; for (int i = 0; i < m_disp_n; i++) { nodeArray[cind].localEv[i] = exp(-1 *local_cost[i]/div_factor); } nodeArray[cind].psiData_pottsSameProb[OneNodeCluster::UP]=1; nodeArray[cind].psiData_pottsSameProb[OneNodeCluster::DOWN]=1; nodeArray[cind].psiData_pottsSameProb[OneNodeCluster::LEFT]=1; nodeArray[cind].psiData_pottsSameProb[OneNodeCluster::RIGHT]=1; if (n == 0) nodeArray[cind].psiData_pottsDiffProb[OneNodeCluster::LEFT] = 1; if (n < numCols-1) { nodeArray[cind+1].psiData_pottsDiffProb[OneNodeCluster::LEFT] = hsmooth_cost; nodeArray[cind].psiData_pottsDiffProb[OneNodeCluster::RIGHT] = hsmooth_cost; } else { nodeArray[cind].psiData_pottsDiffProb[OneNodeCluster::RIGHT]=1; } if (m == 0) nodeArray[cind].psiData_pottsDiffProb[OneNodeCluster::UP] = 1; if(m != numRows -1) { nodeArray[cind+numCols].psiData_pottsDiffProb[OneNodeCluster::UP] = vsmooth_cost; nodeArray[cind].psiData_pottsDiffProb[OneNodeCluster::DOWN] = vsmooth_cost; } else { nodeArray[cind].psiData_pottsDiffProb[OneNodeCluster::DOWN]=1; } smoothcost_horz += 2; smoothcost_vert += 2; local_cost += m_disp_n; } } int numIter; //Initialization Completed Time for the good stuff numIter = 50; printf("Structures Initialized\n"); int iter = 0; int beliefIm[H * W]; for (int i = 0; i < H * W; i++) beliefIm[i] = 0; while (iter < numIter)//( !converged) { printf("Iter %03d\n",iter); struct timeb start, endtime; ftime(&start); for (int m = 0; m < numRows; m++) { passOneNodeMsgsLeft(&nodeArray[m *numCols], numCols, 0,0,dummyNode,alpha); passOneNodeMsgsRight(&nodeArray[m *numCols], numCols, 0,0,dummyNode,alpha); } for(int n = 0; n < numCols; n++) { passOneNodeMsgsDown(nodeArray,(TwoNodeCluster **)0,dummyNode,n,numRows,numCols,alpha); passOneNodeMsgsUp(nodeArray,(TwoNodeCluster **)0,dummyNode,n,numRows,numCols,alpha); } ftime(&endtime); float etime = (float)(endtime.time - start.time) + 0.001 * (float) (endtime.millitm - start.millitm); printf ("%f Seconds ",etime); float tmpBeliefVec[numStates]; float *beliefPtr; for(int m = 0; m < numRows; m++) { int *disp = &m_disparity.Pixel(0, m, 0); for(int n = 0; n < numCols; n++) { double best_prob = 0; int best_d = 0; nodeArray[m * numCols + n].getBelief(tmpBeliefVec); beliefPtr =tmpBeliefVec; for(int j =0; j <numStates;j++) { if (*beliefPtr > best_prob) { best_prob = *beliefPtr; best_d = j; } beliefPtr++; } disp[n] = best_d; } } float locEn, smoothEn; ComputeEnergy(m_cost, m_smooth, m_disparity, locEn, smoothEn); printf("Energy:%.2f Local %.2f Smooth: %.2f\n",locEn+smoothEn, locEn,smoothEn); iter++; } float tmpBeliefVec[numStates]; float *beliefPtr; for(int m = 0; m < numRows; m++) { int *disp = &m_disparity.Pixel(0, m, 0); for(int n = 0; n < numCols; n++) { double best_prob = 0; int best_d = 0; nodeArray[m * numCols + n].getBelief(tmpBeliefVec); beliefPtr =tmpBeliefVec; for(int j =0; j <numStates;j++) { if (*beliefPtr > best_prob) { best_prob = *beliefPtr; best_d = j; } beliefPtr++; } disp[n] = best_d; } } printf("\nTotalMem:%ud\n",totalMem/1024/1024); delete nodeArray; delete oneNodeMsgArray; delete oneNodeLocalEv; }