struct path {
short rowDiff;
short colDiff;
short index;
};
for (
int row =
0; row < rows; ++row)
{
for (
int col =
0; col < cols; ++col)
{
for (
unsigned
int path =
0; path < firstScanPaths.size(); ++path)
{
for (
int d =
0; d < disparityRange; ++d)
{
S[row][col][d] += aggregateCost(row, col, d, firstScanPaths[path], rows, cols, disparityRange, C, A[path]);
}
}
}
lastProgressPrinted = printProgress(row, rows -
1, lastProgressPrinted);
}
aggregateCost(row, col, d, firstScanPaths[path], rows, cols, disparityRange, C, A[path])
。输入的参数中
(row,col,d)
相当于确定了视差空间中三维点的位置,
firstScanPaths[path]
则确定了是哪条路径,或者可以更直观的理解为是像素领域内哪个相邻像素,
(rows,cols,disparityRange)
则确定了视差空间的范围,
C
为上一步代价计算得到的三维代价矩阵,最后的
A[path]
也是一个三维矩阵,用来存储对应方向的聚集代价,返回值则是
A[row][col][d]
。以下为该函数的实现:
unsigned short aggregateCost(int row, int col, int d, path &p, int rows, int cols, int disparityRange, unsigned short ***C, unsigned short ***A)
{
unsigned
short aggregatedCost =
0;
aggregatedCost += C[row][col][d];
//像素匹配的 cost 值
// 1. 边界条件,直接为C
if (row + p.rowDiff <
0 || row + p.rowDiff >= rows || col + p.colDiff <
0 || col + p.colDiff >= cols)
{
A[row][col][d] += aggregatedCost;
return A[row][col][d];
}
// 2. 若未超出边界 ,则进行相应方向的代价聚合
unsigned
short minPrev, minPrevOther, prev, prevPlus, prevMinus;
prev = minPrev = minPrevOther = prevPlus = prevMinus = MAX_SHORT;
//设置初始代价为最大值
//minPrev: 对应路径的视差代价最小值
// 对于该路径方向上,上一个像素,在其视差范围内进行循环
for (
int disp =
0; disp < disparityRange; ++disp)
{
unsigned
short tmp = A[row + p.rowDiff][col + p.colDiff][disp];
//找到这个路径下,前一个像素取不同视差值时最小的A,即为最后减去的那一项,minPrev
if(minPrev > tmp){minPrev = tmp;}
//前一个像素视差取值为d时,即和当前像素的视差相等时,最小的A.
if(disp == d)
{ prev = tmp;}
//前一个像素视差取值为d+1时,即和当前像素的视差相差1时,最小的A,最后将加惩罚系数P1.
else
if(disp == d +
1)
{ prevPlus = tmp;}
//前一个像素视差取值为d-1时,即和当前像素的视差相差1时,最小的A,最后将加惩罚系数P1.
else
if (disp == d -
1)
{ prevMinus = tmp;}
//前一个像素视差与当前像素的视差相差大于等于2时,最小的A,最后将加惩罚系数P2.
else
{ minPrevOther = tmp;}
}
/* 计算四种情况下的代价最小值 */
aggregatedCost +=
std::min(
std::min((
int)prevPlus + SMALL_PENALTY, (
int)prevMinus + SMALL_PENALTY),
std::min((
int)prev, (
int)minPrevOther + LARGE_PENALTY));
aggregatedCost -= minPrev;
//避免值过大,减小内存
A[row][col][d] += aggregatedCost;
return A[row][col][d];
}
欢迎加入公众号读者群一起和同行交流,目前有SLAM、算法竞赛、图像检测分割、自动驾驶、计算射影、医学影像、三维视觉等微信群(以后会逐渐细分),请扫描下面微信号加群,备注:”昵称+学校/公司+研究方向“,例如:”张三 + 上海交大 + 视觉SLAM“。请按照格式备注,否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告,否则会请出群,谢谢理解~
最新AI干货,我在看