置信度传播算法（BP）源码

#include "BP.h" #include<iostream> #include "opencv2/core/core.hpp" #include "opencv2/highgui/highgui.hpp" #include "opencv2/imgproc/imgproc.hpp" #include<vector> using namespace std; using namespace cv; BP::BP(Mat lImage, Mat rImage) { leftImage = lImage; rightImage = rImage; Mat lm = leftImage.clone(); Mat rm = rightImage.clone(); //Mat leftGray(lm.size(), CV_8U); //Mat rightGray(rm.size(), CV_8U); if (lm.channels() > 1) { cvtColor(lm, leftGray, CV_BGR2GRAY); cvtColor(rm, rightGray, CV_BGR2GRAY); } else { leftGray = lm; rightGray = rm; } } void BP::initNode(Node *n, int i, int j) { n->x = i; n->y = j; n->up = NULL; n->down = NULL; n->left = NULL; n->right = NULL; memset(n->messageUp, 0, sizeof(int) * L); memset(n->messageDown, 0, sizeof(int) * L); memset(n->messageLeft, 0, sizeof(int) * L); memset(n->messageRight, 0, sizeof(int) * L); } void BP::generateList() { head = (struct Node *)malloc(sizeof(Node)); Node *pre = head; int height = leftGray.rows; int width = leftGray.cols; initNode(head, 0,0); //建立第一列的链表 for (int i = 1; i < height; i++) { Node *node = (struct Node *)malloc(sizeof(Node)); initNode(node, i, 0 ); pre->down = node; node->up = pre; pre = pre->down; } //建立第一行的列表 Node * p = head; for (int j = 1; j < width; j++) { Node *node = (struct Node *)malloc(sizeof(Node)); initNode(node, 0, j); p->right = node; node->left = p; p = p->right; } Node *preR = head->right; Node *preC = head->down; Node *pr = preR; Node *pc = preC; for (int i = 1; i < height; i++) { pc = preC; pr = preR; for (int j = 1; j < width; j++) { Node *node = (struct Node *)malloc(sizeof(Node)); initNode(node, i, j); pc->right = node; node->left = pc; pr->down = node; node->up = pr; pr = pr->right; pc = pc->right; } preC = preC->down; preR = preR->down; } } void BP::printList() { Node *p = head; Node *r = p; Node *down = p; while (down) { while (r) { cout << "i:" << r->x << "j" << r->y <<" "; r = r->right; } cout << endl; down = down->down; r = down; } } int BP::D(int x, int li, int lj) { int f = abs(leftGray.at<uchar>(li, lj) - rightGray.at<uchar>(li, lj - x)); return f; } void BP::computeME(Node *r, Node *e, int t) { for (int j = 0; j < L; j++) { int minsum = INTMAX_MAX; for (int i = 0; i < L; i++) { if ((r->y - i) < 0) break; int Di = D(i, r->x, r->y); int Vij = C*abs(i-j); int m = 0; Node *li = r->left; if (e != li && li!=NULL) m += li->messageRight[t -1 ][i]; Node *ri = r->right; if (e != ri && ri != NULL) m += ri->messageLeft[t - 1][i]; Node *dow = r->down; if (e != dow && dow != NULL) m += dow->messageUp[t -1][i]; Node *u = r->up; if(e != u && u != NULL) m += u->messageDown[t - 1][i]; int sum = m + Di + Vij; if (minsum > sum) minsum = sum; } if (e == r->right) r->messageRight[t][j] = minsum; if (e == r->left) r->messageLeft[t][j] = minsum; if (e == r->up) r->messageUp[t][j] = minsum; if (e == r->down) r->messageDown[t][j] = minsum; } } void BP::computeM() { int height = leftGray.rows; int width = leftGray.cols; for (int t = 1; t < T; t++) { Node *p = head; Node *r = p; Node *d = p; while (d) { while (r) { /* if (!(r->up && r->down && r->left &&r->right)) { r = r->right; continue; }*/ //对于每一个节点分别使他对周围的邻居节点更新消息 if(r->up) computeME(r, r->up, t); if(r->down) computeME(r, r->down, t); if(r->left) computeME(r, r->left, t); if(r->right) computeME(r, r->right,t); r = r->right; } d = d->down; r = d; } } } int BP::computeDisparityByBelief(Node *n) { int minx = L; int minBelief = INTMAX_MAX; for (int i = 0;i < L;i++) { if ((n->y - i) < 0) break; int Di = D(i, n->x, n->y); int message=0; if(n->up) message += n->up->messageDown[T-1][i]; if (n->down) message += n->down->messageUp[T - 1][i]; if (n->left) message += n->left->messageRight[T - 1][i]; if(n->right) message += n->right->messageLeft[T - 1][i]; int sum = Di + message; if (minBelief > sum) minBelief = sum, minx = i; } return minx; } void BP::generateDisparity() { Node *p = head; Node *r = p; Node *d = p; disparity = Mat(leftGray.size(),CV_8U, Scalar(255)); while (d) { while (r) { /*if (!(r->up && r->down && r->left &&r->right)) { r= r->right; continue; }*/ //对于每一个节点根据周围的消息，找到使置信度最小的视差 int d = computeDisparityByBelief(r); disparity.at<uchar>(r->x, r->y) = d *16; r = r->right; } d = d->down; r = d; } } void BP::freeList() { Node *r = head; Node *c = r; while (r) { c = r; r = r->down; Node *cc = c; while (c) { cc = c->right; free(c); c = cc; } } } void BP::doBp() { generateList(); //生成新的数据链表 computeM(); //迭代计算消息向量 generateDisparity(); //生成视差图 freeList(); //释放数据链表的空间，防止造成内存泄露 } Mat BP::getDisparity() { return disparity; }