Cross-Scale Cost Aggregation for Stereo Matching立体匹配算法介绍

最近，研究了下CVPR2014上的一篇基于多尺度代价聚合的立体匹配算法，这个作者提供了原代码，运行了下，发现效果真心不错，不开后端处理的话，时间在0.4s左右。这个算法比较牛逼的有两点：

1：结合多尺度思想，对原始图像进行下采样，然后在每层图像上计算匹配代价，进行代价聚合，然后多尺度得到的视差进行结合，作为最终的代价聚合值。

2：提供了一个框架，里面包含立体匹配很多常用的经典的方法，可以在每一步使用不同的方式完成。

参考一篇博客：http://blog.csdn.net/wsj998689aa/article/details/44411215

本文结合源码，对这个算法的流程进行详细分析。

首先进行匹配代价计算：

源码里有三种方法，CEN（就是census），CG（Census + TAD + 梯度）,GRD（梯度+TAD） ，这三种方式几乎是当下进行第一步匹配代价计算的常用方式。

1：CEN：选定一个窗口，对窗口中每一个像素与中心像素进行比较，大于中心像素即为0，否则为1。从而得到一个二进制系列，分别对左图和右图进行计算每个像素的匹配代价。并得到初步的代价聚合，计算每个像素在视差范围内每个可能视差的代价聚合值。左右匹配，即两个二进制系列相似值，每个对应位是否相等。

void CenCC::buildCV( const Mat& lImg, const Mat& rImg, const int maxDis, Mat* costVol )
{
// for TAD + Grd input image must be CV_64FC3//输入的图像必须是64FC3
CV_Assert( lImg.type() == CV_64FC3 && rImg.type() == CV_64FC3 );

int hei = lImg.rows;//图像宽高
int wid = lImg.cols;
Mat lGray, rGray;
Mat tmp;
lImg.convertTo( tmp, CV_32F );//64FC3图像数据转化成32F
cvtColor( tmp, lGray, CV_RGB2GRAY );//灰度化
lGray.convertTo( lGray, CV_8U, 255 );//灰度图像转换成8U数据格式，并把开始除的255乘回来
rImg.convertTo( tmp, CV_32F );
cvtColor( tmp, rGray, CV_RGB2GRAY );
rGray.convertTo( rGray, CV_8U, 255 );
// prepare binary code
int H_WD = CENCUS_WND / 2;
bitset<CENCUS_BIT>* lCode = new bitset<CENCUS_BIT>[ wid * hei ];
bitset<CENCUS_BIT>* rCode = new bitset<CENCUS_BIT>[ wid * hei ];
bitset<CENCUS_BIT>* pLCode = lCode;
bitset<CENCUS_BIT>* pRCode = rCode;
for( int y = 0; y < hei; y ++ ) { //求得中心像素的CENSUS码
uchar* pLData = ( uchar* ) ( lGray.ptr<uchar>( y ) );
uchar* pRData = ( uchar* ) ( rGray.ptr<uchar>( y ) );
for( int x = 0; x < wid; x ++ ) {
int bitCnt = 0;
for( int wy = - H_WD; wy <= H_WD; wy ++ ) {
int qy = ( y + wy + hei ) % hei;
uchar* qLData = ( uchar* ) ( lGray.ptr<uchar>( qy ) );
uchar* qRData = ( uchar* ) ( rGray.ptr<uchar>( qy ) );
for( int wx = - H_WD; wx <= H_WD; wx ++ ) {
if( wy != 0 || wx != 0 ) {
int qx = ( x + wx + wid ) % wid;
( *pLCode )[ bitCnt ] = ( pLData[ x ] > qLData[ qx ] );//这里是对窗口内的每个像素与中心像素比较，小于则为1,大于则为0
( *pRCode )[ bitCnt ] = ( pRData[ x ] > qRData[ qx ] );
bitCnt ++;
}

}
}
pLCode ++;
pRCode ++;
}
}
// build cost volume 初始代价聚合
bitset<CENCUS_BIT> lB;
bitset<CENCUS_BIT> rB;
pLCode = lCode;
for( int y = 0; y < hei; y ++ ) {
int index = y * wid;
for( int x = 0; x < wid; x ++ ) {
lB = *pLCode;
for( int d = 0; d < maxDis; d ++ ) {
double* cost   = ( double* ) costVol[ d ].ptr<double>( y );//costVol是视差为d时的匹配代价Mat集合
cost[ x ] = CENCUS_BIT;
if( x - d >= 0 ) {
rB = rCode[ index + x - d ];
cost[ x ] = ( lB ^ rB ).count();//这里是求左右图的census值的相似性，作为视差为d时，当前像素的匹配代价。
}

}
pLCode ++;
}
}
delete [] lCode;
delete [] rCode;
}

下面是从右向左进行匹配的，就是在寻找视差方向上时不同而已

// build cost volume
bitset<CENCUS_BIT> lB;
bitset<CENCUS_BIT> rB;
pRCode = rCode;
for( int y = 0; y < hei; y ++ ) {
int index = y * wid;
for( int x = 0; x < wid; x ++ ) {
rB = *pRCode;
for( int d = 0; d < maxDis; d ++ ) {
double* cost   = ( double* ) rCostVol[ d ].ptr<double>( y );
cost[ x ] = CENCUS_BIT;
if( x + d < wid ) {
lB = lCode[ index + x + d ];
cost[ x ] = ( rB ^ lB ).count();
}

}
pRCode ++;
}
}

这里就完成了利用census原理计算匹配代价，然后进行初始代价聚合。

2：TAD + 梯度

这个算法运用图像三通道颜色值的差的绝对值AD，结合一个阈值，做成TAD样式，求三通道均值; 然后利用参数为1的sobel算子求解x方向的图像梯度，同样加一个阈值，做成TGRD的样式。这两个特征进行加权相加。这里还对图像边缘进行判断，是边缘的话，就把左右TAD和TGRD的右换成边缘阈值。

// X Gradient
// sobel size must be 1
Sobel( lGray, lGrdX, CV_64F, 1, 0, 1 );
Sobel( rGray, rGrdX, CV_64F, 1, 0, 1 );
lGrdX += 0.5;
rGrdX += 0.5;

inline double myCostGrd( double* lC, double* rC,
double* lG, double* rG )
{
double clrDiff = 0;
// three color
for( int c = 0; c < 3; c ++ ) {
double temp = fabs( lC[ c ] - rC[ c ] );
clrDiff += temp;
}
clrDiff *= 0.3333333333;
// gradient diff
double grdDiff = fabs( lG[ 0 ] - rG[ 0 ] );
clrDiff = clrDiff > TAU_1 ? TAU_1 : clrDiff;
grdDiff = grdDiff > TAU_2 ? TAU_2 : grdDiff;
return ALPHA * clrDiff + ( 1 - ALPHA ) * grdDiff;
}

3:census + TAD + 梯度

就是把上面两个加权叠加，下式的前面是TAD+TGRD的匹配代价，后面是census的匹配代价，得到最终的匹配代价。

cost[ x ] = 2 - exp( - tmpCost / 35 ) - exp( - 255 * cost[ x ] / 15 );//这就是TAD+TGD+Census的匹配代价计算

接下来就是进行代价聚合了。

代价聚合：源码提供了多种方式，双边滤波器bilateral filter，引导滤波器guided image filter，box filter，还有非局部的代价聚合方法（《A Non-Local Cost Aggregation Method for Stereo Matching》），基于分割树的代价聚合方法《Segment-Tree based Cost Aggregation for Stereo Matching》。基于滤波器的方法是设定一个窗口，在窗口内进行代价聚合。双边滤波器就是对窗口内像素进行距离加权和亮度加权。

后两种都抛弃了固定窗口模式,基于NL的代价聚合是使用最小生成树代替窗口。基于ST的代价聚合是使用分割块代替窗口。

这里主要分析下基于分割树的代价聚合方法。

这个方式是CVPR2013的文章，是NL方法的升级版。原理类似。

这个方式中，可以不采用第一步计算的匹配代价，不过它自身计算匹配代价和TAD+TGRD类似。这里就使用第一步的匹配代价。

1：初始化一个图G（V，E），每个像素就是顶点V，边是每两个像素之间进行三通道分别求差的绝对值，取最大的那个差值作为边的权值。

float CColorWeight::GetWeight(int x0, int y0, int x1, int y1) const {//得到三通道两个像素值差值最大的那个通道的差值的绝对值
return (float)std::max(
std::max( abs(imgPtr(y0,x0)[0] - imgPtr(y1,x1)[0]), abs(imgPtr(y0,x0)[1] - imgPtr(y1,x1)[1]) ),
abs(imgPtr(y0,x0)[2] - imgPtr(y1,x1)[2])
);
}

2：利用边的权值对原图像进行分割，构建分割树。

依次对每个边连接的像素，判断边权值是否大于阈值，大于则不在同一个分割块，否则就在同一个分割块。这里先对边按权值大小进行升序排列，因为权值小的更容易是一个分割块。这样就构成了一个个分割块，每个分割块就是一个小的树。

然后再次判断每个边连接的两个像素，意在连接每个分割块，使每个小树作为图像整体树的子树连接在一起。

这样就构成了整幅图像的树。

int cnt = 0;
universe *segment_graph(int num_vertices, int num_edges, edge *edges,
float c, unsigned char *edges_mask) {
// sort edges by weight  按边的权重大小升序排列边
std::sort(edges, edges + num_edges);

// make a disjoint-set forest   产生一个并查集森林，每棵树中的成员都可由根结点所代表，意思是将图中的点分割成子树，每个子树的根节点代表子树。然后再把各子树连接起来
universe *u = new universe(num_vertices);

// init thresholds
float *threshold = new float[num_vertices];
for (int i = 0; i < num_vertices; i++) //对每个像素点，初始化阈值为c/1
threshold[i] = THRESHOLD(1,c);

// for each edge, in non-decreasing weight order...对每个边
//这里就是从第0个像素开始，若边权重小于阈值，就划为同一个分割块，若不小于，则不是同一个分割块，这里就得到整幅图像的若干个分割块。
//每个分割块的任意像素的elts[].p都是最开始不是上一个分割块的那个像素的坐标一维值。size记录当前分割块所包含的像素的数量。
//当一个边的一个端点首次加入比较时，此时的边有个mask值为255,分割块之间的边的mask都为0，重复比较的两点之间的边的mask也为0。这里是避免树上节点的重复连接。
for (int i = 0; i < num_edges; i++) {
edge *pedge = &edges[i];

// components connected by this edge找到这个边连接的两个像素
int a = u->find(pedge->a);
int b = u->find(pedge->b);
if (a != b) //如果这两个像素不在同一个分割块内
{
if (pedge->w <= threshold[a] && pedge->w <= threshold[b])//如果这两个像素连的边的权值小于等于这两个像素的阈值
{//就把这两个像素合并，a作为b的根节点，b作为子节点，代表a，以后u->find(a)得到的就是b的属性p这样就可以每个节点只有两个子节点
edges_mask[i]=255;
u->join(a, b);
a = u->find(a);

threshold[a]  = pedge->w + THRESHOLD(u->size(a), c);//THRESHOLD(u->size(a)得到此时这个分块的大小，其实是b的大小。。。
}
}
}

//cv::Mat sg_img = cv::Mat(256,320,CV_8UC3);
char sg_img_1[256*320*3];
cv::Mat sg_img = cv::Mat(256,320,CV_8UC3,sg_img_1);

if((cnt++)%3 == 0)
{
for(int i=0;i<num_edges;i++)
{
int a = u->find(edges[i].a);
int b = u->find(edges[i].b);

sg_img_1[edges[i].a * 3] = a%(256*256);
sg_img_1[edges[i].a * 3+1 ] =256 -  a%256;
sg_img_1[edges[i].a * 3+2 ] = a;

sg_img_1[edges[i].b * 3] = b%(256*256);
sg_img_1[edges[i].b * 3+1 ] =256 - b%256;
sg_img_1[edges[i].b * 3+2 ] = b;

}

cv::imshow("sg_img",sg_img);
cv::waitKey(1);
}

//added by X. Sun: re-organizing the structures to be a single tree
for (int i = 0; i < num_edges; i++)
{
int a = u->find(edges[i].a);
int b = u->find(edges[i].b);
if (a != b)
{
int size_min = MIN(u->size(a), u->size(b));//求分块a和b的最少大小
u->join(a, b);

//record
edges_mask[i]=255;
if(size_min > MIN_SIZE_SEG)
edges[i].w += PENALTY_CROSS_SEG;
}
}
// free up
delete []threshold;
return u;
}

3：对整颗树的所有节点计算其父节点和子节点，并进行排序。组成一个整体树，从根节点到叶节点进行存储，便于下面基于树结构进行循环代价聚合。

//step 2: build node based graph   这里求出每个点与周围四邻域的点的父子关系，并给出这两个点之间的距离。
//因为mask并不全为255,所以只有当这个点的邻域点是根据当前点判断进入分割块，或者分割块之间，才算作子节点。
TreeNode *AdjTable = new TreeNode[pixelsNum];
for(int i = 0;i < pixelsNum;i++) AdjTable[i].id = i;//初始化每个像素的id为像素的位置

for(int i = 0;i < edgeNum;i++) {
if(!edges_mask[i]) continue;//如果该边没有被计算是否分割，则继续

int pa = edges[i].a;
int pb = edges[i].b;
int dis = std::min(int(edges[i].w * weightProvider.GetScale() + 0.5f), 255);

int x0, y0, x1, y1;
x0 = pa % m_imgSize.width; y0 = pa / m_imgSize.width;//求出x0在图像中的确切位置
x1 = pb % m_imgSize.width; y1 = pb / m_imgSize.width;

TreeNode &nodeA = AdjTable[pa];
TreeNode &nodeB = AdjTable[pb];

nodeA.children[nodeA.childrenNum].id = pb;
nodeA.children[nodeA.childrenNum].dist = (uchar)dis;
nodeA.childrenNum++;

nodeB.children[nodeB.childrenNum].id = pa;
nodeB.children[nodeB.childrenNum].dist = (uchar)dis;
nodeB.childrenNum++;
}

//step 3: build ordered tree
if(!m_tree.empty()) m_tree.clear();
m_tree.resize(pixelsNum);

bool *isVisited = new bool[pixelsNum];
memset(isVisited, 0, sizeof(bool) * pixelsNum);

m_tree[0] = AdjTable[0];
isVisited[0] = true;
int start = 0, end = 1;

while(start < end) {
TreeNode &p = m_tree[start++];

for(int i = 0;i < p.childrenNum;i++) {
if(isVisited[p.children[i].id]) continue;

isVisited[p.children[i].id] = true;

TreeNode c;
c.id = p.children[i].id;
c.father.id = p.id;
c.father.dist = p.children[i].dist;

TreeNode &t = AdjTable[c.id];
for(int j = 0;j < t.childrenNum;j++) {
if(t.children[j].id != p.id) {
c.children[c.childrenNum++] = t.children[j];
}
}

m_tree[end++] = c;
}
}

4：从叶节点到根节点，然后再从根节点到叶节点进行代价聚合。论文中的聚合图很好的说明了这个问题。

从叶节点到根节点就是每个像素点的代价聚合值为当前像素的匹配代价 + 该像素所有叶节点的匹配代价聚合值的加权和，权值就是边权值。从下到上，逐层计算。

从根节点到叶节点就是对每个像素点在上一步计算的代价聚合值 + 权值 ×（其父节点的代价聚合值 - 其本身像素对父节点代价聚合值的贡献）。

void CSegmentTree::Filter(cv::Mat costVol, int maxLevel) {
cv::Mat costBuffer = costVol.clone();

KIdx_<float, 3>  costPtr((float *)costVol.data, m_imgSize.height, m_imgSize.width, maxLevel);
KIdx_<float, 3>  bufferPtr((float *)costBuffer.data, m_imgSize.height, m_imgSize.width, maxLevel);

int pixelsNum = m_imgSize.area();
//first pass: from leaf to root
for(int i = pixelsNum - 1;i >= 0;i--) {
TreeNode &node = m_tree[i];
float *cost = &bufferPtr(node.id * maxLevel);
for(int z = 0;z < node.childrenNum;z++) {
float *child_cost = &bufferPtr(node.children[z].id * maxLevel);
float weight = m_table[node.children[z].dist];
for(int k = 0;k < maxLevel;k++) {
cost[k] += child_cost[k] * weight;
}
}
}

//second pass: from root to leaf
memcpy(&costPtr(0), &bufferPtr(0), sizeof(float) * maxLevel);//这个就是最开始的父节点。
for(int i = 1;i < pixelsNum;i++) {//注意上面的循环顺序和此处的不同
TreeNode &node = m_tree[i];
float *final_cost = &costPtr(node.id * maxLevel);
float *cur_cost = &bufferPtr(node.id * maxLevel);
float *father_cost = &costPtr(node.father.id * maxLevel);
float weight = m_table[node.father.dist];
for(int k = 0;k < maxLevel;k++) {
final_cost[k] = weight * (father_cost[k] - weight * cur_cost[k]) + cur_cost[k];
}
}
}

这样就完成了每个像素的代价聚合。同理对右图匹配左图也一样。

由于这篇文章是基于多尺度的代价聚合，那么就对原始图像进行下采样，实验设置为5层金字塔。在每一层上进行匹配代价计算，代价聚合。这里要注意的是每次下采样，图像缩小一倍，那么视差范围变为maxDis_c = maxDis_c / 2 + 1; 求出的视差值也要缩小1倍，disSc_c *= 2;//视差值乘以2.因为图像变小了一倍，然后视差d肯定比原视差差一倍。

求出每层图像的代价聚合值之后，合并到原始图像层。

这里先计算每层图像求得的代价聚合值相对于原始层图像代价聚合值的加权因子。

Mat regMat = Mat::zeros( PY_LVL, PY_LVL, CV_64FC1 );
for( int s = 0; s < PY_LVL; s ++ ) { //对金字塔的每层之间，及每层与上下层之间的相关因子进行计算
if( s == 0 ) {
regMat.at<double>( s, s ) = 1 + REG_LAMBDA;
regMat.at<double>( s, s + 1 ) = - REG_LAMBDA;
} else if( s == PY_LVL - 1 ) {
regMat.at<double>( s, s ) = 1 + REG_LAMBDA;
regMat.at<double>( s, s - 1 ) = - REG_LAMBDA;
} else {
regMat.at<double>( s, s ) = 1 + 2 * REG_LAMBDA;
regMat.at<double>( s, s - 1 ) = - REG_LAMBDA;
regMat.at<double>( s, s + 1 ) = - REG_LAMBDA;
}
}
Mat regInv = regMat.inv( );//对因子矩阵求反
double* invWgt  = new double[ PY_LVL ];
for( int s = 0; s < PY_LVL; s ++ ) {
invWgt[ s ] = regInv.at<double>( 0, s );
}

然后从原始图像的每个像素开始，依次对每个像素在每一层的代价聚合进行加权和。最后得到的和就是原始层图像的代价聚合值。

//
// Left Cost Volume
//
for( int d = 1; d < smPyr[ 0 ]->maxDis; d ++ ) {
// printf( ".s.v." );
for( int y = 0; y < hei; y ++ ) {
for( int x = 0; x < wid; x ++ ) {
int curY = y;
int curX = x;
int curD = d;
double sum = 0;
for( int s = 0; s < PY_LVL; s ++ ) {
double curCost = smPyr[ s ]->costVol[ curD ].at<double>( curY, curX );
#ifdef _DEBUG
if( y == 160 && x == 160 ) {
printf( "\ns=%d(%d,%d)\td=%d\tcost=%.4lf", s, curY, curX, curD, curCost );
}
#endif
sum += invWgt[ s ] * curCost;//这里就体现了多尺度的特点，就是多尺度的像素代价聚合值加权求和
curY = curY / 2;//这是为了求下一层的代价聚合值
curX = curX / 2;
curD = ( curD + 1 ) / 2;
}
smPyr[ 0 ]->costVol[ d ].at<double>( y, x ) = sum;//再把所有层的代价聚合值加权和得到的总的代价聚合值返给原始图像层，作为最终的代价聚合值

}
}

求右图匹配左图也类似。

其实多尺度方法也算是比较简单了，运用多尺度进行代价聚合，作者说符合生物学习惯，先远后近，远处看个大概，近处就能看清细节了。思路很好，但含金量并不算太高，原来发CVPR包装也是很必要的。开玩笑了，实测这种方法效果还真心不错。

代价聚合完之后，那就WTA求最优视差值吧。WTA也能叫做算法……

void SSCA::Match( void )//WTA算法求左右视差
{
//printf( "\n\tMatch" );
for( int y = 0; y < hei; y ++ ) {
uchar* lDisData = ( uchar* ) lDis.ptr<uchar>( y );
for( int x = 0; x < wid; x ++ ) {
double minCost = DOUBLE_MAX;
int    minDis  = 0;
for( int d = 1; d < maxDis; d ++ ) {
double* costData = ( double* )costVol[ d ].ptr<double>( y );
if( costData[ x ] < minCost ) {
minCost = costData[ x ];
minDis  = d;
}
}
lDisData[ x ] = minDis * disSc;
}
}
#ifdef COMPUTE_RIGHT
for( int y = 0; y < hei; y ++ ) {
uchar* rDisData = ( uchar* ) rDis.ptr<uchar>( y );
for( int x = 0; x < wid; x ++ ) {
double minCost = DOUBLE_MAX;
int    minDis  = 0;
for( int d = 1; d < maxDis; d ++ ) {
double* costData = ( double* )rCostVol[ d ].ptr<double>( y );
if( costData[ x ] < minCost ) {
minCost = costData[ x ];
minDis  = d;
}
}
rDisData[ x ] = minDis * disSc;
}
}
#endif
}

求出初始视差后，接下来就是比较繁琐的视差求精了。

源码中给出了两种方式，

1：加权中值滤波，进行左右一致性检测; 进行不可靠点进行找左右最近的有效点选视差值较小的那个; 然后进行对不可靠点进行加权中值滤波;

2：分割后，进行左右一致性检测; 进行不可靠点进行找左右最近的有效点选视差值较小的那个; 然后进行对不可靠点进行加权中值滤波; 然后求分割块的平面参数，对分割块内的每个像素进行视差求精。

后处理一般都是这样，很多方法都是前面随便求个初始值，重要的就是后处理。这块代码比较多，有时间再说了。

本文只是简单的从代码上进行分析这篇论文的原理，没有放实验图片，我也是太懒了….。