SGM重要代码摘抄-JZTXT

Census变换

 1     // 逐像素计算census值
 2     for (sint32 i = 2; i < height - 2; i++) {
 3         for (sint32 j = 2; j < width - 2; j++) {
 4             
 5             // 中心像素值
 6             const uint8 gray_center = source[i * width + j];
 7             
 8             // 遍历大小为5x5的窗口内邻域像素，逐一比较像素值与中心像素值的的大小，计算census值
 9             uint32 census_val = 0u;
10             for (sint32 r = -2; r <= 2; r++) {
11                 for (sint32 c = -2; c <= 2; c++) {
12                     census_val <<= 1;
13                     const uint8 gray = source[(i + r) * width + j + c];
14                     if (gray < gray_center) {
15                         census_val += 1;
16                     }
17                 }
18             }
19 
20             // 中心像素的census值
21             census[i * width + j] = census_val;        
22         }
23     }

汉明距离

 1 uint8 sgm_util::Hamming32(const uint32& x, const uint32& y)
 2 {
 3     uint32 dist = 0, val = x ^ y;
 4 
 5     // Count the number of set bits
 6     while (val) {
 7         ++dist;
 8         val &= val - 1;
 9     }
10 
11     return static_cast<uint8>(dist);
12 }

代价聚合

思想：总体思想利用动态规划：第一个像素代价为其初始视差，第二个像素代价为第二个像素初始代价和第一个像素的初始代价聚合得到聚合代价，第三个像聚合代价为第三个像素初始代价和第二个像素聚合代价的聚合...

技巧：定义一个临时数组用来存储本像素的聚合代价，给下一个像素用；然后每个像素的所有视差下的聚合代价求完放入临时数组中，再求下一个像素的

以横向聚合为例(方法就是一级循环从上往下遍历，二级循环每行从左向右或则从右向左遍历，三级循环从最小视差到最大视差遍历),下面阐述二级循环的步骤

判断从左向右聚合还是从右向左聚合，从左向右；从左向右就是从第一个像素开始循环，从右向左就是从最后一个像素循环。(这里以从左向右为例)
首先第一个像素的所有聚合代价就是它的初始代价，装入聚合数组；并将初始代价装入临时数组，给第二个像素用
进行第二个像素的聚合利用代价聚合公式进行代价聚合和上一步得到的临时数组，进行聚合后将第二个像素的所有聚合结果装入聚合数组；并将初始代价装入临时数组，给第三个像素用
进行第三个像素的聚合......

 1         for (sint32 j = 0; j < width - 1; j++) {
 2             gray = *img_row;
 3             uint8 min_cost = UINT8_MAX;
 4             for (sint32 d = 0; d < disp_range; d++){
 5                 // Lr(p,d) = C(p,d) + min( Lr(p-r,d), Lr(p-r,d-1) + P1, Lr(p-r,d+1) + P1, min(Lr(p-r))+P2 ) - min(Lr(p-r))
 6                 const uint8  cost = cost_init_row[d];
 7                 const uint16 l1 = cost_last_path[d + 1];
 8                 const uint16 l2 = cost_last_path[d] + P1;
 9                 const uint16 l3 = cost_last_path[d + 2] + P1;
10                 const uint16 l4 = mincost_last_path + std::max(P1, P2_Init / (abs(gray - gray_last) + 1));
11                 
12                 const uint8 cost_s = cost + static_cast<uint8>(std::min(std::min(l1, l2), std::min(l3, l4)) - mincost_last_path);
13                 
14                 cost_aggr_row[d] = cost_s;
15                 min_cost = std::min(min_cost, cost_s);
16             }