void Lattice<T>::Set(int x, int y, int z, T t)
{
x = InBound(x);
y = InBound(y);
z = InBound(z);
field.Set(length2 * x + length * y + z, t);
}
T Lattice<T>::Get(int x, int y, int z)
{
x = InBound(x);
y = InBound(y);
z = InBound(z);
return field.Get(length2 * x + length * y + z);
}
utils::InetAddress Peer::GetRemoteAddress() const {
utils::InetAddress address = GetPeerAddress();
if (InBound()) {
address.SetPort((uint16_t)peer_listen_port_);
}
return address;
}
static float UpdateBelief(cv::Point2i &t, MCImg<float> &allBeliefs, MCImg<float> &allMessages, MCImg<float> &unaryCosts)
{
int numRows = unaryCosts.h, numCols = unaryCosts.w, numDisps = unaryCosts.n;
float maxDiff = -1;
float *s2tMsgs[4] = { 0 };
float newBelief[1024]; // Use a large array in stack, to prevent dynamic allocation
for (int k = 0; k < 4; k++) {
cv::Point2i s = t + dirDelta[k];
if (InBound(s, numRows, numCols)) {
s2tMsgs[k] = GetMessage(s, t, allMessages, numDisps);
}
}
float *belief = allBeliefs.get(t.y, t.x);
float accSum = 0.f;
for (int xt = 0; xt < numDisps; xt++) {
newBelief[xt] = unaryCosts.get(t.y, t.x)[xt];
for (int k = 0; k < 4; k++) {
if (s2tMsgs[k]) {
newBelief[xt] += s2tMsgs[k][xt];
}
}
accSum += newBelief[xt];
}
for (int xt = 0; xt < numDisps; xt++) {
newBelief[xt] /= accSum;
maxDiff = std::max(maxDiff, std::abs(newBelief[xt] - belief[xt]));
belief[xt] = newBelief[xt];
}
return maxDiff;
}
void CClassifyGrid::GetNeighbor( PointData & point, int dis, CClassifyChunk * chunk, int x, int y )
{
m_vecPointData.clear();
double distance = m_dbGridLength * dis;
double distance2 = distance * distance;
for ( int xx = x - dis; xx <= x + dis; xx++ )
for ( int yy = y - dis; yy <= y + dis; yy++ ) {
int i = xx;
int j = yy;
CClassifyChunk * data_chunk = chunk;
while ( i < 0 && data_chunk != NULL ) {
i += m_nUnitNumber[ 0 ];
data_chunk = InBound( data_chunk->m_iX - 1, data_chunk->m_iY ) ? m_vecPointer[ Index( data_chunk->m_iX - 1, data_chunk->m_iY ) ] : NULL;
}
while ( i > m_nUnitNumber[ 0 ] - 1 && data_chunk != NULL ) {
i -= m_nUnitNumber[ 0 ];
data_chunk = InBound( data_chunk->m_iX + 1, data_chunk->m_iY ) ? m_vecPointer[ Index( data_chunk->m_iX + 1, data_chunk->m_iY ) ] : NULL;
}
while ( j < 0 && data_chunk != NULL ) {
j += m_nUnitNumber[ 1 ];
data_chunk = InBound( data_chunk->m_iX, data_chunk->m_iY - 1 ) ? m_vecPointer[ Index( data_chunk->m_iX, data_chunk->m_iY - 1 ) ] : NULL;
}
while ( j > m_nUnitNumber[ 1 ] - 1 && data_chunk != NULL ) {
j -= m_nUnitNumber[ 1 ];
data_chunk = InBound( data_chunk->m_iX, data_chunk->m_iY + 1 ) ? m_vecPointer[ Index( data_chunk->m_iX, data_chunk->m_iY + 1 ) ] : NULL;
}
if ( data_chunk != NULL ) {
PointDataVector & cell_vector = data_chunk->m_vecGridIndex[ data_chunk->Index( i, j ) ];
for ( int k = 0; k < ( int )cell_vector.size(); k++ ) {
CVector3D diff = point.v - ( cell_vector[ k ] )->v;
if ( diff.length2() < distance2 ) {
m_vecPointData.push_back( cell_vector[ k ] );
}
}
}
}
}
static unsigned char MedianFilterInWindow(cv::Mat &img, int yc, int xc, int radius)
{
int numRows = img.rows, numCols = img.cols;
std::vector<unsigned char> pixels;
for (int y = yc - radius; y <= yc + radius; y++) {
for (int x = xc - radius; x <= xc + radius; x++) {
if (InBound(y, x, numRows, numCols)) {
pixels.push_back(img.at<unsigned char>(y, x));
}
}
}
std::sort(pixels.begin(), pixels.end());
ASSERT(pixels.size() > 0);
return pixels[pixels.size() / 2];
}
void CClassifyGrid::NotifyCCSClassified( int ix, int iy )
{
for ( int x = MinBound( ix ); x <= MaxBound( ix ); x++ )
for ( int y = MinBound( iy ); y <= MaxBound( iy ); y++ ) {
bool bAllClassified = true;
for ( int xx = MinBound( x ); xx <= MaxBound( x ) && bAllClassified; xx++ )
for ( int yy = MinBound( y ); yy <= MaxBound( y ) && bAllClassified; yy++ ) {
if ( InBound( xx, yy ) && CheckCCSClassified( Index( xx, yy ) ) == false ) {
bAllClassified = false;
}
}
if ( bAllClassified ) {
RefineChunk( m_vecPointer[ Index( x, y ) ] );
}
}
}
int main()
{
inbound_queue = QueueCreate(sizeof(char), 20) ;
ClearScreen(0x07) ;
SetCursorVisible(TRUE) ;
InitSerial() ;
InitOutBound() ;
InitInBound() ;
for (;;)
{
OutBound() ;
InBound() ;
}
return 0 ;
}
static void UpdateMessage(cv::Point2i &s, cv::Point2i &t, MCImg<float> &oldMessages,
MCImg<float> &newMessages, MCImg<float> &unaryCosts, cv::Mat &img)
{
extern bool USE_CONVEX_BP;
if (USE_CONVEX_BP) {
extern float ISING_GAMMA;
int numRows = unaryCosts.h, numCols = unaryCosts.w, numDisps = unaryCosts.n;
if (!InBound(s, numRows, numCols) || !InBound(t, numRows, numCols)) {
return;
}
float simWeight = exp(-L1Dist(img.at<cv::Vec3b>(s.y, s.x), img.at<cv::Vec3b>(t.y, t.x)) / ISING_GAMMA);
float *s2tMsgNew = GetMessage(s, t, newMessages, numDisps);
float *t2sMsgOld = GetMessage(t, s, oldMessages, numDisps);
float minMsgVal = FLT_MAX;
for (int xt = 0; xt < numDisps; xt++) {
float bestCost = FLT_MAX;
for (int xs = 0; xs < numDisps; xs++) {
//float tmpCost = unaryCosts.get(s.y, s.x)[xs] + simWeight * PairwiseCost(xs, xt);
float tmpCost = unaryCosts.get(s.y, s.x)[xs];
float hat_c_s = 1.f;
for (int k = 0; k < 4; k++) {
cv::Point2i q = s + dirDelta[k];
if (InBound(q, numRows, numCols)) {
hat_c_s += 1.f;
tmpCost += GetMessage(q, s, oldMessages, numDisps)[xs];
}
}
tmpCost *= (1.f / hat_c_s);
tmpCost += simWeight * PairwiseCost(xs, xt);
tmpCost -= t2sMsgOld[xs];
bestCost = std::min(bestCost, tmpCost);
}
s2tMsgNew[xt] = bestCost;
minMsgVal = std::min(minMsgVal, bestCost);
}
// Normalize messages
for (int xt = 0; xt < numDisps; xt++) {
s2tMsgNew[xt] -= minMsgVal;
}
}
else {
extern float ISING_GAMMA;
int numRows = unaryCosts.h, numCols = unaryCosts.w, numDisps = unaryCosts.n;
if (!InBound(s, numRows, numCols) || !InBound(t, numRows, numCols)) {
return;
}
float simWeight = exp(-L1Dist(img.at<cv::Vec3b>(s.y, s.x), img.at<cv::Vec3b>(t.y, t.x)) / ISING_GAMMA);
float *s2tMsgNew = GetMessage(s, t, newMessages, numDisps);
float minMsgVal = FLT_MAX;
for (int xt = 0; xt < numDisps; xt++) {
float bestCost = FLT_MAX;
for (int xs = 0; xs < numDisps; xs++) {
float tmpCost = unaryCosts.get(s.y, s.x)[xs] + simWeight * PairwiseCost(xs, xt);
for (int k = 0; k < 4; k++) {
cv::Point2i q = s + dirDelta[k];
if (InBound(q, numRows, numCols) && q != t) {
tmpCost += GetMessage(q, s, oldMessages, numDisps)[xs];
}
}
bestCost = std::min(bestCost, tmpCost);
}
s2tMsgNew[xt] = bestCost;
//s2tMsg[xt] = 0.7 * s2tMsg[xt] + 0.3 * bestCost;
minMsgVal = std::min(minMsgVal, bestCost);
}
// Normalize messages
for (int xt = 0; xt < numDisps; xt++) {
s2tMsgNew[xt] -= minMsgVal;
}
}
}
